无名飞控Time.c文件
由于无名飞控主要核心：传感器滤波、姿态解算、惯导、控制等代码在TIME.c里面运行，所以先来分析这个文件。
打开文件第一个函数：
void Timer4_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//结构体变量,这个结构体的成员是定时器的配置寄存器,如果将定时器的基址强制转换为这个结构体指针就可以依靠这个指针操作寄存器
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能或者失能APB1外设时钟
TIM_DeInit(TIM4);//将外设TIM4寄存器重设为缺省值
TIM_TimeBaseStructure.TIM_Period=4000;//TIM_Period设置了在下一个更新事件装入活动的自动重装载寄存器周期的值。它的取值必须在0x0000和0xFFFF之间。
TIM_TimeBaseStructure.TIM_Prescaler= (72 - 1);//设置了用来作为TIM4时钟频率除数的预分频值。它的取值必须在0x0000和0xFFFF之间
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;//设置了时钟分割
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;//TIM向上计数模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);//1us*1000=1ms
TIM_ClearFlag(TIM4, TIM_FLAG_Update);
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
IM_ClearFlag(TIM4, TIM_FLAG_Update);// 清除TIM4的待处理标志位
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE);// 使能或者失能指定的TIM中断, TIM中断源
TIM_Cmd(TIM4, ENABLE);// 使能或者失能TIMx外设
这个函数是初始化TIM4时钟，时钟开始运行后应发中断，从而运行姿态结算和控制代码。
//定义变量赋值
uint8 US_100_Start=0,US_100_Finished=1,US_100_Sampling_Cnt=0,US_100_Retry_Cnt=0;
uint8 ADNS3080_Cnt=0;
uint8 HC_SR04_Cnt=0;
Testime Time4_Delta;//结构体
uint16_t High_Okay_flag=0;typedef struct
{
float Last_Time;
float Now_Time;
float Time_Delta;
uint16 Time_Delta_INT;单位ms
}Testime;接下来判断中断是否发生
if(TIM_GetITStatus(TIM4,TIM_IT_Update)!=RESET )//检查指定的TIM4中断发生与否
{ 假如中断发生
Test_Period(&Time4_Delta);//系统调度时间测试器
NRF24L01_RC();//遥控器查询接收，非中端方式
/*************加速度计、陀螺仪数字量采集***********/
GET_MPU_DATA();//1.4msvoid Test_Period(Testime *Time_Lab)
{
Time_Lab->Last_Time=Time_Lab->Now_Time;
Time_Lab->Now_Time=(10000*TIME_ISR_CNT
+TIM2->CNT/2)/1000.0;单位ms
Time_Lab->Time_Delta=Time_Lab->Now_Time-Time_Lab->Last_Time;
Time_Lab->Time_Delta_INT=(uint16)(Time_Lab->Time_Delta);
}
这个函数是有TIME_ISR_CNT，这个量是从TIM2中来，这个量主要是记录现在已经工作了多少0.01S，TIM2->CNT/2是现在又经过了多少微秒，可以从TIM2中看出一点端倪：
void TIM2_Configuration_Cnt(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
TIM_DeInit(TIM2);
//TIM_TimeBaseStructure.TIM_Period = 60000;//30ms
//TIM_TimeBaseStructure.TIM_Period = 20000;//10ms
TIM_TimeBaseStructure.TIM_Period = 20000;//0.01s，系统运行1S，会计数2000000次，这里数20000即引发中断
TIM_TimeBaseStructure.TIM_Prescaler = 36-1; //36M/36/2=0.5us，分频36M，
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;系统时钟72M
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_ClearFlag(TIM2, TIM_FLAG_Update);
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM2, ENABLE);
}先不管NRF24L01_RC函数，是 nRF24L01的驱动函数，如果使用自己的接收机的话，这里应该用不着。
nRF24L01是由NORDIC生产的工作在2.4GHz~2.5GHz的ISM 频段的单片无线收发器芯片
进入 GET_MPU_DATA()得到mpu的数据
定义：
void GET_MPU_DATA(void)
{
//GET_ACC_DATA();
IMU_Filter();
GET_GYRO_DATA();
}
首先IMU_Filter函数
float X_Origion,Y_Origion,Z_Origion;
void IMU_Filter(void)/*得到加速度数据，并进行低通滤波*/
{
uint8_t axis;
imu.accelRaw[0] = GetData(ACCEL_XOUT_H);//imu结构体
imu.accelRaw[1] = GetData(ACCEL_YOUT_H);
imu.accelRaw[2] = GetData(ACCEL_ZOUT_H);
Acce_Correct_Filter();//对imu.accelRaw进行低通滤波得到数组Acce_Correct
X_Origion=K[0]*(imu.accelRaw[0])-B[0];//经过椭球校正后的三轴加速度量//初值均为0；
Y_Origion=K[1]*(imu.accelRaw[1])-B[1];
Z_Origion=K[2]*(imu.accelRaw[2])-B[2];
/*typedef struct
{
float Position[Axis_Num];//位置估计量
float Speed[Axis_Num];//速度估计量
float Acceleration[Axis_Num];//加速度估计量
float Pos_History[Axis_Num][Num];//历史惯导位置
float Vel_History[Axis_Num][Num];//历史惯导速度
float Acce_Bias[Axis_Num];//惯导加速度漂移量估计量
}SINS;
FilterBefore_NamelessQuad是一个SINS结构体 *///我注释的
FilterBefore_NamelessQuad.Acceleration[_YAW]=
-Sin_Roll* X_Origion
+ Sin_Pitch *Cos_Roll *Y_Origion
+ Cos_Pitch * Cos_Roll *Z_Origion;
FilterBefore_NamelessQuad.Acceleration[_PITCH]=
Cos_Yaw* Cos_Roll * X_Origion
+(Sin_Pitch*Sin_Roll*Cos_Yaw-Cos_Pitch * Sin_Yaw) * Y_Origion
+(Sin_Pitch * Sin_Yaw+Cos_Pitch * Sin_Roll * Cos_Yaw) * Z_Origion;
FilterBefore_NamelessQuad.Acceleration[_ROLL]=
Sin_Yaw* Cos_Roll * X_Origion
+(Sin_Pitch * Sin_Roll * Sin_Yaw +Cos_Pitch * Cos_Yaw) * Y_Origion
+ (Cos_Pitch * Sin_Roll * Sin_Yaw - Sin_Pitch * Cos_Yaw) * Z_Origion;
FilterBefore_NamelessQuad.Acceleration[_YAW]*=AcceGravity/AcceMax;
FilterBefore_NamelessQuad.Acceleration[_YAW]-=AcceGravity;
FilterBefore_NamelessQuad.Acceleration[_YAW]*=100;//加速度cm/s^2
FilterBefore_NamelessQuad.Acceleration[_PITCH]*=AcceGravity/AcceMax;
FilterBefore_NamelessQuad.Acceleration[_PITCH]*=100;//加速度cm/s^2
FilterBefore_NamelessQuad.Acceleration[_ROLL]*=AcceGravity/AcceMax;
FilterBefore_NamelessQuad.Acceleration[_ROLL]*=100;//加速度cm/s^2
Acce_Control_Filter();//加速度滤波，用于惯导、加速度控制反馈量再次滤波
/* 加速度计Butterworth滤波 */
/* 获取最新x(n) */
accButter[0].input[2] =Int_Sort(X_Origion);
accButter[1].input[2] =Int_Sort(Y_Origion);
accButter[2].input[2] =Int_Sort(Z_Origion);
/* Butterworth滤波 */
for (axis = 0; axis < 3; axis++)
{
accButter[axis].output[2] =
Int_Sort(b_acc[0] * accButter[axis].input[2]
+ b_acc[1] * accButter[axis].input[1]
+ b_acc[2] * accButter[axis].input[0]
- a_acc[1] * accButter[axis].output[1]
- a_acc[2] * accButter[axis].output[0]);
imu.accelFilter[axis] = accButter[axis].output[2];
}
for (axis = 0; axis < 3; axis++)
{
/* x(n) 序列保存 */
accButter[axis].input[0] = accButter[axis].input[1];
accButter[axis].input[1] = accButter[axis].input[2];
/* y(n) 序列保存 */
accButter[axis].output[0] = accButter[axis].output[1];
accButter[axis].output[1] = accButter[axis].output[2];
}
X_g_av=imu.accelFilter[0];
Y_g_av=imu.accelFilter[1];
Z_g_av=imu.accelFilter[2];
}
typedef struct
{
float accelRaw[3]; // 加速度计原始数据
float gyroRaw[3]; // 陀螺仪原始数据
float accelFilter[3]; // 加速度计滤波后数据
float gyroFilter[3]; // 陀螺仪滤波后数据
}_IMU_Tag;
int16_t GetData(uint8_t REG_Address)
{
uint8_t Hd,Ld;
Hd=Single_ReadI2C(0xD0,REG_Address);
Ld=Single_ReadI2C(0xD0,REG_Address+1);
return (Hd<<8)+Ld;
}
取完数据后，回到IMU_Filter进入
Acce_Correct_Filter();butterworth滤波，对imu.accelRaw进行滤波赋值给Acce_Correct
void Acce_Correct_Filter()
{
Acce_Correct[0]=Int_Sort(LPButterworth(imu.accelRaw[0],
&Butter_Buffer_Correct[0],&Butter_1HZ_Parameter_Acce));
Acce_Correct[1]=Int_Sort(LPButterworth(imu.accelRaw[1]
,&Butter_Buffer_Correct[1],&Butter_1HZ_Parameter_Acce));
Acce_Correct[2]=Int_Sort(LPButterworth(imu.accelRaw[2]
,&Butter_Buffer_Correct[2],&Butter_1HZ_Parameter_Acce));
}对于Acce_Correct有定义 int16_t Acce_Correct[3]={0};//用于矫正加速度量，截至频率很低
对于int_Sort有定义 #define Int_Sort (int16_t)
对于LPButterworth，其参数有：
Butter_BufferData Butter_Buffer_Correct[3];
Butter_Parameter Butter_1HZ_Parameter_Acce={
//250hz---1hz
1, -1.822694925196, 0.837181651256,
0.003621681514929,0.007243363029857, 0.003621681514929
};
从名字上推应该是低通Butterworth滤波器定义
float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
{
/* 加速度计Butterworth滤波 */
/* 获取最新x(n) */
static int LPB_Cnt=0;
Buffer->Input_Butter[2]=curr_input;
if(LPB_Cnt>=100)
{
/* Butterworth滤波 */
Buffer->Output_Butter[2]=
Parameter->b[0] * Buffer->Input_Butter[2]
+Parameter->b[1] * Buffer->Input_Butter[1]
+Parameter->b[2] * Buffer->Input_Butter[0]
-Parameter->a[1] * Buffer->Output_Butter[1]
-Parameter->a[2] * Buffer->Output_Butter[0];
}
else
{
Buffer->Output_Butter[2]=Buffer->Input_Butter[2];
LPB_Cnt++;
}
/* x(n) 序列保存 */
Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
/* y(n) 序列保存 */
Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
return Buffer->Output_Butter[2];
}
对imu.accelRaw[i]进行滤波，之后赋给 Acce_Correct[i]
经校正后给I_Origion
X_Origion=K[0]*(imu.accelRaw[0])-B[0];//经过椭球校正后的三轴加速度量//初值均为0；
Y_Origion=K[1]*(imu.accelRaw[1])-B[1];
Z_Origion=K[2]*(imu.accelRaw[2])-B[2];{ floatK[3]={1.0,1.0,1.0};//标度误差
float B[3]={0,0,0};//零位误差}
坐标系转换，将加速度有地理系转到机体系
FilterBefore_NamelessQuad.Acceleration[_YAW]=
-Sin_Roll* X_Origion
+ Sin_Pitch *Cos_Roll *Y_Origion
+ Cos_Pitch * Cos_Roll *Z_Origion;
FilterBefore_NamelessQuad.Acceleration[_PITCH]=
Cos_Yaw* Cos_Roll * X_Origion
+(Sin_Pitch*Sin_Roll*Cos_Yaw-Cos_Pitch * Sin_Yaw) * Y_Origion
+(Sin_Pitch * Sin_Yaw+Cos_Pitch * Sin_Roll * Cos_Yaw) * Z_Origion;
FilterBefore_NamelessQuad.Acceleration[_ROLL]=
Sin_Yaw* Cos_Roll * X_Origion
+(Sin_Pitch * Sin_Roll * Sin_Yaw +Cos_Pitch * Cos_Yaw) * Y_Origion
+ (Cos_Pitch * Sin_Roll * Sin_Yaw - Sin_Pitch * Cos_Yaw) * Z_Origion; FilterBefore_NamelessQuad是一个SINS结构体
{
float Position[Axis_Num];//位置估计量
float Speed[Axis_Num];//速度估计量
float Acceleration[Axis_Num];//加速度估计量
float Pos_History[Axis_Num][Num];//历史惯导位置
float Vel_History[Axis_Num][Num];//历史惯导速度
float Acce_Bias[Axis_Num];//惯导加速度漂移量估计量
}SINS;继续回到IMU_Filter
FilterBefore_NamelessQuad.Acceleration[_YAW]*=AcceGravity/AcceMax;
FilterBefore_NamelessQuad.Acceleration[_YAW]-=AcceGravity;
FilterBefore_NamelessQuad.Acceleration[_YAW]*=100;//加速度cm/s^2
FilterBefore_NamelessQuad.Acceleration[_PITCH]*=AcceGravity/AcceMax;
FilterBefore_NamelessQuad.Acceleration[_PITCH]*=100;//加速度cm/s^2
FilterBefore_NamelessQuad.Acceleration[_ROLL]*=AcceGravity/AcceMax;
FilterBefore_NamelessQuad.Acceleration[_ROLL]*=100;//加速度cm/s^2////把加速度量化处理，也是乘以一个量程
接下来又有：（好像并没有什么卵用）
Acce_Control_Filter();//加速度滤波，用于惯导、加速度控制反馈量再次滤波，对imu.accelRaw[i]进行滤波，之后赋给 Acce_Correct[i]
又有：再次butterworth滤波，对纠正后的数据进行滤波，滤波后数据赋值给I_g_av
/* 加速度计Butterworth滤波 */
/* 获取最新x(n) */
accButter[0].input[2]=Int_Sort(X_Origion);
accButter[1].input[2]=Int_Sort(Y_Origion);
accButter[2].input[2]=Int_Sort(Z_Origion);
/* Butterworth滤波 */
for (axis = 0; axis < 3; axis++)
{
accButter[axis].output[2] =
Int_Sort(b_acc[0] *accButter[axis].input[2]
+ b_acc[1] *accButter[axis].input[1]
+ b_acc[2] *accButter[axis].input[0]
- a_acc[1] *accButter[axis].output[1]
- a_acc[2] *accButter[axis].output[0]);
imu.accelFilter[axis] =accButter[axis].output[2];
}
for (axis = 0; axis < 3; axis++)
{
/* x(n) 序列保存 */
accButter[axis].input[0] =accButter[axis].input[1];
accButter[axis].input[1] =accButter[axis].input[2];
/* y(n) 序列保存 */
accButter[axis].output[0] =accButter[axis].output[1];
accButter[axis].output[1] =accButter[axis].output[2];
}
X_g_av=imu.accelFilter[0];
Y_g_av=imu.accelFilter[1];
Z_g_av=imu.accelFilter[2];
}
struct _ButterWorth2d_Acc_Tag accButter[3] =
{
/* input[3] output[3] */
{0, 0, 0, 0, 0, 0}, // X-axis
{0, 0, 0, 0, 0, 0}, // Y-axis
{0, 0, 0, 0, 0, 0}, // Z-axis
};加速度的数据经过两次滤波，一次是1赫兹的，然后纠偏，转换到机体系，再滤波，这次滤波是15赫兹(程序注释是这样具体什么样再看看)
至此也就得到了加速度数据要取陀螺仪数据
回到GET_MPU_DATA，进入GET_GYRO_DATA();
void GET_GYRO_DATA(void)//角速度低通滤波后用于姿态解算
{
X_w = GetData(GYRO_XOUT_H)-X_w_off;
Y_w = GetData(GYRO_YOUT_H)-Y_w_off;
Z_w = GetData(GYRO_ZOUT_H)-Z_w_off;
X_w_av=GYRO_LPF(X_w,
&Gyro_BufferData[0],
&Gyro_Parameter
);
Y_w_av=GYRO_LPF(Y_w,
&Gyro_BufferData[1],
&Gyro_Parameter
);
Z_w_av=GYRO_LPF(Z_w,
&Gyro_BufferData[2],
&Gyro_Parameter
);
}
其中：float X_w_off =0,Y_w_off =0,Z_w_off =0;
和加速度处理类似，将原始数据赋值给一个16位无符号整形的数，减去偏差，然后滤波
对于GYRO_LPF：
float GYRO_LPF(float curr_inputer,
Butter_BufferData *Buffer,
Butter_Parameter *Parameter)
{
/* 加速度计Butterworth滤波 */
/* 获取最新x(n) */
Buffer->Input_Butter[2]=curr_inputer;
/* Butterworth滤波 */
Buffer->Output_Butter[2]=
Parameter->b[0] * Buffer->Input_Butter[2]
+Parameter->b[1] * Buffer->Input_Butter[1]
+Parameter->b[2] * Buffer->Input_Butter[0]
-Parameter->a[1] * Buffer->Output_Butter[1]
-Parameter->a[2] * Buffer->Output_Butter[0];
/* x(n) 序列保存 */
Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
/* y(n) 序列保存 */
Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
return (Buffer->Output_Butter[2]);
}1, -1.475480443593, 0.5869195080612,
0.02785976611714, 0.05571953223427, 0.02785976611714,
};
Butter_BufferData Gyro_BufferData[3];
就此得到加速度计和陀螺仪数据，也就得到了mpu所有数据
下面回到void TIM4_IRQHandler(void)
根据飞控介绍，飞控使用气压计spl06_01，磁力计IST8310
则应该进入
SPL06_001_StateMachine();
Get_Mag_IST8310();
磁力计和气压计
#elsedef IMU_BOARD_NC686
SPL06_001_StateMachine();
Get_Mag_IST8310();具体定义
先进入SPL06_001_StateMachine，表面上无输入无输出，但是
uint8 SPL06_001_Offset_Okay=0;
uint8 SPL06_001_Finished=0;
float SPL06_001_Filter_P,SPL06_001_Filter_high;
unsigned int SPL06_001_Cnt=0;
void SPL06_001_StateMachine(void)
{
user_spl0601_get();
if(SPL06_001_Cnt<=50) SPL06_001_Cnt++;
if(SPL06_001_Cnt==49)
{
SPL06_001_Offset_Okay=1;
High_Okay_flag=1;
SPL06_001_Offset=pressure;
}
if(SPL06_001_Offset_Okay==1)//初始气压零点设置完毕
{
SPL06_001_Filter_P=pressure;
SPL06_001_Filter_high=Get_Altitude_SPL06_001(SPL06_001_Filter_P);
}但是它有调用了user_spl0601_get，顾名思义就是得到spl0601的数据，具体什么数据，接着看。
float temperature;
float pressure;
float user_spl0601_get()
{
static uint16_t spl06_cnt=0;
spl06_cnt++;
if(spl06_cnt==1)
{
spl0601_get_raw_temp();
temperature = spl0601_get_temperature();
}
else
{
spl0601_get_raw_pressure();
pressure = spl0601_get_pressure();
spl06_cnt=0;
}
return 0;
}static uint16_t spl06_cnt=0;
spl06_cnt++;
这两句把spl06_cnt变成1，于是if(spl06_cnt==1)就成立，每次判断都成立，
判断成立就有：
spl0601_get_raw_temp();
temperature = spl0601_get_temperature();
又调用函数spl0601_get_raw_temp
/*****************************************************************************
函 数 名 : spl0601_get_raw_temp
功能描述 : 获取温度的原始值，并转换成32Bits整数
输入参数 : void
输出参数 : 无
返 回 值 :
调用函数 :
被调函数 :
修改历史 :
1.日 期 : 2015年11月30日
作 者 : WL
修改内容 : 新生成函数
*****************************************************************************/
void spl0601_get_raw_temp(void)
{
uint8 h[3] = {0};
h[0] = spl0601_read(HW_ADR, 0x03);
h[1] = spl0601_read(HW_ADR, 0x04);
h[2] = spl0601_read(HW_ADR, 0x05);
p_spl0601->i32rawTemperature = (int32)h[0]<<16 | (int32)h[1]<<8 | (int32)h[2];
p_spl0601->i32rawTemperature= (p_spl0601->i32rawTemperature&0x800000) ? (0xFF000000|p_spl0601->i32rawTemperature) : p_spl0601->i32rawTemperature;
}
/*****************************************************************************
函 数 名 : spl0601_get_temperature
功能描述 : 在获取原始值的基础上，返回浮点校准后的温度值
输入参数 : void
输出参数 : 无
返 回 值 :
调用函数 :
被调函数 :
修改历史 :
1.日 期 : 2015年11月30日
作 者 : WL
修改内容 : 新生成函数
*****************************************************************************/
float spl0601_get_temperature(void)
{
float fTCompensate;
float fTsc;
fTsc = p_spl0601->i32rawTemperature / (float)p_spl0601->i32kT;
fTCompensate = p_spl0601->calib_param.c0 * 0.5 + p_spl0601->calib_param.c1 * fTsc;
return fTCompensate;
}
总之就此得到了温度，这是气压计啊，为什么得到温度？不应该是压力吗？
接着看 user_spl0601_get结束，下面进入
if(SPL06_001_Cnt<=50) SPL06_001_Cnt++;
if(SPL06_001_Cnt==49)
{
SPL06_001_Offset_Okay=1;
High_Okay_flag=1;
SPL06_001_Offset=pressure;
}
if(SPL06_001_Offset_Okay==1)//初始气压零点设置完毕
{
SPL06_001_Filter_P=pressure;
SPL06_001_Filter_high=Get_Altitude_SPL06_001(SPL06_001_Filter_P);
}
}
先不管
看看磁力计
Get_Mag_IST8310得到磁力计数据
IST8310 Mag_IST8310;
void Get_Mag_IST8310(void)
{
static uint16_t IST8310_Sample_Cnt=0;
float MagTemp[3]={0};
IST8310_Sample_Cnt++;
if(IST8310_Sample_Cnt==1)
{
Single_WriteI2C(IST8310_SLAVE_ADDRESS,IST8310_REG_CNTRL1,0x01);//Single Measurement Mode
}
else if(IST8310_Sample_Cnt==3)//至少间隔6ms,此处为8ms
{
Mag_IST8310.Buf[0]=Single_ReadI2C(IST8310_SLAVE_ADDRESS,0x03);//OUT_X_L_A
Mag_IST8310.Buf[1]=Single_ReadI2C(IST8310_SLAVE_ADDRESS,0x04);//OUT_X_H_A
Mag_IST8310.Buf[2]=Single_ReadI2C(IST8310_SLAVE_ADDRESS,0x05);//OUT_Y_L_A
Mag_IST8310.Buf[3]=Single_ReadI2C(IST8310_SLAVE_ADDRESS,0x06);//OUT_Y_H_A
Mag_IST8310.Buf[4]=Single_ReadI2C(IST8310_SLAVE_ADDRESS,0x07);//OUT_Z_L_A
Mag_IST8310.Buf[5]=Single_ReadI2C(IST8310_SLAVE_ADDRESS,0x08);//OUT_Z_H_A
/*****************合成三轴磁力计数据******************/
Mag_IST8310.Mag_Data[0]=(Mag_IST8310.Buf[1]<<8)|Mag_IST8310.Buf[0];
Mag_IST8310.Mag_Data[1]=(Mag_IST8310.Buf[3]<<8)|Mag_IST8310.Buf[2];
Mag_IST8310.Mag_Data[2]=(Mag_IST8310.Buf[5]<<8)|Mag_IST8310.Buf[4];
IST8310_Sample_Cnt=0;
}
#ifdef MAG_REVERSE_SIDE//重新映射磁力计三轴数据
Mag_IST8310.x = Mag_IST8310.Mag_Data[0];
Mag_IST8310.y = -Mag_IST8310.Mag_Data[1];
Mag_IST8310.z = Mag_IST8310.Mag_Data[2];
#else
Mag_IST8310.x = Mag_IST8310.Mag_Data[0];
Mag_IST8310.y = Mag_IST8310.Mag_Data[1];
Mag_IST8310.z = Mag_IST8310.Mag_Data[2];
#endif
DataMag.x=Mag_IST8310.x;
DataMag.y=Mag_IST8310.y;
DataMag.z=Mag_IST8310.z;
MagTemp[0]=Mag_IST8310.x-Mag_Offset[0];
MagTemp[1]=Mag_IST8310.y-Mag_Offset[1];
MagTemp[2]=Mag_IST8310.z-Mag_Offset[2];
/************磁力计倾角补偿*****************/
Mag_IST8310.thx = MagTemp[0] * Cos_Roll+ MagTemp[2] * Sin_Roll;
Mag_IST8310.thy = MagTemp[0] * Sin_Pitch*Sin_Roll
+MagTemp[1] * Cos_Pitch
-MagTemp[2] * Cos_Roll*Sin_Pitch;
/***********反正切得到磁力计观测角度*********/
Mag_IST8310.Angle_Mag=atan2(Mag_IST8310.thx,Mag_IST8310.thy)*57.296;
}
693
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
