本文详细介绍了四轴飞行器的控制原理，包括欧拉角、加速度传感器、陀螺仪、磁力计和气压传感器在飞行姿态测量中的作用。通过匿名飞控拓空pro的硬件配置，展示了数据读取和处理流程，如imu_update()函数中姿态解算的过程，利用四元数进行姿态更新和误差修正。同时，还涉及到串级PID的预备知识。
四轴飞行器的控制原理
四轴飞行器的结构（待补）
四轴飞行器的运动控制方法（待补）
四轴飞行器各部分工作原理
飞行姿态与升力关系
1.绕y轴旋转α角度
2.绕x轴旋转β角度
3.绕z轴旋转γ角度
飞行姿态测量
飞行姿态是一个真实飞行物体与参考坐标系之间的角度关系。如上方提到的α、β、γ角，这三个角度也称之为欧拉角，对应Pitch、Yaw、Roll。常用的姿态测量传感器有加速度传感器、角速度传感器、磁力传感器、气压传感器、超声波及GPS等。**这里匿名飞控拓空pro，采用的是惯性传感器icm20602（3轴陀螺 + 3轴加速度 + 恒温设计）、磁场传感器ak8975（3磁罗盘）、 气压传感器spl06 （高精度气压计，灵敏度5cm）**获取较为精确姿态定位数据，则需要融合计算上述多个传感器测量数据。
加速度传感器
加速度传感器是测量由物体重力加速度引起的加速度量。物理静止或运动过程中。受重力作用，会产生物体相对于三个坐标轴方向上的重力分量，通过对重力分量进行量化，运用三角函数计算出物体相对于三个坐标轴的倾角。
陀螺仪（角度+角速度）
角速度传感器（陀螺仪）用来测量一段时间内角度变化速率。对两次测量时间差值进行积分可以得到角度增量值。增量值可正可负，正值表示向角度增大方向旋转，负值表示向原角度减小反方向旋转，积分后与测量前初始角度求和可计算出当前角度。
磁力计传感器
与加速度计组成电子罗盘。当磁力计与惯性传感器组合使用进行姿态测试时，磁力计用来估算运动目标的航向角，用以校正陀螺仪漂移误差。
匿名拓空pro TI
main.c
#include "sysconfig.h"
#include "Drv_Bsp.h"
#include "Ano_Scheduler.h"
#include "Ano_FcData.h"
int main(void)
{
Drv_BspInit();
//
flag.start_ok = 1;
while(1)
{
//轮询任务调度器
Main_Task();
}
}
进入Main_Task()查看函数：
Ano_Scheduler.c
u8 Main_Task(void)
{
uint8_t index = 0;
//查询1ms任务是否需要执行，通过判断 lt0_run_flag 是否为0， lt0_run_flag 在 im20602 中断服务函数里，当数据准备好后发出中断请求
if(lt0_run_flag!=0)
{
//
lt0_run_flag--;
Loop_Task_0();
}
//循环判断其他所有线程任务，是否应该执行
uint32_t time_now,delta_time_us;
for(index=0;index < TASK_NUM;index++)
{
//获取系统当前时间，单位US
time_now = GetSysRunTimeUs();//SysTick_GetTick();
//进行判断，如果当前时间减去上一次执行的时间，大于等于该线程的执行周期，则执行线程
if(time_now - sched_tasks[index].last_run >= sched_tasks[index].interval_ticks)
{
delta_time_us = (u32)(time_now - sched_tasks[index].last_run);
//更新线程的执行时间，用于下一次判断
sched_tasks[index].last_run = time_now;
//执行线程函数，使用的是函数指针
sched_tasks[index].task_func(delta_time_us);
}
}
return 0;
}
先分析 Loop_Task_0() 通过判断 lt0_run_flag 是否为0， lt0_run_flag 在 im20602 中断服务函数里，当数据准备好后发出中断请求
Ano_Scheduler.c
/**
Loop_Task_0
负责硬件数据读取的任务
**/
static void Loop_Task_0()//1ms执行一次
{
//
/*传感器数据读取*/
Fc_Sensor_Get();
/*惯性传感器数据准备*/
Sensor_Data_Prepare(1);
/*姿态解算更新*/
IMU_Update_Task(1);
/*获取WC_Z加速度*/
WCZ_Acc_Get_Task();
WCXY_Acc_Get_Task();
/*飞行状态任务*/
Flight_State_Task(1,CH_N);
/*开关状态任务*/
Swtich_State_Task(1);
/*光流融合数据准备任务*/
ANO_OF_Data_Prepare_Task(0.001f);
/*数传数据交换*/
ANO_DT_Data_Exchange();
}
分析本节所讲到的各种传感器数据读取函数 Fc_Sensor_Get()
Ano_FlightDataCal.c
void Fc_Sensor_Get()//1ms
{
static u8 cnt;
if(flag.start_ok)
{
/*读取陀螺仪加速度计数据*/
Drv_Icm20602_Read();
cnt ++;
cnt %= 20;
//by WPR :20ms执行一次
if(cnt==0)
{
/*读取电子罗盘磁力计数据*/
Drv_AK8975_Read();
/*读取气压计数据*/
baro_height = (s32)Drv_Spl0601_Read();
}
}
test_time_cnt++;
}
紧接分析Drv_Icm20602_Read() 读取陀螺仪加速度计数据
Drv_icm20602.c
void Drv_Icm20602_Read( void )
{
//读取传感器寄存器，连续读14个字节
icm20602_readbuf(MPUREG_ACCEL_XOUT_H,14,mpu_buffer);
//数据赋值
ICM_Get_Data();
}
重点分析 ICM_Get_Data()
Drv_icm20602.c
void ICM_Get_Data()
{
s16 temp[2][3];
// /*读取buffer原始数据*/
temp[0][X] = (s16)((((u16)mpu_buffer[0]) << 8) | mpu_buffer[1]);//>>1;// + 2 *sensor.Tempreature_C;// + 5 *sensor.Tempreature_C;
temp[0][Y] = (s16)((((u16)mpu_buffer[2]) << 8) | mpu_buffer[3]);//>>1;// + 2 *sensor.Tempreature_C;// + 5 *sensor.Tempreature_C;
temp[0][Z] = (s16)((((u16)mpu_buffer[4]) << 8) | mpu_buffer[5]);//>>1;// + 4 *sensor.Tempreature_C;// + 7 *sensor.Tempreature_C;
temp[1][X] = (s16)((((u16)mpu_buffer[ 8]) << 8) | mpu_buffer[ 9]) ;
temp[1][Y] = (s16)((((u16)mpu_buffer[10]) << 8) | mpu_buffer[11]) ;
temp[1][Z] = (s16)((((u16)mpu_buffer[12]) << 8) | mpu_buffer[13]) ;
sensor.Tempreature = ((((int16_t)mpu_buffer[6]) << 8) | mpu_buffer[7]); //tempreature
/*icm20602温度*/
sensor.Tempreature_C = sensor.Tempreature/326.8f + 25 ;//sensor.Tempreature/340.0f + 36.5f;
//调整物理坐标轴与软件坐标轴方向定义一致
//by WPR :读取数据保存到sensor.Acc_Original[]中（原始数据）
sensor.Acc_Original[X] = temp[0][X];
sensor.Acc_Original[Y] = temp[0][Y];
sensor.Acc_Original[Z] = temp[0][Z];
sensor.Gyro_Original[X] = temp[1][X];
sensor.Gyro_Original[Y] = temp[1][Y];
sensor.Gyro_Original[Z] = temp[1][Z];
}
回到 Ano_FlightDataCal.c
分析Drv_AK8975_Read() 读取电子罗盘数据进入函数
drv_ak8975.c
具体里面读取的数据形式和存储格式还未理解（待补）
static u8 ak8975_buf[6];
void Drv_AK8975_Read(void)
{
ak8975_enable(1);
Drv_Spi0SingleWirteAndRead(AK8975_HXL_REG|0x80);
for(u8 i=0; i<6; i++)
ak8975_buf[i] = Drv_Spi0SingleWirteAndRead(0xff);
ak8975_enable(0);
ak8975_Trig();
}
同上，气压计Drv_Spl0601_Read（）
drv_spl06.c
float Drv_Spl0601_Read ( void )
{
spl0601_get_raw_temp();
temperature = spl0601_get_temperature();
spl0601_get_raw_pressure();
baro_pressure = spl0601_get_pressure();
//alt_high = powf((temp/101325),1/5.255f);
/
alt_3 = ( 101400 - baro_pressure ) / 1000.0f;
height = 0.82f * alt_3 * alt_3 * alt_3 + 0.09f * ( 101400 - baro_pressure ) * 100.0f ;
alt_high = ( height - baro_Offset ) ; //cm +
/
return alt_high; //by WPR :反馈高度信息
}
回到Ano_Scheduler.c中的Main_Task()
分析/惯性传感器数据准备/
Sensor_Data_Prepare(1);
Ano_Sensor_Basic.c
将传感器读取数据进行：除去偏置、滤波、得到角加速度。
再之后，求出角加速度的目的是对加速度计的结果进行修正，最终得到角速度、加速度。
最重要部分：最后的单位换算
void Sensor_Data_Prepare(u8 dT_ms)
{
float hz = 0 ;
if(dT_ms != 0) hz = 1000/dT_ms;
// MPU6050_Read();
// sensor_rotate_func(dT);
/*静止检测*/
motionless_check(dT_ms);
MPU6050_Data_Offset(); //校准函数
/*得出校准后的数据*/
for(u8 i=0;i<3;i++)
{
sensor_val[A_X+i] = sensor.Acc_Original[i] ;
sensor_val[G_X+i] = sensor.Gyro_Original[i] - save.gyro_offset[i] ;
//sensor_val[G_X+i] = (sensor_val[G_X+i] >>2) <<2;
}
/*可将整个传感器坐标进行旋转*/
// for(u8 j=0;j<3;j++)
// {
// float t = 0;
//
// for(u8 i=0;i<3;i++)
// {
//
// t += sensor_val[A_X + i] *wh_matrix[j][i];
// }
//
// sensor_val_rot[A_X + j] = t;
// }
// for(u8 j=0;j<3;j++)
// {
// float t = 0;
//
// for(u8 i=0;i<3;i++)
// {
//
// t += sensor_val[G_X + i] *wh_matrix[j][i];
// }
//
// sensor_val_rot[G_X + j] = t;
// }
/*赋值*/
for(u8 i = 0;i<6;i++)
{
sensor_val_rot[i] = sensor_val[i];
}
/*数据坐标转90度*/
sensor_val_ref[G_X] = sensor_val_rot[G_Y] ;
sensor_val_ref[G_Y] = -sensor_val_rot[G_X] ;
sensor_val_ref[G_Z] = sensor_val_rot[G_Z];
sensor_val_ref[A_X] = (sensor_val_rot[A_Y] - save.acc_offset[Y] ) ;
sensor_val_ref[A_Y] = -(sensor_val_rot[A_X] - save.acc_offset[X] ) ;
sensor_val_ref[A_Z] = (sensor_val_rot[A_Z] - save.acc_offset[Z] ) ;
/*单独校准z轴模长*/
mpu_auto_az();
//======================================================================
/*软件低通滤波*/
for(u8 i=0;i<3;i++)
{
//
gyr_f[4][X +i] = (sensor_val_ref[G_X + i] );
acc_f[4][X +i] = (sensor_val_ref[A_X + i] );
//
for(u8 j=4;j>0;j--)
{
//
gyr_f[j-1][X +i] += GYR_ACC_FILTER *(gyr_f[j][X +i] - gyr_f[j-1][X +i]);
acc_f[j-1][X +i] += GYR_ACC_FILTER *(acc_f[j][X +i] - acc_f[j-1][X +i]);
}
// LPF_1_(100,dT_ms*1e-3f,sensor_val_ref[G_X + i],sensor.Gyro[X +i]);
// LPF_1_(100,dT_ms*1e-3f,sensor_val_ref[A_X + i],sensor.Acc[X +i]);
}
/*旋转加速度补偿*/
//======================================================================
for(u8 i=0;i<3;i++)
{
center_pos.gyro_rad_old[i] = center_pos.gyro_rad[i];
center_pos.gyro_rad[i] = gyr_f[0][X + i] *RANGE_PN2000_TO_RAD;//0.001065f;
center_pos.gyro_rad_acc[i] = hz *(center_pos.gyro_rad[i] - center_pos.gyro_rad_old[i]);
}
center_pos.linear_acc[X] = +center_pos.gyro_rad_acc[Z] *center_pos.center_pos_cm[Y] - center_pos.gyro_rad_acc[Y] *center_pos.center_pos_cm[Z];
center_pos.linear_acc[Y] = -center_pos.gyro_rad_acc[Z] *center_pos.center_pos_cm[X] + center_pos.gyro_rad_acc[X] *center_pos.center_pos_cm[Z];
center_pos.linear_acc[Z] = +center_pos.gyro_rad_acc[Y] *center_pos.center_pos_cm[X] - center_pos.gyro_rad_acc[X] *center_pos.center_pos_cm[Y];
//======================================================================
/*赋值*/
for(u8 i=0;i<3;i++)
{
sensor.Gyro[X+i] = gyr_f[0][i];//sensor_val_ref[G_X + i];
sensor.Acc[X+i] = acc_f[0][i] - center_pos.linear_acc[i]/RANGE_PN16G_TO_CMSS;//sensor_val_ref[A_X+i];//
}
/*转换单位*/
for(u8 i =0 ;i<3;i++)
{
/*陀螺仪转换到度每秒，量程+-2000度*/
sensor.Gyro_deg[i] = sensor.Gyro[i] *0.061036f ;// /65535 * 4000; +-2000度 0.061
/*陀螺仪转换到弧度度每秒，量程+-2000度*/
sensor.Gyro_rad[i] = sensor.Gyro_deg[i] *0.01745f;//sensor.Gyro[i] *RANGE_PN2000_TO_RAD ;// 0.001065264436f //微调值 0.0010652f
/*加速度计转换到厘米每平方秒，量程+-8G*/
sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );// /65535 * 16*981; +-8G
}
}
回到Ano_Scheduler.c中的Main_Task()
分析 /姿态解算更新/
IMU_Update_Task(1);
Ano_FlightDataCal.c
static u8 reset_imu_f;
void IMU_Update_Task(u8 dT_ms)
{
/*如果准备飞行，复位重力复位标记和磁力计复位标记*/
if(flag.unlock_sta )
{
imu_state.G_reset = imu_state.M_reset = 0;
reset_imu_f = 0;
}
else
{
if(flag.motionless == 0)
{
// imu_state.G_reset = 1;//自动复位
//sensor.gyr_CALIBRATE = 2;
}
if(reset_imu_f==0 )//&& flag.motionless == 1)
{
imu_state.G_reset = 1;//自动复位
sensor.gyr_CALIBRATE = 2;//校准陀螺仪，不保存
reset_imu_f = 1; //已经置位复位标记
}
}
if(0)
{
imu_state.gkp = 0.0f;
imu_state.gki = 0.0f;
}
else
{
if(0)
{
imu_state.gkp = 0.2f;
}
else
{
/*设置重力互补融合修正kp系数*/
imu_state.gkp = 0.2f;//0.4f;
}
/*设置重力互补融合修正ki系数*/
imu_state.gki = 0.01f;
/*设置罗盘互补融合修正ki系数*/
imu_state.mkp = 0.1f;
}
imu_state.M_fix_en = sens_hd_check.mag_ok; //磁力计修正使能
/*姿态计算，更新，融合*/
IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
//
}
进入上面函数中的 IMU_update（）/姿态计算，更新，融合/ 分析
Ano_Imu.c
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], s32 acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)
{
// const float kp = 0.2f,ki = 0.001f;
// const float kmp = 0.1f;
static float kp_use = 0,ki_use = 0,mkp_use = 0;
float acc_norm_l,acc_norm_l_recip,q_norm_l;
float acc_norm[VEC_XYZ];
float d_angle[VEC_XYZ];
// q0q0 = imu->w * imu->w;
q0q1 = imu->w * imu->x;
q0q2 = imu->w * imu->y;
q1q1 = imu->x * imu->x;
q1q3 = imu->x * imu->z;
q2q2 = imu->y * imu->y;
q2q3 = imu->y * imu->z;
q3q3 = imu->z * imu->z;
q1q2 = imu->x * imu->y;
q0q3 = imu->w * imu->z;
if(state->obs_en)
{
//计算机体坐标下的运动加速度观测量。坐标系为北西天
for(u8 i = 0;i<3;i++)
{
s32 temp = 0;
for(u8 j = 0;j<3;j++)
{
temp += imu->obs_acc_w[j] *att_matrix[j][i];//t[i][j] 转置为 t[j][i]
}
imu->obs_acc_a[i] = temp;
imu->gra_acc[i] = acc[i] - imu->obs_acc_a[i];
}
}
else
{
for(u8 i = 0;i<3;i++)
{
imu->gra_acc[i] = acc[i];
}
}
//
acc_norm_l_recip = my_sqrt_reciprocal(my_pow(imu->gra_acc[X]) + my_pow(imu->gra_acc[Y]) + my_pow(imu->gra_acc[Z]));
acc_norm_l = safe_div(1,acc_norm_l_recip,0);
// 加速度计的读数，单位化。
for(u8 i = 0;i<3;i++)
{
acc_norm[i] = imu->gra_acc[i] *acc_norm_l_recip;
}
// 载体坐标下的x方向向量，单位化。
att_matrix[0][0] = imu->x_vec[X] = 1 - (2*q2q2 + 2*q3q3);
att_matrix[0][1] = imu->x_vec[Y] = 2*q1q2 - 2*q0q3;
att_matrix[0][2] = imu->x_vec[Z] = 2*q1q3 + 2*q0q2;
// 载体坐标下的y方向向量，单位化。
att_matrix[1][0] = imu->y_vec[X] = 2*q1q2 + 2*q0q3;
att_matrix[1][1] = imu->y_vec[Y] = 1 - (2*q1q1 + 2*q3q3);
att_matrix[1][2] = imu->y_vec[Z] = 2*q2q3 - 2*q0q1;
// 载体坐标下的z方向向量（等效重力向量、重力加速度向量），单位化。
att_matrix[2][0] = imu->z_vec[X] = 2*q1q3 - 2*q0q2;
att_matrix[2][1] = imu->z_vec[Y] = 2*q2q3 + 2*q0q1;
att_matrix[2][2] = imu->z_vec[Z] = 1 - (2*q1q1 + 2*q2q2);
//水平面方向向量
float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;
// 计算载体坐标下的运动加速度。(与姿态解算无关)
for(u8 i = 0;i<3;i++)
{
imu->a_acc[i] = (s32)(acc[i] - 981 *imu->z_vec[i]);
}
//计算世界坐标下的运动加速度。坐标系为北西天
for(u8 i = 0;i<3;i++)
{
s32 temp = 0;
for(u8 j = 0;j<3;j++)
{
temp += imu->a_acc[j] *att_matrix[i][j];
}
imu->w_acc[i] = temp;
}
w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);
// 测量值与等效重力向量的叉积（计算向量误差）。
vec_err[X] = (acc_norm[Y] * imu->z_vec[Z] - imu->z_vec[Y] * acc_norm[Z]);
vec_err[Y] = -(acc_norm[X] * imu->z_vec[Z] - imu->z_vec[X] * acc_norm[Z]);
vec_err[Z] = -(acc_norm[Y] * imu->z_vec[X] - imu->z_vec[Y] * acc_norm[X]);
#ifdef USE_MAG
//电子罗盘赋值为float矢量
for(u8 i = 0;i<3;i++)
{
mag_val_f[i] = (float)mag_val[i];
}
if(!(mag_val[X] ==0 && mag_val[Y] == 0 && mag_val[Z] == 0))
{
//把载体坐标下的罗盘数据转换到地理坐标下
a2w_3d_trans(mag_val_f,imu->w_mag);
//计算方向向量归一化系数（模的倒数）
float l_re_tmp = my_sqrt_reciprocal(my_pow(imu->w_mag[0]) + my_pow(imu->w_mag[1]));
//计算南北朝向向量
mag_2d_w_vec[1][0] = imu->w_mag[0] *l_re_tmp;
mag_2d_w_vec[1][1] = imu->w_mag[1] *l_re_tmp;
//计算南北朝向误差(叉乘)，地理坐标中，水平面磁场方向向量应恒为南北 (1,0)
mag_yaw_err = vec_2_cross_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
//计算南北朝向向量点乘，判断同向或反向
mag_err_dot_prudoct = vec_2_dot_product(mag_2d_w_vec[1],mag_2d_w_vec[0]);
//若反向，直接给最大误差
if(mag_err_dot_prudoct<0)
{
mag_yaw_err = my_sign(mag_yaw_err) *1.0f;
}
//
}
#endif
for(u8 i = 0;i<3;i++)
{
#ifdef USE_EST_DEADZONE
if(state->G_reset == 0 && state->obs_en == 0)
{
vec_err[i] = my_deadzone(vec_err[i],0,imu->gacc_deadzone[i]);
}
#endif
#ifdef USE_LENGTH_LIM
if(acc_norm_l>1060 || acc_norm_l<900)
{
vec_err[X] = vec_err[Y] = vec_err[Z] = 0;
}
#endif
//误差积分
vec_err_i[i] += LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;
// 构造增量旋转（含融合纠正）。
// d_angle[X] = (gyr[X] + (vec_err[X] + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
// d_angle[Y] = (gyr[Y] + (vec_err[Y] + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
// d_angle[Z] = (gyr[Z] + (vec_err[Z] + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;
#ifdef USE_MAG
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif
}
// 计算姿态。
imu->w = imu->w - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
imu->x = imu->w*d_angle[X] + imu->x + imu->y*d_angle[Z] - imu->z*d_angle[Y];
imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y + imu->z*d_angle[X];
imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;
q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
imu->w *= q_norm_l;
imu->x *= q_norm_l;
imu->y *= q_norm_l;
imu->z *= q_norm_l;
/修正开关///
#ifdef USE_MAG
if(state->M_fix_en==0)//磁力
{
mkp_use = 0;//不修正
state->M_reset = 0;//罗盘修正不复位，清除复位标记
}
else
{
if(state->M_reset)//
{
//通过增量进行对准
mkp_use = 10.0f;
if(mag_yaw_err != 0 && ABS(mag_yaw_err)<0.02f)
{
state->M_reset = 0;//误差小于2的时候，清除复位标记
}
}
else
{
mkp_use = state->mkp; //正常修正
}
}
#endif
if(state->G_fix_en==0)//重力方向修正
{
kp_use = 0;//不修正
}
else
{
if(state->G_reset == 0)//正常修正
{
kp_use = state->gkp;
ki_use = state->gki;
}
else//快速修正，通过增量进行对准
{
kp_use = 10.0f;
ki_use = 0.0f;
// imu->est_speed_w[X] = imu->est_speed_w[Y] = 0;
// imu->est_acc_w[X] = imu->est_acc_w[Y] = 0;
// imu->est_acc_h[X] = imu->est_acc_h[Y] =0;
//计算静态误差是否缩小
// imu_reset_val += (ABS(vec_err[X]) + ABS(vec_err[Y])) *1000 *dT;
// imu_reset_val -= 0.01f;
imu_reset_val = (ABS(vec_err[X]) + ABS(vec_err[Y]));
imu_reset_val = LIMIT(imu_reset_val,0,1.0f);
if((imu_reset_val < 0.05f) && (state->M_reset == 0))
{
//计时
reset_cnt += 2;
if(reset_cnt>400)
{
reset_cnt = 0;
state->G_reset = 0;//已经对准，清除复位标记
}
}
else
{
reset_cnt = 0;
}
}
}
}
以上代码看似繁琐，实则只需要知道是根据角速度和加速度计修正得到了当前的姿态，并且打开对应头文件Ano_Imu.h会发现，这些修正后的数据以四元数形式保存，并且是一个结构体（全面涵盖了有关无人机运动状态的各种数据）
typedef struct
{
float w;//q0;
float x;//q1;
float y;//q2;
float z;//q3;
float x_vec[VEC_XYZ];
float y_vec[VEC_XYZ];
float z_vec[VEC_XYZ];
float hx_vec[VEC_XYZ];
float a_acc[VEC_XYZ];
float w_acc[VEC_XYZ];
float h_acc[VEC_XYZ];
float w_mag[VEC_XYZ];
float gacc_deadzone[VEC_XYZ];
float obs_acc_w[VEC_XYZ];
float obs_acc_a[VEC_XYZ];
float gra_acc[VEC_XYZ];
float est_acc_a[VEC_XYZ];
float est_acc_h[VEC_XYZ];
float est_acc_w[VEC_XYZ];
float est_speed_h[VEC_XYZ];
float est_speed_w[VEC_XYZ];
float rol;
float pit;
float yaw;
} _imu_st ;
姿态解算到此为止
下一节讲述串级PID
扫一扫
375
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
