匿名四轴【 任务一(1000Hz)惯性传感器数据读取一】
上一篇说了传感器数据读取，接下来来看惯性传感器数据读取
//我们来看这个函数现在到了第二部分惯性传感器数据读取
u32 test_dT_1000hz[3],test_rT[6];
static void Loop_1000Hz(void) //1ms执行一次
{
test_dT_1000hz[0] = test_dT_1000hz[1];
test_rT[3] = test_dT_1000hz[1] = GetSysTime_us ();
//记录当前的时间，并将存储的时间存储在test_rT[3]，
//test_dT_1000hz[1]中
//这样我们再看上面就是把上次存储的时间存储在test_dT_1000hz[0]
//总的来说
//test_dT_1000hz[0]上一次时间
//test_dT_1000hz[1]这次的时间
test_dT_1000hz[2] = (u32)(test_dT_1000hz[1] - test_dT_1000hz[0]) ;
//test_dT_1000hz[2]记录这次时间与上一次时间的差值
/*传感器数据读取*/
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();
test_rT[4]= GetSysTime_us ();
test_rT[5] = (u32)(test_rT[4] - test_rT[3]) ;
}
我们来看具体的传感器数据准备
//惯性传感器数据准备
void Sensor_Data_Prepare(u8 dT_ms)
//在上面函数中这边的dT_ms为1，
//还记得我们前面谈到的1000Hz是1ms执行一次，
//这边的dT_ms指的应该就是两次之间相隔的时间，
{
//定义频率，如果前后执行任务之间间距是1ms则可以看到频率为1000HZ
float hz = 0 ;
if(dT_ms != 0) hz = 1000/dT_ms;//判断dT_ms是否为0，否则不能作为除数，避免错误
// 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
}
}
motionless_check函数
//静止检测，motionless英文的意思就是静止的，不运动的
enum
{
X = 0,
Y = 1,
Z = 2,
VEC_XYZ,
//这里放一个数是为了说明这个枚举已经有3个值了，所以VEC为3
};
//首先我们看一下这个枚举，如果看不懂建议先去看一下我前面写的
//这里是一个枚举类型前面有讲过，这里的VEC_XYZ为3，因为枚举递增
float g_d_sum[VEC_XYZ] = {500,500,500};
s16 g_old[VEC_XYZ];//s16就是int16_t
//这边我们还看不出这个变量有什么用，
//不过关于这个变量的运用基本上就是下面这个函数
//所以只要看懂下面这个函数，就可以知道这个变量是用来干什么的
void motionless_check(u8 dT_ms)
{
u8 t = 0;
for(u8 i = 0;i<3;i++)
{
//#define ABS(x) ( (x)>0?(x):-(x) )----ABS就是取绝对值
g_d_sum[i] += 3*ABS(sensor.Gyro_Original[i] - g_old[i]) ;
//看到sensor.Gyro_Original[i] 不要慌，看一下我们前面有没讲过
//果然在前面读取传感器数据我们曾讲过这个东西，来看一下代码
/*
这里的Gyro应该是角速度
sensor.Gyro_Original[X] = temp[1][X];
sensor.Gyro_Original[Y] = temp[1][Y];
sensor.Gyro_Original[Z] = temp[1][Z];
*/
//而前面我们分析过temp中记录中的20602传感器的数据，
//就相当于Gyro_Original存储的是从20602读出的角速度数据
//那么现在这行代码这剩下g_old没解决，g_old引用地方只有下面那句话
//g_old[i] = sensor.Gyro_Original[i];
//很显然了存储的也是角速度，那只有可能是过去的角速度，
//想象一下，我们上面提到的几行代码是在下面哪一行代码执行
//的那么我们下次执行到这里时那么Gyro_Original的值已经更新了，
//但是g_old存储的还是原来的，
//那这里g_de_sum存储的就是原来的旧的数据
//那么这行代码的意思就是（这次的值与上一次值的偏差）的绝对值*3，
//这边*3是什么意思就是放大3倍的意思
g_d_sum[i] -= dT_ms ;
//看到这里需要讲解一下匿名代码是怎么检测静止的
//把前面说到的角速度差值放大3倍然后累加起来，
//每毫秒-1，如果这个值大于10说明飞机还没实现自稳
//如果小于10说明飞机已经基本实现自稳
g_d_sum[i] = LIMIT(g_d_sum[i],0,200);
//把这个值限定在0-200之间
if( g_d_sum[i] > 10)
{
t++;
}
g_old[i] = sensor.Gyro_Original[i];
}
if(t>=2)
{
flag.motionless = 0;
//如果出现3次大于0则可判定飞机未自稳
}
else
{
flag.motionless = 1;
}
}
//我们看完这个函数发现这个函数最主要作用就是返回一个自稳系数
//flag.motionless，如果motionless=1则可认为实现了自稳，
//如果为0则可认为没有实现自稳。
//flag是匿名代码贯穿始终的一个结构体所以最好遇到的时候最好注意点，
//方便之后理解代码
typedef struct
{
//基本状态/传感器
u8 start_ok;//初始化All_Init()这个值就为恒定的1
u8 sensor_imu_ok;
u8 mems_temperature_ok;
u8 motionless;//自稳系数，如果为1，实现自稳，如果为0，则没有实行自稳
u8 power_state;
u8 wifi_ch_en;
u8 chn_failsafe;
u8 rc_loss;
u8 rc_loss_back_home;
u8 gps_ok;
//控制状态
u8 manual_locked;
u8 unlock_err;
u8 unlock_cmd;
u8 unlock_sta;//unlocked
u8 thr_low;
u8 locking;
u8 taking_off; //起飞
u8 set_yaw;
u8 ct_loc_hold;
u8 ct_alt_hold;
//飞行状态
u8 flying;
u8 auto_take_off_land;
u8 home_location_ok;
u8 speed_mode;
u8 thr_mode;
u8 flight_mode;
u8 flight_mode2;
u8 gps_mode_en;
u8 motor_preparation;
u8 locked_rotor;
}_flag;
如有错误希望在评论区指出
排版不佳