#ifndef __SOFT_IMU_H #define __SOFT_IMU_H #include "common.h" #include "soft_gs.h" #include "stdbool.h" #include "vklink.h" #include 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