匿名四軸【任務一(1000Hz)之讀取傳感器】

//爲什麼講上面的呢因爲下面有GetSysTime_us函數,
//返回結果值是value就是我們的記錄的時間
//看到1000你應該就明白了每1ms執行一次
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]) ;	
}

讀取傳感器數據

u16 test_time_cnt;
void Fc_Sensor_Get()//1ms
{
	static u8 cnt;
    /*
    在主函數中flag.start_ok = All_Init();
    //進行所有設備的初始化，並將初始化結果保存
    初始化成功返回1，其實看過All_Init()知道無論什麼時候都返回的是1，
    也就說只要函數All_Init()運行過	  
    不管傳感器存不存在start_ok都爲1
    */
	if(flag.start_ok)//All_Init運行過就爲1
	{
		/*讀取陀螺儀加速度計數據*/
		Drv_Icm20602_Read();
		cnt ++;
		cnt %= 20;
        //每一次讀取數據cnt都實現+1操作，
        //當加到20實現下面的電子羅盤磁力計數據讀取和氣壓計數據讀取
        //這樣實現了20ms讀取電子羅盤磁力計和氣壓計讀取
		if(cnt==0)
		{
			/*讀取電子羅盤磁力計數據*/
			Drv_AK8975_Read();
			/*讀取氣壓計數據*/
			baro_height = (s32)Drv_Spl0601_Read();
            //baro_height指的是氣壓高度
		}
	}	
	test_time_cnt++;
}

陀螺儀讀數

u8 mpu_buffer[14];
void Drv_Icm20602_Read()
{
    //#define MPUREG_ACCEL_XOUT_H                     0x3B
	icm20602_readbuf(MPUREG_ACCEL_XOUT_H,14,mpu_buffer);//mpu_buffer用來接收收到的數據
	ICM_Get_Data();
}

icm20602

/*
	陀螺儀數據讀取,我們採取的陀螺儀 ICM20602是INVENSENSE一款產品
	INVENSENSE生產MPU6050，其操作和ICM20602差不多
	這裏的data是mpu_buffer，length=14，reg = 0X3B
*/
static void icm20602_readbuf(u8 reg, u8 length, u8 *data)
{
	icm20602_enable(1);
    //通過設置CS信號來達到使能和失能，CS即片選信號當有多個芯片接在一條總線上
    //可以通過片選信號來實現對應芯片工作
	Drv_SPI2_RW(reg|0x80);
    //寫數據和讀數據
	Drv_SPI2_Receive(data,length);
    //接收函數
	icm20602_enable(0);
}
//接下來我們來具體看一下上面提到的兩個函數
//接收函數 發送0然後將接收到的數存儲在mpu_buffer中，一共十四次，
//正好填滿mpu_buffer[14]
void Drv_SPI2_Receive(uint8_t *pData, uint16_t Size)
{
    for(uint16_t i=0; i<Size; i++)
    {
        pData[i] = Drv_SPI2_RW(0);
    }
}
//發一個數據然後返回接收到的數據
u8 Dcrv_SPI2_RW(u8 dat)
{
    while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
    SPI_I2S_SendData(SPI2, dat);
    while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
    return SPI_I2S_ReceiveData(SPI2);
}

到此我們可以看懂icm20602_readbuf函數了但是我們還來看一下icm20602_writebyte函數日後可能會用到

//顯然經過上面readbuf講解看着部分代碼要顯得容易得多
static u8 icm20602_writebyte(u8 reg, u8 data)
{
    u8 status;
    icm20602_enable(1);
    status = Drv_SPI2_RW(reg);//發送reg數據接收返回值給status
    Drv_SPI2_RW(data);//發送data數據
    icm20602_enable(0);
    return status;
}
//這一部分代碼我們後續用到再進行講解

ICM_Get_Data

enum
{
	X = 0,
	Y = 1,
	Z = 2,
	VEC_XYZ,//3
};
//看到這裏可能有些人有點慌了，不要急
//enum是C語言提供的一種枚舉類型，這裏的枚舉是沒有typename的，
//作用類似於define
void ICM_Get_Data()
{
    //typedef int16_t s16;
	s16 temp[2][3];
	//	/*讀取buffer原始數據*/
  //還記得mpu_buffer[14]就是Drv_Icm20602_Read讀取的數據存儲的方法，
  //這裏我們知道了原來讀出來的是原始數據temp是16位數據，
  //收到的都是8位數據，收到的數據2個8位表示一個16位數據，
  //這兩個8位合在一起纔有意義，
  //單單一個8位只能表示一個數的低八位和高八位，
  //我們接受到的數據前一個數據表示高八位
	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溫度*/
//這個應該是溫度的計算，應該再陀螺儀手冊中有講到溫度二點換算公式
//senor指的應該是傳感器的意思，包括陀螺儀，氣壓計，羅盤磁力計
sensor.Tempreature_C = sensor.Tempreature/326.8f + 25 
//sensor.Tempreature/340.0f + 36.5f;
//讀出的sensor.Tempreature爲原始的溫度數據值，
//sensor.Tempreature_C爲換算後的攝氏度
//調整物理座標軸與軟件座標軸方向定義一致
//這裏的ACC表示加速度，看來陀螺儀很好，讀出來的直接是處理好的數據
sensor.Acc_Original[X] = temp[0][X];
sensor.Acc_Original[Y] = temp[0][Y];
sensor.Acc_Original[Z] = temp[0][Z];
//這裏的Gyro網上說的就是陀螺儀，暫時不清楚什麼意思，
//可能要到拿到芯片資料才知道了
sensor.Gyro_Original[X] = temp[1][X];
sensor.Gyro_Original[Y] = temp[1][Y];
sensor.Gyro_Original[Z] = temp[1][Z];
}
//這邊的sensor和下面提到的_sensor_st是同一個東西
typedef struct 
{
	u8 acc_CALIBRATE;
	u8 gyr_CALIBRATE;
	u8 acc_z_auto_CALIBRATE;
	
	s16 Acc_Original[VEC_XYZ];
	s16 Gyro_Original[VEC_XYZ];
	
	s16 Acc[VEC_XYZ];
	s32 Acc_cmss[VEC_XYZ];
	float Gyro[VEC_XYZ];
	float Gyro_deg[VEC_XYZ];
	float Gyro_rad[VEC_XYZ];

	s16 Tempreature;//出來的原始溫度
	float Tempreature_C;//出來的攝氏度
}_sensor_st;

如果有錯誤，希望再評論區指出
好了代碼先到這，後續代碼還會繼續講解
排版不加，見諒