/*
***********************************************************************
*????????????????YanJunFly?V1.0?-?Copyright?（c）?2017
*?All?rights?reserved.More?information?please?browse?www.yanjuntech.cn
*??????????????????燕駿智控——以極客技術推進工程教育
*?????????????手把手教你，如何從頭開始做一個電子設計類項目。
*?我們將以?四軸飛行器?為項目載體，帶領大家進行項目式學習做出屬于自己的四旋翼。
*?
*?其他聲明：
*						對代碼中產(chǎn)生的bug，本人不承擔任何直接或間接的損害賠償
*						若使用本代碼即視為默認本條款。若發(fā)現(xiàn)bug，請將詳細信息
*						發(fā)送下面鏈接至評論區(qū)，謝謝！
*						http://www.yanjuntech.cn/forum.php?gid=87
*
*?文件名稱：control.ccontrol.h
*?文件摘要：無
*
*?注意事項：
*						1、有的32型號沒有?定時器5678?如STM32F103RBT6?使用stm32cube查看
*						2、外設時鐘初始化的時候，鼠標右鍵?進底層看看時鐘函書?上面的注釋，有沒有對應好
*							如?RCC_APB2PeriphClockCmd（RCC_APB2Periph_TIM1?ENABLE）;
*						3、各種中斷?如果不知道，是哪個中斷名字，可以復制一下，ctrl?+?f，然后使用find?in?files
*							在文件中查找，能分方便的查出來，如已知：TIM3_IRQHandler，想查定時器1的中斷服務函數(shù)
*							ctrl+f快捷鍵，彈出查找?find?in?files?，然后點擊查找結果?即可找到?TIM1_UP_IRQHandler
*						
*?當前版本：v1.0
*?當前作者：YJ_朱子文
*?完成日期：2017-3-16?17:45:05
*?改動說明：創(chuàng)建文件
*
*?取代版本：無
*?原?作?者：YJ_朱子文
*?完成日期：2017-3-16?17:45:05
***********************************************************************
*/

#include?“./Control/control.h“

/**************************/

/*?聲明實時姿態(tài)數(shù)據(jù)?*/
CONTROL_AttitudeDataTypedef?x_AttitudeNow?=?
{
	{0.0f0.0f0.0f}
	{0.0f0.0f0.0f}
	{0.0f0.0f0.0f}
	{0.0f0.0f0.0f}
};

/*
***********************************************************************
*函數(shù)名稱：void?TIM3_IRQHandler（void）??
*函數(shù)功能：定時器3中斷服務函數(shù)
*
*使用說明：硬件自動調(diào)用
*入口參數(shù)：無
*返?回?值：無
*
*函數(shù)作者：YJ_朱子文
*創(chuàng)建日期：2017-3-17?09:58:14
***********************************************************************
*/

extern?float?f_AccInertiaAfterFilter;

void?TIM3_IRQHandler（void）???										
{
	/*?SENSOR_SensorTypedef該數(shù)據(jù)類型在sensor.h文件中?*/
	static?SENSOR_SensorTypedef?x_SensorData?={0};
	/*?大地坐標系（慣性坐標系）下的加速度值?這個驅動工程中沒有使用到
		?實際上在IMU.C姿態(tài)解算?四元數(shù)轉歐拉角算法中?計算的數(shù)據(jù)。
		?在這里定義了這個結構體變量，是因為總工程中的sensor中用到了
	*/
	static?CONTROL_3FloatDataTypedef?x_AccInertia?=?{0.0f};
	/*?用于分頻的系數(shù)?*/
	static?uint32_t?ui_Timer30Ms?=?0;
	static?uint32_t?ui_Timer2Ms?=?0;
	
	/*?判斷是否更新中斷?UIF?位是否置位?*/
	if?（TIM3->SR?&?（uint32_t）（1））?											
	{
		/*?每1ms調(diào)用一次數(shù)據(jù)讀取函數(shù)?*/
		vSensorDataRead（&x_SensorData（SENSOR_3FloatDataTypedef?*）&x_AccInertia）;
		
		if（++ui_Timer2Ms?>?2）
		{
			ui_Timer2Ms?=?0;
			/*?調(diào)用姿態(tài)解算函數(shù)?*/
			vImuUpdate（（IMU_AttitudeTypedef?*）&x_AttitudeNow
								?（IMU_SensorTypedef?*）&x_SensorData
								?（IMU_3FloatTypedef?*）&x_AccInertia）;
			
			/*?由于這里IMU是反著放的?放在了飛機背面?所以要經(jīng)過計算處理才能得到真正的角度數(shù)據(jù)?*/
			
			x_AttitudeNow.x_OriginalAngle.f_X?=?x_AttitudeNow.x_OriginalAngle.f_X;
			if（x_AttitudeNow.x_OriginalAngle.f_Y?>?0.0f）
			{
				x_AttitudeNow.x_OriginalAngle.f_Y?=?（x_AttitudeNow.x_OriginalAngle.f_Y?-?180.0f）;
			}else?
			{
				x_AttitudeNow.x_OriginalAngle.f_Y?=?（x_AttitudeNow.x_OriginalAngle.f_Y?+?180.0f）;
			}
			
			/*?減去地面起飛偏置的角度值?得到現(xiàn)在正確角度?*/
			x_AttitudeNow.x_AngleNow.f_X?=?x_AttitudeNow.x_OriginalAngle.f_X?-?x_SensorCaliInitData.x_AngleInitData.f_X;
			x