SkyLiCoder
9313310a87
Replace compile-time orientation enum/macro approach with a general 3x3 rotation matrix IMU2BODY_ROTATION. User fills in the matrix according to actual IMU mounting orientation. Matrix must be a proper rotation (det = +1) to preserve right-handed coordinate system.
157 lines
3.4 KiB
C
157 lines
3.4 KiB
C
/**
|
|
******************************************************************************
|
|
* @file ins_task.h
|
|
* @author Wang Hongxi
|
|
* @author annotation and modification by NeoZeng
|
|
* @version V2.0.0
|
|
* @date 2022/2/23
|
|
* @brief
|
|
******************************************************************************
|
|
* @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
|
|
* 应用层调用初始化函数.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
#ifndef __INS_TASK_H
|
|
#define __INS_TASK_H
|
|
|
|
#include "stdint.h"
|
|
#include "BMI088driver.h"
|
|
#include "QuaternionEKF.h"
|
|
|
|
#define X 0
|
|
#define Y 1
|
|
#define Z 2
|
|
|
|
#define INS_TASK_PERIOD 1
|
|
|
|
/* IMU坐标系 -> 机体坐标系旋转矩阵（必须为右手系旋转，行列式为+1）
|
|
* 用户根据IMU实际安装方向填写，out = R * raw
|
|
* 常见安装方向示例（供参考）：
|
|
* 正常安装: {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1}}
|
|
* 绕Z转180°: {{-1, 0, 0}, { 0,-1, 0}, { 0, 0, 1}}
|
|
* 绕X转180°: {{ 1, 0, 0}, { 0,-1, 0}, { 0, 0,-1}}
|
|
* 绕Y转180°: {{-1, 0, 0}, { 0, 1, 0}, { 0, 0,-1}}
|
|
*/
|
|
#define IMU2BODY_ROTATION { \
|
|
{ 1, 0, 0}, \
|
|
{ 0, -1, 0}, \
|
|
{ 0, 0, -1} \
|
|
}
|
|
|
|
typedef struct
|
|
{
|
|
float q[4]; // 四元数估计值
|
|
|
|
float MotionAccel_b[3]; // 机体坐标加速度
|
|
float MotionAccel_n[3]; // 绝对系加速度
|
|
|
|
float AccelLPF; // 加速度低通滤波系数
|
|
|
|
// bodyframe在绝对系的向量表示
|
|
float xn[3];
|
|
float yn[3];
|
|
float zn[3];
|
|
|
|
// 加速度在机体系和XY两轴的夹角
|
|
// float atanxz;
|
|
// float atanyz;
|
|
|
|
// IMU量测值
|
|
float Gyro[3]; // 角速度
|
|
float Accel[3]; // 加速度
|
|
// 位姿
|
|
float Roll;
|
|
float Pitch;
|
|
float Yaw;
|
|
float YawTotalAngle;
|
|
|
|
uint8_t init;
|
|
} INS_t;
|
|
|
|
/* 用于修正安装误差的参数 */
|
|
typedef struct
|
|
{
|
|
uint8_t flag;
|
|
|
|
float scale[3];
|
|
|
|
float Yaw;
|
|
float Pitch;
|
|
float Roll;
|
|
} IMU_Param_t;
|
|
|
|
/**
|
|
* @brief 通过变换矩阵将IMU坐标系数据变换到机体坐标系
|
|
*
|
|
* out = IMU2BODY_ROTATION * raw
|
|
*
|
|
* @param raw IMU坐标系原始数据（3轴）
|
|
* @param out 机体坐标系数据（3轴）
|
|
*/
|
|
void IMU_Mount_Correct(const float raw[3], float out[3]);
|
|
|
|
/**
|
|
* @brief 初始化惯导解算系统
|
|
*
|
|
*/
|
|
INS_t *INS_Init(void);
|
|
|
|
/**
|
|
* @brief 此函数放入实时系统中,以1kHz频率运行
|
|
* p.s. osDelay(1);
|
|
*
|
|
*/
|
|
void INS_Task(void);
|
|
|
|
/**
|
|
* @brief 四元数更新函数,即实现dq/dt=0.5Ωq
|
|
*
|
|
* @param q 四元数
|
|
* @param gx
|
|
* @param gy
|
|
* @param gz
|
|
* @param dt 距离上次调用的时间间隔
|
|
*/
|
|
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
|
|
|
|
/**
|
|
* @brief 四元数转换成欧拉角 ZYX
|
|
*
|
|
* @param q
|
|
* @param Yaw
|
|
* @param Pitch
|
|
* @param Roll
|
|
*/
|
|
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
|
|
|
|
/**
|
|
* @brief ZYX欧拉角转换为四元数
|
|
*
|
|
* @param Yaw
|
|
* @param Pitch
|
|
* @param Roll
|
|
* @param q
|
|
*/
|
|
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
|
|
|
|
/**
|
|
* @brief 机体系到惯性系的变换函数
|
|
*
|
|
* @param vecBF body frame
|
|
* @param vecEF earth frame
|
|
* @param q
|
|
*/
|
|
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
|
|
|
|
/**
|
|
* @brief 惯性系转换到机体系
|
|
*
|
|
* @param vecEF
|
|
* @param vecBF
|
|
* @param q
|
|
*/
|
|
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
|
|
|
|
#endif
|