131 lines
5.3 KiB
C
131 lines
5.3 KiB
C
#include "stm32f4xx.h" // Device header
|
||
#include "stm32f4xx_conf.h"
|
||
#include "IST8310.h"
|
||
#include "BMI088.h"
|
||
#include "ahrs_lib.h"
|
||
#include "WaveFiltering_Kalman_Filtering.h"
|
||
|
||
KFP kfp_Pitch={0.02,0,0,0,0.01,0.543};//陀螺仪Pitch轴卡尔曼滤波器结构体
|
||
KFP kfp_Yaw={0.02,0,0,0,0.01,0.543};//陀螺仪yaw轴卡尔曼滤波器结构体
|
||
|
||
//KFP kfp_Pitch={0.02,0,0,0,0.01,0.96};//陀螺仪Pitch轴卡尔曼滤波器结构体
|
||
//KFP kfp_Yaw={0.02,0,0,0,0.01,0.96};//陀螺仪yaw轴卡尔曼滤波器结构体
|
||
|
||
float AttitudeAlgorithms_q[4];//姿态解算四元数
|
||
float AttitudeAlgorithms_RadYaw,AttitudeAlgorithms_RadPitch,AttitudeAlgorithms_RadRoll;//弧度制角度
|
||
float AttitudeAlgorithms_DegYaw,AttitudeAlgorithms_DegPitch,AttitudeAlgorithms_DegRoll;//角度制角度
|
||
float BMI088_GyroZeroOffset[3];//陀螺仪初始零偏
|
||
float BMI088_GyroWithoutOffset[3];//消除零偏后的陀螺仪数据
|
||
float fake_IST8310[3]={0,0,0};
|
||
|
||
/*
|
||
*函数简介:BMI088清除零偏
|
||
*参数说明:无
|
||
*返回类型:无
|
||
*备注:采集前100次数据取平均值消除零偏
|
||
*/
|
||
void ZeroOffset_Calibration(void)
|
||
{
|
||
|
||
uint16_t BMI088_CountFlag;
|
||
for (BMI088_CountFlag=0; BMI088_CountFlag < 1000; BMI088_CountFlag++)
|
||
{
|
||
BMI088_GyroZeroOffset[0]=BMI088_GyroZeroOffset[0]+BMI088_Gyro[0];
|
||
}
|
||
BMI088_GyroZeroOffset[0]=BMI088_GyroZeroOffset[0]/1000;
|
||
|
||
for (BMI088_CountFlag=0; BMI088_CountFlag < 1000; BMI088_CountFlag++)
|
||
{
|
||
BMI088_GyroZeroOffset[1]=BMI088_GyroZeroOffset[1]+BMI088_Gyro[1];
|
||
}
|
||
BMI088_GyroZeroOffset[1]=BMI088_GyroZeroOffset[1]/1000;
|
||
|
||
|
||
}
|
||
/*
|
||
*函数简介:姿态解算初始化
|
||
*参数说明:无
|
||
*返回类型:无
|
||
*备注:定时器定时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);//清除标志位
|
||
|
||
BMI088_GyroWithoutOffset[0]=BMI088_Gyro[0]+BMI088_GyroZeroOffset[0];
|
||
BMI088_GyroWithoutOffset[1]=BMI088_Gyro[1]+BMI088_GyroZeroOffset[1];
|
||
BMI088_GyroWithoutOffset[2]=BMI088_Gyro[2];
|
||
//debug
|
||
//AHRS_update(AttitudeAlgorithms_q,0.001f,BMI088_Gyro,BMI088_Accel,IST8310_MagneticField);
|
||
|
||
AHRS_update(AttitudeAlgorithms_q,0.001f,BMI088_GyroWithoutOffset,BMI088_Accel,fake_IST8310);
|
||
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;//转换为角度制
|
||
|
||
AttitudeAlgorithms_DegPitch=kalmanFilter(&kfp_Pitch,AttitudeAlgorithms_DegPitch);
|
||
AttitudeAlgorithms_DegRoll=kalmanFilter(&kfp_Yaw,AttitudeAlgorithms_DegRoll);
|
||
|
||
|
||
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;
|
||
}
|
||
}
|