匿名四轴【任务一(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;

如果有错误,希望再评论区指出
好了代码先到这,后续代码还会继续讲解
排版不加,见谅

你可能感兴趣的:(匿名四轴【任务一(1000Hz)之读取传感器】)