/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* support for serial connected InertialLabs INS system */ #pragma once #include "AP_ExternalAHRS_config.h" #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED #include "AP_ExternalAHRS_backend.h" class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { public: AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); // get serial port number, -1 for not enabled int8_t get_port(void) const override; // accessors for AP_AHRS bool healthy(void) const override; bool initialised(void) const override; bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; void get_filter_status(nav_filter_status &status) const override; void send_status_report(class GCS_MAVLINK &link) const override; // check for new data void update() override { check_uart(); } // Get model/type name const char* get_name() const override { return "ILabs"; } enum class MessageType : uint8_t { GPS_INS_TIME_MS = 0x01, GPS_WEEK = 0x3C, ACCEL_DATA_HR = 0x23, GYRO_DATA_HR = 0x21, BARO_DATA = 0x25, MAG_DATA = 0x24, ORIENTATION_ANGLES = 0x07, VELOCITIES = 0x12, POSITION = 0x10, KF_VEL_COVARIANCE = 0x58, KF_POS_COVARIANCE = 0x57, UNIT_STATUS = 0x53, GNSS_EXTENDED_INFO = 0x4A, NUM_SATS = 0x3B, GNSS_POSITION = 0x30, GNSS_VEL_TRACK = 0x32, GNSS_POS_TIMESTAMP = 0x3E, GNSS_INFO_SHORT = 0x36, GNSS_NEW_DATA = 0x41, GNSS_JAM_STATUS = 0xC0, DIFFERENTIAL_PRESSURE = 0x28, TRUE_AIRSPEED = 0x86, WIND_SPEED = 0x8A, AIR_DATA_STATUS = 0x8D, SUPPLY_VOLTAGE = 0x50, TEMPERATURE = 0x52, UNIT_STATUS2 = 0x5A, GNSS_ANGLES = 0x33, GNSS_ANGLE_POS_TYPE = 0x3A, GNSS_HEADING_TIMESTAMP = 0x40, GNSS_DOP = 0x42, INS_SOLUTION_STATUS = 0x54, }; /* packets consist of: ILabsHeader list of MessageType sequence of ILabsData checksum */ struct PACKED ILabsHeader { uint16_t magic; // 0x55AA uint8_t msg_type; // always 1 for INS data uint8_t msg_id; // always 0x95 uint16_t msg_len; // msg_len+2 is total packet length }; struct PACKED vec3_16_t { int16_t x,y,z; Vector3f tofloat(void) { return Vector3f(x,y,z); } }; struct PACKED vec3_32_t { int32_t x,y,z; Vector3f tofloat(void) { return Vector3f(x,y,z); } }; struct PACKED vec3_u8_t { uint8_t x,y,z; Vector3f tofloat(void) { return Vector3f(x,y,z); } }; struct PACKED vec3_u16_t { uint16_t x,y,z; Vector3f tofloat(void) { return Vector3f(x,y,z); } }; struct gnss_extended_info_t { uint8_t fix_type; uint8_t spoofing_status; }; struct gnss_info_short_t { uint8_t info1; uint8_t info2; }; union PACKED ILabsData { uint32_t gnss_time_ms; // ms since start of GNSS week uint16_t gnss_week; vec3_32_t accel_data_hr; // g * 1e6 vec3_32_t gyro_data_hr; // deg/s * 1e5 struct PACKED { uint16_t pressure_pa2; // Pascals/2 int32_t baro_alt; // meters*100 } baro_data; vec3_16_t mag_data; // nT/10 struct PACKED { uint16_t yaw; // deg*100 int16_t pitch; // deg*100 int16_t roll; // deg*100 } orientation_angles; // 321 euler order? vec3_32_t velocity; // m/s * 100 struct PACKED { int32_t lat; // deg*1e7 int32_t lon; // deg*1e7 int32_t alt; // m*100, AMSL } position; vec3_u8_t kf_vel_covariance; // mm/s vec3_u16_t kf_pos_covariance; // mm uint16_t unit_status; // set ILABS_UNIT_STATUS_* gnss_extended_info_t gnss_extended_info; uint8_t num_sats; struct PACKED { int32_t lat; // deg*1e7 int32_t lon; // deg*1e7 int32_t alt; // m*100 } gnss_position; struct PACKED { int32_t hor_speed; // m/s*100 uint16_t track_over_ground; // deg*100 int32_t ver_speed; // m/s*100 } gnss_vel_track; uint32_t gnss_pos_timestamp; // ms gnss_info_short_t gnss_info_short; uint8_t gnss_new_data; uint8_t gnss_jam_status; int32_t differential_pressure; // mbar*1e4 int16_t true_airspeed; // m/s*100 vec3_16_t wind_speed; // m/s*100 uint16_t air_data_status; uint16_t supply_voltage; // V*100 int16_t temperature; // degC*10 uint16_t unit_status2; struct PACKED { uint16_t heading; // deg*100 int16_t pitch; // deg*100 } gnss_angles; uint8_t gnss_angle_pos_type; uint32_t gnss_heading_timestamp; // ms struct PACKED { uint16_t gdop; uint16_t pdop; uint16_t hdop; uint16_t vdop; uint16_t tdop; } gnss_dop; // 10e3 uint8_t ins_sol_status; }; AP_ExternalAHRS::gps_data_message_t gps_data; AP_ExternalAHRS::mag_data_message_t mag_data; AP_ExternalAHRS::baro_data_message_t baro_data; AP_ExternalAHRS::ins_data_message_t ins_data; AP_ExternalAHRS::airspeed_data_message_t airspeed_data; uint16_t buffer_ofs; uint8_t buffer[256]; // max for normal message set is 167+8 protected: uint8_t num_gps_sensors(void) const override { return 1; } private: AP_HAL::UARTDriver *uart; int8_t port_num; uint32_t baudrate; bool setup_complete; void update_thread(); bool check_uart(); bool check_header(const ILabsHeader *h) const; // re-sync on header bytes void re_sync(void); static const struct MessageLength { MessageType mtype; uint8_t length; } message_lengths[]; struct { float baro_alt; Vector3f kf_vel_covariance; Vector3f kf_pos_covariance; uint16_t unit_status; uint16_t unit_status2; float differential_pressure; float true_airspeed; Vector3f wind_speed; uint16_t air_data_status; float supply_voltage; uint8_t ins_sol_status; } state2; struct { float lat; float lng; float alt; float hor_speed; float ver_speed; float track_over_ground; uint8_t new_data; uint32_t pos_timestamp; uint32_t heading_timestamp; uint8_t spoof_status; uint8_t jam_status; uint8_t angle_pos_type; gnss_info_short_t info_short; float heading; float pitch; float gdop; float pdop; float tdop; } gnss_data; uint16_t last_unit_status; uint16_t last_unit_status2; uint16_t last_air_data_status; uint8_t last_spoof_status; uint8_t last_jam_status; uint32_t last_critical_msg_ms; uint32_t last_att_ms; uint32_t last_vel_ms; uint32_t last_pos_ms; uint32_t last_gps_ms; }; #endif // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED