384 lines
13 KiB
C
384 lines
13 KiB
C
/**
|
||
******************************************************************************
|
||
* @file ins_task.c
|
||
* @author Wang Hongxi
|
||
* @author annotation and modificaiton by neozng
|
||
* @version V2.0.0
|
||
* @date 2022/2/23
|
||
* @brief
|
||
******************************************************************************
|
||
* @attention
|
||
*
|
||
******************************************************************************
|
||
*/
|
||
#include "ins_task.h"
|
||
#include "controller.h"
|
||
#include "QuaternionEKF.h"
|
||
#include "spi.h"
|
||
#include "tim.h"
|
||
#include "user_lib.h"
|
||
#include "general_def.h"
|
||
#include "master_process.h"
|
||
|
||
#include "vofa.h"
|
||
|
||
static INS_t INS;
|
||
static IMU_Param_t IMU_Param;
|
||
static PIDInstance TempCtrl = {0};
|
||
|
||
//const float xb[3] = {1, 0, 0};
|
||
//const float yb[3] = {0, 1, 0};
|
||
//const float zb[3] = {0, 0, 1};
|
||
|
||
const float xb[3] = {0, 0, 1};
|
||
const float yb[3] = {0, 1, 0};
|
||
const float zb[3] = {-1, 0, 0};
|
||
|
||
// 用于获取两次采样之间的时间间隔
|
||
static uint32_t INS_DWT_Count = 0;
|
||
static float dt = 0, t = 0;
|
||
static float RefTemp = 40; // 恒温设定温度
|
||
|
||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
|
||
|
||
static void IMUPWMSet(uint16_t pwm)
|
||
{
|
||
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
|
||
}
|
||
|
||
/**
|
||
* @brief 温度控制
|
||
*
|
||
*/
|
||
static void IMU_Temperature_Ctrl(void)
|
||
{
|
||
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
|
||
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
|
||
}
|
||
|
||
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
|
||
static void InitQuaternion(float *init_q4)
|
||
{
|
||
float acc_init[3] = {0};
|
||
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
|
||
float axis_rot[3] = {0}; // 旋转轴
|
||
// 读取100次加速度计数据,取平均值作为初始值
|
||
for (uint8_t i = 0; i < 100; ++i)
|
||
{
|
||
BMI088_Read(&BMI088);
|
||
acc_init[X] += -BMI088.Accel[2];
|
||
acc_init[Y] += BMI088.Accel[Y];
|
||
acc_init[Z] += BMI088.Accel[0];
|
||
DWT_Delay(0.001);
|
||
}
|
||
for (uint8_t i = 0; i < 3; ++i)
|
||
acc_init[i] /= 100;
|
||
Norm3d(acc_init);
|
||
// 计算原始加速度矢量和导航系重力加速度矢量的夹角
|
||
float angle = acosf(Dot3d(acc_init, gravity_norm));
|
||
Cross3d(acc_init, gravity_norm, axis_rot);
|
||
Norm3d(axis_rot);
|
||
|
||
//q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
|
||
//其中a表示旋转角度，(x,y,z)表示旋转轴 计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
|
||
init_q4[0] = cosf(angle / 2.0f);
|
||
for (uint8_t i = 0; i < 2; ++i)
|
||
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
|
||
}
|
||
|
||
attitude_t *INS_Init(void)
|
||
{
|
||
if (!INS.init)
|
||
INS.init = 1;
|
||
else
|
||
return (attitude_t *)&INS.Gyro;
|
||
|
||
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
|
||
|
||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
|
||
;
|
||
IMU_Param.scale[X] = 1;
|
||
IMU_Param.scale[Y] = 1;
|
||
IMU_Param.scale[Z] = 1;
|
||
IMU_Param.Yaw = 0;
|
||
IMU_Param.Pitch = 0;
|
||
IMU_Param.Roll = 0;
|
||
IMU_Param.flag = 1;
|
||
|
||
float init_quaternion[4] = {0};
|
||
InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
|
||
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
|
||
// imu heat init
|
||
PID_Init_Config_s config = {.MaxOut = 2000,
|
||
.IntegralLimit = 300,
|
||
.DeadBand = 0,
|
||
.Kp = 1000,
|
||
.Ki = 20,
|
||
.Kd = 0,
|
||
.Improve = 0x01}; // enable integratiaon limit
|
||
PIDInit(&TempCtrl, &config);
|
||
|
||
// noise of accel is relatively big and of high freq,thus lpf is used
|
||
INS.AccelLPF = 0.0085;
|
||
DWT_GetDeltaT(&INS_DWT_Count);
|
||
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||
}
|
||
|
||
/* 注意以1kHz的频率运行此任务 */
|
||
void INS_Task(void)
|
||
{
|
||
static uint32_t count = 0;
|
||
const float gravity[3] = {0, 0, 9.81f};
|
||
|
||
dt = DWT_GetDeltaT(&INS_DWT_Count);
|
||
t += dt;
|
||
|
||
// ins update
|
||
if ((count % 1) == 0)
|
||
{
|
||
BMI088_Read(&BMI088);
|
||
|
||
INS.Accel[X] = -BMI088.Accel[2];
|
||
INS.Accel[Y] = BMI088.Accel[1];
|
||
INS.Accel[Z] = BMI088.Accel[0];
|
||
INS.Gyro[X] = -BMI088.Gyro[2];
|
||
INS.Gyro[Y] = BMI088.Gyro[1];
|
||
INS.Gyro[Z] = BMI088.Gyro[0];
|
||
|
||
// demo function,用于修正安装误差,可以不管,本demo暂时没用
|
||
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
|
||
|
||
// 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
|
||
// INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
|
||
// INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
|
||
|
||
// 核心函数,EKF更新四元数
|
||
IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
|
||
|
||
memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
|
||
|
||
// 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
|
||
BodyFrameToEarthFrame(xb, INS.xn, INS.q);
|
||
BodyFrameToEarthFrame(yb, INS.yn, INS.q);
|
||
BodyFrameToEarthFrame(zb, INS.zn, INS.q);
|
||
|
||
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
|
||
float gravity_b[3];
|
||
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
|
||
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
|
||
{
|
||
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
|
||
}
|
||
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
|
||
|
||
|
||
INS.Yaw = QEKF_INS.Yaw;
|
||
INS.Pitch = QEKF_INS.Pitch;
|
||
INS.Roll = QEKF_INS.Roll;
|
||
|
||
// // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
|
||
// if (INS.Yaw - INS.YawAngleLast > 180.0f)
|
||
// {
|
||
// INS.YawRoundCount--;
|
||
// }
|
||
// else if (INS.Yaw - INS.YawAngleLast < -180.0f)
|
||
// {
|
||
// INS.YawRoundCount++;
|
||
// }
|
||
// INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw;
|
||
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
|
||
|
||
// float vofa_send_data[9];
|
||
// vofa_send_data[0]=INS.Yaw;
|
||
// vofa_send_data[1]=INS.Pitch;
|
||
// vofa_send_data[2]=INS.Roll;
|
||
// vofa_send_data[3]=INS.Gyro[X];
|
||
// vofa_send_data[4]=INS.Gyro[Y];
|
||
// vofa_send_data[5]=INS.Gyro[Z];
|
||
// vofa_send_data[6]=BMI088.Accel[X];
|
||
// vofa_send_data[7]=BMI088.Accel[Y];
|
||
// vofa_send_data[8]=BMI088.Accel[Z];
|
||
// vofa_justfloat_output(vofa_send_data,36,&huart1);
|
||
|
||
VisionSetAltitude(INS.Yaw, INS.Pitch);
|
||
}
|
||
|
||
// temperature control
|
||
if ((count % 2) == 0)
|
||
{
|
||
// 500hz
|
||
IMU_Temperature_Ctrl();
|
||
}
|
||
|
||
if ((count++ % 1000) == 0)
|
||
{
|
||
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief Transform 3dvector from BodyFrame to EarthFrame
|
||
* @param[1] vector in BodyFrame
|
||
* @param[2] vector in EarthFrame
|
||
* @param[3] quaternion
|
||
*/
|
||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
|
||
{
|
||
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
|
||
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
|
||
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
|
||
|
||
vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
|
||
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
|
||
(q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
|
||
|
||
vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
|
||
(q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
|
||
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
|
||
}
|
||
|
||
/**
|
||
* @brief Transform 3dvector from EarthFrame to BodyFrame
|
||
* @param[1] vector in EarthFrame
|
||
* @param[2] vector in BodyFrame
|
||
* @param[3] quaternion
|
||
*/
|
||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
|
||
{
|
||
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
|
||
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
|
||
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
|
||
|
||
vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
|
||
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
|
||
(q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
|
||
|
||
vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
|
||
(q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
|
||
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
|
||
}
|
||
|
||
/**
|
||
* @brief reserved.用于修正IMU安装误差与标度因数误差,即陀螺仪轴和云台轴的安装偏移
|
||
*
|
||
*
|
||
* @param param IMU参数
|
||
* @param gyro 角速度
|
||
* @param accel 加速度
|
||
*/
|
||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
|
||
{
|
||
static float lastYawOffset, lastPitchOffset, lastRollOffset;
|
||
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
|
||
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
|
||
|
||
if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
|
||
fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
|
||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
|
||
{
|
||
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
|
||
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
|
||
cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
|
||
sinYaw = arm_sin_f32(param->Yaw / 57.295779513f);
|
||
sinPitch = arm_sin_f32(param->Pitch / 57.295779513f);
|
||
sinRoll = arm_sin_f32(param->Roll / 57.295779513f);
|
||
|
||
// 1.yaw(alpha) 2.pitch(beta) 3.roll(gamma)
|
||
c_11 = cosYaw * cosRoll + sinYaw * sinPitch * sinRoll;
|
||
c_12 = cosPitch * sinYaw;
|
||
c_13 = cosYaw * sinRoll - cosRoll * sinYaw * sinPitch;
|
||
c_21 = cosYaw * sinPitch * sinRoll - cosRoll * sinYaw;
|
||
c_22 = cosYaw * cosPitch;
|
||
c_23 = -sinYaw * sinRoll - cosYaw * cosRoll * sinPitch;
|
||
c_31 = -cosPitch * sinRoll;
|
||
c_32 = sinPitch;
|
||
c_33 = cosPitch * cosRoll;
|
||
param->flag = 0;
|
||
}
|
||
float gyro_temp[3];
|
||
for (uint8_t i = 0; i < 3; ++i)
|
||
gyro_temp[i] = gyro[i] * param->scale[i];
|
||
|
||
gyro[X] = c_11 * gyro_temp[X] +
|
||
c_12 * gyro_temp[Y] +
|
||
c_13 * gyro_temp[Z];
|
||
gyro[Y] = c_21 * gyro_temp[X] +
|
||
c_22 * gyro_temp[Y] +
|
||
c_23 * gyro_temp[Z];
|
||
gyro[Z] = c_31 * gyro_temp[X] +
|
||
c_32 * gyro_temp[Y] +
|
||
c_33 * gyro_temp[Z];
|
||
|
||
float accel_temp[3];
|
||
for (uint8_t i = 0; i < 3; ++i)
|
||
accel_temp[i] = accel[i];
|
||
|
||
accel[X] = c_11 * accel_temp[X] +
|
||
c_12 * accel_temp[Y] +
|
||
c_13 * accel_temp[Z];
|
||
accel[Y] = c_21 * accel_temp[X] +
|
||
c_22 * accel_temp[Y] +
|
||
c_23 * accel_temp[Z];
|
||
accel[Z] = c_31 * accel_temp[X] +
|
||
c_32 * accel_temp[Y] +
|
||
c_33 * accel_temp[Z];
|
||
|
||
lastYawOffset = param->Yaw;
|
||
lastPitchOffset = param->Pitch;
|
||
lastRollOffset = param->Roll;
|
||
}
|
||
|
||
//------------------------------------functions below are not used in this demo-------------------------------------------------
|
||
//----------------------------------you can read them for learning or programming-----------------------------------------------
|
||
//----------------------------------they could also be helpful for further design-----------------------------------------------
|
||
|
||
/**
|
||
* @brief Update quaternion
|
||
*/
|
||
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
|
||
{
|
||
float qa, qb, qc;
|
||
|
||
gx *= 0.5f * dt;
|
||
gy *= 0.5f * dt;
|
||
gz *= 0.5f * dt;
|
||
qa = q[0];
|
||
qb = q[1];
|
||
qc = q[2];
|
||
q[0] += (-qb * gx - qc * gy - q[3] * gz);
|
||
q[1] += (qa * gx + qc * gz - q[3] * gy);
|
||
q[2] += (qa * gy - qb * gz + q[3] * gx);
|
||
q[3] += (qa * gz + qb * gy - qc * gx);
|
||
}
|
||
|
||
/**
|
||
* @brief Convert quaternion to eular angle
|
||
*/
|
||
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
|
||
{
|
||
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
|
||
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
|
||
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
|
||
}
|
||
|
||
/**
|
||
* @brief Convert eular angle to quaternion
|
||
*/
|
||
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
|
||
{
|
||
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
|
||
Yaw /= 57.295779513f;
|
||
Pitch /= 57.295779513f;
|
||
Roll /= 57.295779513f;
|
||
cosPitch = arm_cos_f32(Pitch / 2);
|
||
cosYaw = arm_cos_f32(Yaw / 2);
|
||
cosRoll = arm_cos_f32(Roll / 2);
|
||
sinPitch = arm_sin_f32(Pitch / 2);
|
||
sinYaw = arm_sin_f32(Yaw / 2);
|
||
sinRoll = arm_sin_f32(Roll / 2);
|
||
q[0] = cosPitch * cosRoll * cosYaw + sinPitch * sinRoll * sinYaw;
|
||
q[1] = sinPitch * cosRoll * cosYaw - cosPitch * sinRoll * sinYaw;
|
||
q[2] = sinPitch * cosRoll * sinYaw + cosPitch * sinRoll * cosYaw;
|
||
q[3] = cosPitch * cosRoll * sinYaw - sinPitch * sinRoll * cosYaw;
|
||
}
|