777爽死你无码免费看一二区,91精品国产在热久久无毒不卡

_POS_NOWstatus pos_vel_xy;
float z_est[3]={0,0,0},x_est[3]={0,0,0},y_est[3]={0,0,0};
typedef struct {
float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED)*/
}vehicle_attitude_s;
vehicle_attitude_s vehicle_att;
vehicle_attitude_s vehicle_optical;
realacc real_acceleration;
_vertical_information baro_acc_alt;
//#define ALT_VEL_HISTORY 10
//volatile float alt_vel_history[ALT_VEL_HISTORY] = {0};
//#define XY_VEL_HISTORY 3
//volatile float x_vel_history[XY_VEL_HISTORY] = {0}, y_vel_history[XY_VEL_HISTORY] = {0};
static float z_bias = 0,x_bias = 0,y_bias = 0;
float w_z_baro=0.5f;
static float corr_baro = 0;//corr_baro_filter = 0;
static float corr_xvel = 0,corr_yvel = 0;
static float corr_xpos = 0,corr_ypos = 0;
static float acck = 0;
void accel_filter_predict(float dt,float x[3])
{
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
}
void inertial_filter_correct(float e,float dt,float x[3],int i,float w,float vp)
{
float ewdt = e * w * dt;
x[i] += ewdt;
if(i == 0)
{
x[1] += vp*w * ewdt;
}
}
void check_stop(int16 accz_velocity,int16 accz)
{
static uint8 slow_down_count=0;
static uint16 down_time=0;
down_time++;
if(fly_status.HOLD_STATUS == HOLD_DOWN)
{
if(down_time>=200)
{
if((accz_velocity>=0&&accz<50&&accz>-50) || (accz_velocity>=50) || (accz>300 || accz<-300))
{
slow_down_count++;
if(slow_down_count>3)
{
fly_status.HOLD_STATUS=HOLD_STOP;
}
}
else if(slow_down_count>0)
{
if(accz_velocity<0)
slow_down_count=0;
else
slow_down_count--;
}
}
}
else
{
down_time=0;
slow_down_count=0;
}
}
void accel_baro_mag_gps_calculator(int16 accel_x,int16 accel_y,int16 accel_z)
{
static uint8 usart1_count = 0;
static int16 acc_xyz[3];
static float no_filteraccxx,no_filteraccyy,no_filteracczz;
if (accel_x != 0 && accel_y != 0 && accel_z != 0)
{
acck=(1/Fylib_Qrsqrt(accel_x*accel_x + accel_y*accel_y+accel_z*accel_z)-MV_ACC_RP)/MV_ACC_RP;
if(acck<0) acck=-acck;
}
acc_xyz[0]=accel_x*980/MV_ACC_RP;acc_xyz[1]=accel_y*980/MV_ACC_RP;acc_xyz[2]=accel_z*980/MV_ACC_RP;
//ÉÏÕýÏÂ¸º£¬±±ÕýÄÏ¸º£¬¶«ÕýÎ÷¸º
no_filteracczz=-acc_xyz[1]*vehicle_att.R[2][0]-acc_xyz[0]*vehicle_att.R[2][1]+acc_xyz[2]*vehicle_att.R[2][2]-980;
no_filteraccxx=-(-acc_xyz[1]*vehicle_optical.R[0][0]-acc_xyz[0]*vehicle_optical.R[0][1]+acc_xyz[2]*vehicle_optical.R[0][2]);
no_filteraccyy=(acc_xyz[1]*vehicle_optical.R[1][0]+acc_xyz[0]*vehicle_optical.R[1][1]-acc_xyz[2]*vehicle_optical.R[1][2]);
// no_filteracczz += (float)(agx + agy);
// no_filteraccxx += (float)(agy + agz);
// no_filteraccyy += (float)(agx + agz);
//high_pass
real_acceleration.xx=(int16)no_filteraccxx;
real_acceleration.yy=(int16)no_filteraccyy;
real_acceleration.zz=(int16)no_filteracczz;
TimeUpdate(&baro_acc_alt,(realacc*)&real_acceleration);
check_stop(baro_acc_alt.velocity_hat,no_filteracczz);
usart1_count++;
if(usart1_count>=1)
{
usart1_count = 0;
// SendDatShowINT((int)z_est[0],(int)(baro_data.baro_home),(int)(baro_data.velocity*3.1f),(int)baro_to_alt);
SendDatShow((int)y_est[1],(int)x_est[1],(int)(acck * 100),(int)baro_data.baro_home);
}
}
void ahrs_update_R_bf_to_ef(float angle_pitch,float angle_roll,float angle_yaw)
{
float sin_pitch = sin(angle_pitch * M_DEG_TO_RAD);
float cos_pitch = cos(angle_pitch * M_DEG_TO_RAD);
float sin_roll = sin(angle_roll * M_DEG_TO_RAD);
float cos_roll = cos(angle_roll * M_DEG_TO_RAD);
float sin_yaw = sin(angle_yaw * M_DEG_TO_RAD);
float cos_yaw = cos(angle_yaw * M_DEG_TO_RAD);
vehicle_att.R[0][0] = cos_pitch * cos_yaw;
vehicle_optical.R[0][0]=cos_pitch;
vehicle_att.R[0][1] = sin_roll * sin_pitch * cos_yaw + cos_roll * sin_yaw;
vehicle_optical.R[0][1] = 0;//sin_roll * sin_pitch + cos_roll;
vehicle_att.R[0][2] = cos_roll * sin_pitch * cos_yaw - sin_roll * sin_yaw;
vehicle_optical.R[0][2] = sin_pitch;//cos_roll * sin_pitch - sin_roll;
vehicle_att.R[1][0] = cos_pitch * sin_yaw;
vehicle_optical.R[1][0] = 0;//cos_pitch;
vehicle_att.R[1][1] = -sin_roll * sin_pitch * sin_yaw - cos_roll * cos_yaw;
vehicle_optical.R[1][1] =cos_roll;// -sin_roll * sin_pitch - cos_roll;
vehicle_att.R[1][2] = cos_roll * sin_pitch * sin_yaw + sin_roll * cos_yaw;
vehicle_optical.R[1][2] =-sin_roll;// cos_roll * sin_pitch;// + sin_roll;
vehicle_att.R[2][0] = -sin_pitch;
vehicle_att.R[2][1] = sin_roll * cos_pitch;
vehicle_att.R[2][2] = cos_roll * cos_pitch;
}
#define HIGH_PASS_PARAM 0.999f
float baro_vel_gain = 0.2f, x_vel_gain = 1.8f, y_vel_gain = 1.8f;
float baro_gain=0.05f;
void TimeUpdate(struct vertical_information *pAltitude,realacc* acc)
{
static float high_pass_zvel = 0;//, z_est1f = 0;
z_est[2]=acc->zz-z_bias;
accel_filter_predict(INAV_T,z_est);
z_est[0] += corr_baro * w_z_baro * INAV_T;
z_bias -= corr_baro * 0.001f * INAV_T;
z_est[1] += corr_baro * baro_vel_gain * INAV_T;
// if (z_est[1] >-20 && z_est[1] <20)
// high_pass_zvel = (high_pass_zvel + z_est[1] - z_est1f) * HIGH_PASS_PARAM;
// else
high_pass_zvel = z_est[1];
// z_est1f = z_est[1];
pAltitude->altitude_hat=z_est[0];
pAltitude->velocity_hat=high_pass_zvel;
ObservationOPT_Update(x_est[0],optflow.velx,y_est[0],optflow.vely);
x_est[2]=acc->xx-x_bias;
accel_filter_predict(INAV_T,x_est);
x_est[1] += corr_xvel * x_vel_gain * INAV_T;
y_est[2]=acc->yy-y_bias;
accel_filter_predict(INAV_T,y_est);
y_est[1] += corr_yvel * y_vel_gain * INAV_T;
pos_vel_xy.posx = 0;
pos_vel_xy.posy = 0;
pos_vel_xy.velx = optflow.velx;//x_est[1];
pos_vel_xy.vely = optflow.vely;//y_est[1];
}
__AVERAGE_DATAF altitude_vcal={0,0};
float ABS_f(float values)
{
if (values < 0 )
return -values;
else
return values;
}
void ObservationUpdate(int baro_disdance,int16 baro_vel)
{
uint8 i;
corr_baro = (float)baro_disdance - z_est[0];
if (corr_baro >20 || corr_baro <-20)
baro_vel_gain = 0.4f;
else
baro_vel_gain = 0.2f;
if (fly_status.HOLD_STATUS == HOLD_STOP)
{
z_est[0] = baro_disdance;
z_est[1] = 0;
}
return;
}
void ObservationOPT_Update(float pos_x,float vel_x,float pos_y,float vel_y)
{
uint8 i;
corr_xpos = pos_x-x_est[0];
corr_xvel = vel_x-x_est[1];//x_vel_history[0]);
corr_ypos = pos_y-y_est[0];
corr_yvel = vel_y-y_est[1];//y_vel_history[0]);
// for (i = 0; i < XY_VEL_HISTORY - 1; i++)
// {
// x_vel_history[i] = x_vel_history[i+1];
// y_vel_history[i] = y_vel_history[i+1];
// }
// x_vel_history[XY_VEL_HISTORY - 1] = x_est[1];
// y_vel_history[XY_VEL_HISTORY - 1] = y_est[1];
return;
}

復(fù)制代碼