hpm6750/controller_yy_app/software/soft_inc/soft_imu.h

#ifndef __SOFT_IMU_H
#define __SOFT_IMU_H

#include "common.h"
#include "soft_gs.h"
#include "stdbool.h"
#include "vklink.h"
#include <stdint.h>

enum
{
    // 磁校准-无
    MAG_CAL_NO = 0,
    // 磁校准-XY平面
    MAG_CAL_XY = 1,
    // 磁校准-XZ平面
    MAG_CAL_XZ = 2,
    // 磁校准-校准成功
    MAG_CAL_OK = 3,
    // 磁校准-校准失败
    MAG_CAL_FAIL = 4,
};

enum
{
    // 上电未收到星采用AHRS算法
    I_AHRS = 0,
    // 上电搜到星后使用INS/GPS算法
    INSGPS = 1,
    // 发生丢星准备过渡到ahrs状态
    T_AHRS = 2,
    // INS/GPS算法中持续丢星10s后切换回AHRS不再回INS算法
    F_AHRS = 3,
};

enum
{
    IMU_MSG_ID = 0,         // IMU 包消息
    GPS_MSG_ID = 1,         // GPS 包消息
    CALIBRARION_MSG_ID = 2, // 校准消息
    SENSOR_MSG_ID = 3,      // 传感器消息
    VER_MSG_ID = 4,         // 版本消息
    IMU_POS_OFFSET_ID = 7,  // IMU 安装偏差
    GPS_POS_OFFSET_ID = 8,  // GPS 安装偏差
    REQ_MSG_ID = 20,        // 请求消息
    ACK_MSG_ID = 21,        // 应答消息
    HDW_MSG_ID = 0X20,      // 互顶位数据
};

enum
{
    MAG_CALIB = 1,                  // 磁校准
    ATTITUE_CALIB = 2,              // 水平姿态校准
    ASSEMBLE_CALIB = 3,             // 安装校准
    FILTER_PARAM_CALIB = 4,         // 滤波参数校准
    LOCK_UNLOCK_CALIB = 5,          // 上锁解锁位置校准
    DGPS_DIRECTION_CALIB = 6,       // 双差分天向安装方式校准
    IMU_POS_CALIB = 7,              // IMU 安装偏差
    GPS_POS_CALIB = 8,              // GPS 安装偏差
    RTK_POS_CALIB = 9,              // RTK 安装偏差
    BASE_STATION_STATUS_CALIB = 10, // 基站工作状态
    DISABLE_GPS_ALT = 11,           // 是否禁用GPS高度
};

enum
{
    GPS_VERSION_NORMAL = 1,
    GPS_VERSION_PRO = 2,
    GPS_VERSION_RTK = 3
};

// 状态标志位的错误状态位mask
#define GPS_STATUS1_ERROR_MASK 0XFB
#define GPS_STATUS2_ERROR_MASK 0X3C
#define ACCGYRO_STATUS_ERROR_MASK 0X3F
#define MAG_STATUS_ERROR_MASK 0XBB

typedef struct
{
    int version;
    // 滤波档位
    uint8_t filter_param;
    // 安装方向
    uint8_t assemble_direction_param;
    // 双差分天线安装方向
    uint8_t dgps_direction_param;
    // GPS 安装位置偏差 cm
    int8_t gps_pos_param[6];
    // IMU 安装位置偏差 cm
    int8_t imu_pos_param[3];
} IMU_Config;

// 采用一字节对齐的方式，便于直接memcpy，与解析
// 现在采用改变对齐方式的方式，牺牲了CPU的效率。后期与IMU端商定一致的结构体序列，sizeof()是30个字节
#pragma pack(1)
typedef struct
{
    float _zitai_yaw;   // 4--5    磁航向       单位：0.01° ，
                        // 范围：0---360， 北偏西为正
    float _zitai_roll;  // 6--7    横滚		 单位：0.01° ，
                        // 范围：-18000---18000，左负右正
    float _zitai_pitch; // 8--9    俯仰		 单位：0.01° ，
                        // 范围：-18000---18000，左负右正
    float _x_gyro;      // 10--11  X陀螺      单位：°/s
    float _y_gyro;      // 12--13  y陀螺	     单位：°/s
    float _z_gyro;      // 14--15  z陀螺	     单位：°/s
    float _x_vel;       // 16--17  相对左右速度	 单位：cm/s     左->右 为正 左<-右 为负
    float _y_vel;       // 18--19  相对前后速度	 单位：cm/s     后->前 为正 前<-后 为负
    float _z_vel;       // 20--21  垂直速度	 单位：cm/s     上正下负
    short _x_acc;       // 22--23  机体左右加速度	 单位：cm/s^2   左->右 为正 左<-右 为负
    short _y_acc;       // 24--25  机体前后加速度	 单位：cm/s^2   后->前 为正 前<-后 为负
    short _z_acc;       // 26--27  Z轴加速度计	 单位：cm/s^2   上正下负
    double _c_lng;      // 28--31  解算X偏差   cm    左负右正
    double _c_lat;      // 32--35  解算Y偏差   cm    前正后负
    float _baro_alt;    // 36--39  高度         单位：cm
} REV_IMU;
#pragma pack()

extern REV_IMU raw_imu;

#pragma pack(1)
typedef struct
{
    int _longitude;                 //  经度	   单位：1e-7
    int _latitude;                  //  纬度	   单位：1e-7
    short _gps_yaw;                 //  航向	  单位：0.1度， 范围：0--360， 北偏东为正
    int _gps_alt_mm;                //   GPS高度	 单位：mm
    short _gps_vel;                 //   GPS速度	 单位：dm/s
    unsigned char _gps_num;         //      GPS星数
    unsigned char _gps_pdop;        //      PDOP 单位：0.1
    short _gps_year;                //
    unsigned char _gps_month;       //
    unsigned char _gps_day;         //
    unsigned char _gps_hour;        //
    unsigned char _gps_minute;      //
    unsigned char _gps_second;      //
    unsigned char _gps_millisecond; // 毫秒/100

    //----GPS状态标志----
    char _gps_status;

    //----气压计状态标志----
    char _gps_status2;

    //----陀螺加计状态标识----
    char _acc_gyro_status;

    //----磁传感器状态标识----
    char _mag_status;

    //---- 振动系数----
    char _oscillating_coefficient; //  振动系数

    // 温度单位：度
    signed char _temprature;

    // 姿态标志位 0-AHRS  1-INSGPS  2-FAHRS
    char _insgps_ok;

    // 磁校准状态
    char _state_flag;

    // RTK状态标识 0-无RTK   1-RTK  2-GPS高度可用
    unsigned char _rtk_state;

    // imu安装方向 1-正向, 2-右向, 3-后向，4-左向 低4位
    // imu滤波参数 0-强滤波, 1-中滤波, 2-弱滤波  高4位
    char _imu_assembly_direction;

    // 垂直方向gps速度, mm/s
    short _gps_vetical_vel;

    // IMU 收取解锁落锁状态
    char _imu_get_throttle_flag;

    // 标定的重力加速度
    char _g0;

    // 双天线安装方向 0-前后 1-左右
    char _dgps_assembly_direction;

    // 冗余 gps 星数
    char _redundant_gps_num;

    // 双天线定向中 ant2 卫星数目
    char _dgps_ant2_gps_num;

    // 从 gps 经度
    int _auxiliary_gps_lon;

    // 从 gps 纬度
    int _auxiliary_gps_lat;

    // ins 姿态正确性
    // bit0  0-ins 姿态正确 1-ins 姿态异常
    char _ins_error;

    // 车载相对位置信息
    float _vehicle_vector_east;
    float _vehicle_vector_north;
    float _vehicle_vector_up;
    // 车载相对位置锁定标志 1-锁定 0-不锁定
    uint8_t _vehicle_vector_flag;
} REV_GPS;
#pragma pack()

extern REV_GPS raw_gps;

#pragma pack(1)
typedef struct
{
    short _xgyro1; // 0.02deg/s
    short _ygyro1;
    short _zgyro1;

    short _xgyro2;
    short _ygyro2;
    short _zgyro2;

    signed char _xacc1; // 0.1m/s^2
    signed char _yacc1;
    signed char _zacc1;

    signed char _xacc2;
    signed char _yacc2;
    signed char _zacc2;

    short _xmag1; // 0.1mGause
    short _ymag1;
    short _zmag1;

    short _xmag2;
    short _ymag2;
    short _zmag2;

    float _baroalt; // 1m
    char _filter_reset_flag;
    signed char _mag_yaw;
    char _ahrs_roll;
    char _ahrs_pitch;
    short _gps_espeed;
    short _gps_nspeed;
} REV_SENSOR;
#pragma pack()
extern REV_SENSOR raw_sensor;

struct INS_DATA
{

    IMU_Config imu_conf;
    int horz_vel; // 水平合成速度。单位cm/s

    char insgps_ok;      // ins/gps准备好了，可以起飞的标志位
    char mag_calib_flag; // 状态标志位，磁校准状态

    float Q[4];      // 当前姿态四元数表示
    float dcm[3][3]; // 当前姿态旋转矩阵表示

    int lng;        // 经度
    int lat;        // 纬度
    float gps_yaw;  // GPS航向。单位度
    int gps_alt;    // GPS高度。单位cm
    int gps_vel;    // GPS速度。单位cm/s
    short gps_num;  // GPS星数。单位 颗
    short gps_pdop; // GPS定位精度。单位
    int baro_alt;   // 气压高度。单位 cm

    short gps_year;                      // 年
    char gps_month;                      // 月
    char gps_day;                        // 日
    char gps_hour;                       // 时
    char gps_minute;                     // 分
    char gps_second;                     // 秒
    char gps_millisecond;                // 毫秒
    uint32_t last_gps_second_local_time; // 上一次 gps 秒变更的本地时间

    char rtk_state; // 差分状态 //  RTK状态标识 0-无RTK   1-RTK  2-GPS高度可用

    char oscillating_coefficient; // 振动系数

    signed char temprature; // 温度

    bool has_cal_delta_lonlat_to_cm_ratio; // 是否已经计算了经纬度转平面误差因子
    double delta_lon_to_cm_ratio;          // 经度差转平面距离因子
    double delta_lat_to_cm_ratio;          // 纬度差转平面距离因子
};
extern struct INS_DATA ins;
extern unsigned char mag_cal_status; // 0---无校准  1-- 校准xz    2--校准完成

// 收到Imu数据标志位
extern bool recieve_isimudata;
// 收到Gps数据标志位
extern bool recieve_isgpsdata;
// 收到Sensor数据标志位
extern bool recieve_issensordata;
// 接收到了INS数据
extern comp_status imu_link_status;
// DMA传输出错标志位
extern bool dma_iserror;

// 磁校准
extern bool ci_jiao_zhun;

// 通知 IMU 解锁了
extern bool notify_imu_unlock;
// 通知 IMU 上锁了
extern bool notify_imu_lock;


// 位置平均滤波
extern char average_filter_counts;

void send_req_msg_to_imu(uint8_t req_id);

void send_calibration_msg_to_imu(uint8_t calib_id, uint8_t *calib_value,
                                 uint16_t len);

void imu_uart3_initial(unsigned int imu_bps);

void imu_euler_angle_clear(void);

void imu_attitude_ananlysis(void);

void dma_analysis_imudata(void);
void dma_analysis_gpsdata(void);
void dma_analysis_sensordata(void);

void get_right_ins_data(void);
void get_ins_status(void);

void send_msg_to_imu(void);

void unclock_clear_altitude(void);

void send_vklink_msg_to_imu(const VKlink_Msg_Type *txMsg);


void imu_tx_msg_to_gcs(struct GCS_Link *pgcs);

void send_imu_hdw_data(uint8_t update_type, void* data);

#endif