#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]）;


	//這是把四元數(shù)換算成《方向余弦矩陣》中的第三列的三個元素。
	//根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，正好是這三個元素。
	//所以這里的vx\y\z，其實就是當(dāng)前的歐拉角（即四元數(shù)）的機(jī)體坐標(biāo)參照系上，換算出來的重力單位向量。???????
	//=============================================================================
	//?計算加速度向量的模
	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）
	{
		//把加計的三維向量轉(zhuǎn)成單位向量。
		ax?=?ax?/?norm_acc_lpf;//4096.0f;
		ay?=?ay?/?norm_acc_lpf;//4096.0f;
		az?=?az?/?norm_acc_lpf;//4096.0f;?

		if?（7600?		{
			/*?叉乘得到誤差?*/
			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