上一篇說了傳感器數據讀取，接下來來看慣性傳感器數據讀取
//我們來看這個函數現在到了第二部分慣性傳感器數據讀取
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;
如有錯誤希望在評論區指出
排版不佳