IMU姿态解算

整合了9轴传感器：3轴加速度、3轴陀螺、3轴磁力计数据来解算姿态，参照了一些开源代码，这是我国奖代码，请放心使用

#include “imu.h“
#include “ctrl.h“
#include “magnet.h“
#include “filter.h“
#include “rc.h“
#include “height_ctrl.h“
#include “arm_math.h“
IMU_DCM imu_dcm;

void IMU_DCM::IMUupdate（float half_T float gx float gy float gz float ax float ay float az float *rol float *pit float *yaw）
{
	float ref_err_lpf_hz;
	static float yaw_correct;

	//filter.simple_3d_trans（&reference_v &mag_s.Mag_Val_af &mag_sim_3d）;
	//mag_norm = my_sqrt（mag_sim_3d.x * mag_sim_3d.x + mag_sim_3d.y *mag_sim_3d.y）;
        mag_norm=my_sqrt（mag_s.Mag_outcome.x*mag_s.Mag_outcome.x+mag_s.Mag_outcome.y*mag_s.Mag_outcome.y）;
        //yaw_mag_180=fast_atan2（（mag_s.Mag_outcome.y/mag_norm）（mag_s.Mag_outcome.x/mag_norm））*57.3f;//+180.0f;
        yaw_mag=fast_atan2（（mag_s.Mag_outcome.y/mag_norm）（mag_s.Mag_outcome.x/mag_norm））*57.3f;//+180.0f;
        if（yaw_mag<0）   yaw_mag+=360.0f;
        //yaw_mag=yaw_mag_caculate（（mag_s.Mag_outcome.y/mag_norm）（mag_s.Mag_outcome.x/mag_norm））;

//	if （mag_sim_3d.x != 0 && mag_sim_3d.y != 0 && mag_sim_3d.z != 0 && mag_norm != 0）
//	{
//          yaw_mag = fast_atan2（（mag_sim_3d.y / mag_norm） （mag_sim_3d.x / mag_norm）） *57.3f;
//          //非线性矫正
//          //   yaw_mag=  fit_mag_yaw（yaw_mag）;
//	}
	//=============================================================================
	// 计算等效重力向量
	reference_v.x = 2 * （ref_q[1] * ref_q[3] - ref_q[0] * ref_q[2]）;
	reference_v.y = 2 * （ref_q[0] * ref_q[1] + ref_q[2] * ref_q[3]）;
	reference_v.z = 1 - 2 * （ref_q[1] * ref_q[1] + ref_q[2] * ref_q[2]）;//ref_q[0]*ref_q[0] - ref_q[1]*ref_q[1] - ref_q[2]*ref_q[2] + ref_q[3]*ref_q[3];
        
        reference_y.x = 2*（ref_q[1] * ref_q[2] + ref_q[0] * ref_q[3]）;
        reference_y.y = 1 - 2 * （ref_q[1] * ref_q[1] + ref_q[3] * ref_q[3]）;
        reference_y.z = 2*（ref_q[2] * ref_q[3] - ref_q[0] * ref_q[1]）;
        
        reference_x.x=1 - 2 * （ref_q[2] * ref_q[2] + ref_q[3] * ref_q[3]）;
        reference_x.y=2*（ref_q[1] * ref_q[2] - ref_q[0] * ref_q[3]）;
        reference_x.z=2*（ref_q[1] * ref_q[3] + ref_q[0] * ref_q[2]）;


	//这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
	//根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
	//所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。       
	//=============================================================================
	// 计算加速度向量的模
	norm_acc = my_sqrt（ax*ax + ay*ay + az*az）;
	norm_acc_lpf += NORM_ACC_LPF_HZ *（6.28f *half_T） *（norm_acc - norm_acc_lpf）;  //10hz *3.14 * 2*0.001


	if （ABS（ax）<8800 && ABS（ay）<8800 && ABS（az）<8800）
	{
		//把加计的三维向量转成单位向量。
		ax = ax / norm_acc_lpf;//4096.0f;
		ay = ay / norm_acc_lpf;//4096.0f;
		az = az / norm_acc_lpf;//4096.0f; 

		if （7600 < norm_acc && norm_acc < 8800）
		{
			/* 叉乘得到误差 */
			ref.err_tmp.x = ay*reference_v.z - az*reference_v.y;
			ref.err_tmp.y = az*reference_v.x - ax*reference_v.z;
			ref.err_tmp.z = ax*reference_v.y - ay*reference_v.x;

			/* 误差低通 */
			ref_err_lpf_hz = REF_ERR_LPF_HZ *（6.28f *half_T）;
			ref.err_lpf.x

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       7406  2016-12-22 10:24  imu.cpp

     文件       1383  2016-12-21 16:57  imu.h

----------- ---------  ---------- -----  ----

                 8789                    2