姿态解算imu

看了几天，公式都看明白了，但回到代码就难受了，看不懂。

此函数在文件 Ano_FlightDataCal.c 被调用，
入口参数	采样周期/imu状态结构体指针(实际传入的是&imu_state)
三向陀螺仪/三向加速度计/三向磁力计/imu结构体句柄指针(实际传入的是&imu_data)
void IMU_update(float dT,_imu_state_st *state,float gyr[VEC_XYZ], float acc[VEC_XYZ],s16 mag_val[VEC_XYZ],_imu_st *imu)

这个是输入参数进行姿态解算

上面的程序就不分析了，大部分没看懂；下面的是输出。
void calculate_RPY()
{
	///输出姿态角///
		t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);
		//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;
		if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算
		{
			imu_data.pit =  fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;
			imu_data.rol =  fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f; 
			imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f; 
		}输出欧拉角 rol pit yaw 知道输入输出就行了 

输出的欧拉角在角度控制文件调用：	/*赋值反馈角度值*/att_2l_ct.fb_rol = (imu_data.rol ) ; 在openmv里也有使用，暂时不管。
		
}

具体的公式

我最终的目的是要实现程序控制飞行，这需要遥控器通道修改，以达到触发程控flag，之后修改程序飞行flag触发后，要实现的角度或者高度参数。具体就这两步。

一.遥控器通道设置：在DT文件
可以修改上位机显示数据，用来显示是否修改正确。
飞行模式在FlightCtrl文件里修改void Swtich_State_Task(u8 dT_ms)

4.19 断更了好久好久，因为基本的程序已经看的差不多了。接下来就是完成一键起飞和平移，然后加视觉模块，测距模块等等。
一键起飞已经可以了，目前还在写平移，剩下的应该很快。其实很简单，但是之前一直不得要领，我发愁了一个星期，没有思路和例程的感觉巨难受，我问东问西才搞懂，之后我弄好会把思路和程序都放上去，让大家节省时间。

5.19 赶论文，有时间了把平移函数放上去。说来惭愧其实很简单 我花了好长好长时间才实现。就这样把。