同步步兵代码，暂时关闭底盘跟随

云台/云台-old/Function/AttitudeAlgorithms.c

@@ -0,0 +1,85 @@
+#include "stm32f4xx.h"                  // Device header
+#include "stm32f4xx_conf.h"
+#include "IST8310.h"
+#include "BMI088.h"
+#include "ahrs_lib.h"
+
+float AttitudeAlgorithms_q[4];//姿态解算四元数
+float AttitudeAlgorithms_RadYaw,AttitudeAlgorithms_RadPitch,AttitudeAlgorithms_RadRoll;//弧度制角度
+float AttitudeAlgorithms_DegYaw,AttitudeAlgorithms_DegPitch,AttitudeAlgorithms_DegRoll;//角度制角度
+
+
+/*
+ *函数简介:姿态解算初始化
+ *参数说明:无
+ *返回类型:无
+ *备注:定时器定时1ms,更新四元数并解算角度
+ */
+void AttitudeAlgorithms_Init(void)
+{
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11,ENABLE);//开启时钟
+	
+	TIM_InternalClockConfig(TIM11);//选择时基单元的时钟
+	
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;//配置时基单元
+	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;//配置时钟分频为1分频
+	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;//配置计数器模式为向上计数
+	TIM_TimeBaseInitStructure.TIM_Period=500-1;//配置自动重装值ARR
+	TIM_TimeBaseInitStructure.TIM_Prescaler=336-1;//配置分频值PSC,默认频率1000Hz
+	TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0;//配置重复计数单元的置为0
+	TIM_TimeBaseInit(TIM11,&TIM_TimeBaseInitStructure);//初始化TIM2
+	
+	TIM_ClearFlag(TIM11,TIM_FLAG_Update);//清除配置时基单元产生的中断标志位
+	
+	TIM_ITConfig(TIM11,TIM_IT_Update,ENABLE);//使能更新中断
+	
+	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//选择NVIC分组
+	
+	NVIC_InitTypeDef NVIC_InitStructure;//配置NVIC（配置参数）
+	NVIC_InitStructure.NVIC_IRQChannel=TIM1_TRG_COM_TIM11_IRQn;//选择中断通道为TIM11
+	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;//使能中断通道
+	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;//TIM2的抢占优先级
+	NVIC_InitStructure.NVIC_IRQChannelSubPriority=1;//TIM2的响应优先级
+	NVIC_Init(&NVIC_InitStructure);//初始化NVIC
+	
+	
+	IST8310_Init();//初始化IST8310
+	BMI088_Init();//初始化BMI088
+	AHRS_init(AttitudeAlgorithms_q,BMI088_Accel,IST8310_MagneticField);//AHRS初始化
+
+	TIM_Cmd(TIM11,ENABLE);//启动定时器
+}
+
+/*
+ *函数简介:TIM11定时器更新中断函数
+ *参数说明:无
+ *返回类型:无
+ *备注:定时1ms更新四元数并且解算角度
+ */
+void TIM1_TRG_COM_TIM11_IRQHandler(void)
+{
+	static float AttitudeAlgorithms_LastDegYaw,AttitudeAlgorithms_ThisDegYaw;//上一次角度制偏航角,本次角度制偏航角
+	static int64_t AttitudeAlgorithms_YawR=0;//角度制偏航角圈数
+	static uint8_t AttitudeAlgorithms_YawFirstFlag=1;//第一次接收数据标志位
+	
+	if(TIM_GetITStatus(TIM11,TIM_IT_Update)==SET)//检测TIM2更新
+	{
+		TIM_ClearITPendingBit(TIM11,TIM_IT_Update);//清除标志位
+		
+		AHRS_update(AttitudeAlgorithms_q,0.001f,BMI088_Gyro,BMI088_Accel,IST8310_MagneticField);
+		get_angle(AttitudeAlgorithms_q,&AttitudeAlgorithms_RadYaw,&AttitudeAlgorithms_RadPitch,&AttitudeAlgorithms_RadRoll);
+		
+		AttitudeAlgorithms_LastDegYaw=AttitudeAlgorithms_ThisDegYaw;
+		AttitudeAlgorithms_ThisDegYaw=AttitudeAlgorithms_RadYaw*57.295779513082320876798154814105f;//转换为角度制
+		AttitudeAlgorithms_DegPitch=AttitudeAlgorithms_RadPitch*57.295779513082320876798154814105f;//转换为角度制
+		AttitudeAlgorithms_DegRoll=AttitudeAlgorithms_RadRoll*57.295779513082320876798154814105f;//转换为角度制
+		
+		if(AttitudeAlgorithms_YawFirstFlag==0)//获取带圈数的角度制偏航角
+		{
+			if(AttitudeAlgorithms_ThisDegYaw-AttitudeAlgorithms_LastDegYaw>180.0f)AttitudeAlgorithms_YawR--;
+			else if(AttitudeAlgorithms_LastDegYaw-AttitudeAlgorithms_ThisDegYaw>180.0f)AttitudeAlgorithms_YawR++;
+		}
+		else AttitudeAlgorithms_YawFirstFlag=0;
+		AttitudeAlgorithms_DegYaw=AttitudeAlgorithms_YawR*360.0f+AttitudeAlgorithms_ThisDegYaw;
+	}
+}