diff --git a/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt new file mode 100644 index 0000000000000..ab2be0d1a4547 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt @@ -0,0 +1,7 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 +5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 5c76db5d35d88..09041b17dab92 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3279,6 +3279,10 @@ def MicroStrainEAHRS7(self): '''Test MicroStrain EAHRS series 7 support''' self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt") + def InertialLabsEAHRS(self): + '''Test InertialLabs EAHRS support''' + self.fly_external_AHRS("ILabs", 5, "ap1.txt") + def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -5358,6 +5362,7 @@ def tests(self): self.VectorNavEAHRS, self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, + self.InertialLabsEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 312471f0e9f5b..c48b06bfd3773 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -220,12 +220,12 @@ void AP_AHRS::init() } #if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE if (_ekf_type.get() == 2) { - _ekf_type.set(3); + _ekf_type.set(EKFType::THREE); EKF3.set_enable(true); } #elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE if (_ekf_type.get() == 3) { - _ekf_type.set(2); + _ekf_type.set(EKFType::TWO); EKF2.set_enable(true); } #endif @@ -234,7 +234,7 @@ void AP_AHRS::init() // a special case to catch users who had AHRS_EKF_TYPE=2 saved and // updated to a version where EK2_ENABLE=0 if (_ekf_type.get() == 2 && !EKF2.get_enable() && EKF3.get_enable()) { - _ekf_type.set(3); + _ekf_type.set(EKFType::THREE); } #endif diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 0f5c985eeb1a7..1d6841a28a0dd 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -421,6 +421,29 @@ class AP_AHRS { return _ekf_type; } + enum class EKFType : uint8_t { +#if AP_AHRS_DCM_ENABLED + DCM = 0, +#endif +#if HAL_NAVEKF3_AVAILABLE + THREE = 3, +#endif +#if HAL_NAVEKF2_AVAILABLE + TWO = 2, +#endif +#if AP_AHRS_SIM_ENABLED + SIM = 10, +#endif +#if HAL_EXTERNAL_AHRS_ENABLED + EXTERNAL = 11, +#endif + }; + + // set the selected ekf type, for RC aux control + void set_ekf_type(EKFType ahrs_type) { + _ekf_type.set(ahrs_type); + } + // these are only out here so vehicles can reference them for parameters #if HAL_NAVEKF2_AVAILABLE NavEKF2 EKF2; @@ -668,7 +691,7 @@ class AP_AHRS { */ AP_Int8 _wind_max; AP_Int8 _board_orientation; - AP_Int8 _ekf_type; + AP_Enum _ekf_type; /* * DCM-backend parameters; it takes references to these @@ -683,23 +706,6 @@ class AP_AHRS { AP_Enum _gps_use; AP_Int8 _gps_minsats; - enum class EKFType { -#if AP_AHRS_DCM_ENABLED - DCM = 0, -#endif -#if HAL_NAVEKF3_AVAILABLE - THREE = 3, -#endif -#if HAL_NAVEKF2_AVAILABLE - TWO = 2, -#endif -#if AP_AHRS_SIM_ENABLED - SIM = 10, -#endif -#if HAL_EXTERNAL_AHRS_ENABLED - EXTERNAL = 11, -#endif - }; EKFType active_EKF_type(void) const { return state.active_EKF; } bool always_use_EKF() const { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5dd7f681ba195..b26bdeb472714 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -101,6 +101,22 @@ AP_AHRS_DCM::update() // remember the last origin for fallback support IGNORE_RETURN(AP::ahrs().get_origin(last_origin)); + +#if HAL_LOGGING_ENABLED + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_log_ms >= 100) { + // log DCM at 10Hz + last_log_ms = now_ms; + AP::logger().WriteStreaming("DCM", "TimeUS,Roll,Pitch,Yaw", + "sddd", + "F000", + "Qfff", + AP_HAL::micros64(), + degrees(roll), + degrees(pitch), + wrap_360(degrees(yaw))); + } +#endif // HAL_LOGGING_ENABLED } void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 690ef22810bed..0d3dedc5b54d9 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -286,6 +286,8 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { // pre-calculated trig cache: float _sin_yaw; float _cos_yaw; + + uint32_t last_log_ms; }; #endif // AP_AHRS_DCM_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index a13480b531c7e..65c0b7b291131 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -52,6 +52,7 @@ #include "AP_Airspeed_DroneCAN.h" #include "AP_Airspeed_NMEA.h" #include "AP_Airspeed_MSP.h" +#include "AP_Airspeed_External.h" #include "AP_Airspeed_SITL.h" extern const AP_HAL::HAL &hal; @@ -433,6 +434,11 @@ void AP_Airspeed::allocate() case TYPE_MSP: #if AP_AIRSPEED_MSP_ENABLED sensor[i] = new AP_Airspeed_MSP(*this, i, 0); +#endif + break; + case TYPE_EXTERNAL: +#if AP_AIRSPEED_EXTERNAL_ENABLED + sensor[i] = new AP_Airspeed_External(*this, i); #endif break; } @@ -731,6 +737,24 @@ void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt) } #endif +#if AP_AIRSPEED_EXTERNAL_ENABLED +/* + handle airspeed airspeed data + */ +void AP_Airspeed::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) +{ + if (!lib_enabled()) { + return; + } + + for (uint8_t i=0; ihandle_external(pkt); + } + } +} +#endif + // @LoggerMessage: HYGR // @Description: Hygrometer data // @Field: TimeUS: Time since system startup diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 5574f807557a6..8f6666f62b822 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -10,6 +10,9 @@ #if AP_AIRSPEED_MSP_ENABLED #include #endif +#if AP_AIRSPEED_EXTERNAL_ENABLED +#include +#endif class AP_Airspeed_Backend; @@ -187,6 +190,7 @@ class AP_Airspeed TYPE_NMEA_WATER=13, TYPE_MSP=14, TYPE_I2C_ASP5033=15, + TYPE_EXTERNAL=16, TYPE_SITL=100, }; @@ -208,6 +212,10 @@ class AP_Airspeed void handle_msp(const MSP::msp_airspeed_data_message_t &pkt); #endif +#if AP_AIRSPEED_EXTERNAL_ENABLED + void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt); +#endif + enum class CalibrationState { NOT_STARTED, IN_PROGRESS, diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 58ca025883a7a..bdb4dded5c562 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -49,6 +49,9 @@ class AP_Airspeed_Backend { virtual bool get_airspeed(float& airspeed) {return false;} virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {} +#if AP_AIRSPEED_EXTERNAL_ENABLED + virtual void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) {} +#endif #if AP_AIRSPEED_HYGROMETER_ENABLE // optional hygrometer support diff --git a/libraries/AP_Airspeed/AP_Airspeed_External.cpp b/libraries/AP_Airspeed/AP_Airspeed_External.cpp new file mode 100644 index 0000000000000..5fc709f069bc9 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_External.cpp @@ -0,0 +1,58 @@ +#include "AP_Airspeed_External.h" + +#if AP_AIRSPEED_EXTERNAL_ENABLED + +AP_Airspeed_External::AP_Airspeed_External(AP_Airspeed &_frontend, uint8_t _instance) : + AP_Airspeed_Backend(_frontend, _instance) +{ + set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,0,_instance,0)); +} + +// return the current differential_pressure in Pascal +bool AP_Airspeed_External::get_differential_pressure(float &pressure) +{ + WITH_SEMAPHORE(sem); + if (press_count == 0) { + return false; + } + pressure = sum_pressure/press_count; + press_count = 0; + sum_pressure = 0; + return true; +} + +// get last temperature +bool AP_Airspeed_External::get_temperature(float &temperature) +{ + WITH_SEMAPHORE(sem); + if (temperature_count == 0) { + return false; + } + temperature = sum_temperature/temperature_count; + temperature_count = 0; + sum_temperature = 0; + return true; +} + +void AP_Airspeed_External::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) +{ + WITH_SEMAPHORE(sem); + + sum_pressure += pkt.differential_pressure; + press_count++; + if (press_count > 100) { + // prevent overflow + sum_pressure /= 2; + press_count /= 2; + } + + sum_temperature += pkt.temperature; + temperature_count++; + if (temperature_count > 100) { + // prevent overflow + sum_temperature /= 2; + temperature_count /= 2; + } +} + +#endif // AP_AIRSPEED_EXTERNAL_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_External.h b/libraries/AP_Airspeed/AP_Airspeed_External.h new file mode 100644 index 0000000000000..cd27d757f2ce2 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_External.h @@ -0,0 +1,38 @@ +/* + external AHRS airspeed backend + */ +#pragma once + +#include "AP_Airspeed_config.h" + +#if AP_AIRSPEED_EXTERNAL_ENABLED + +#include "AP_Airspeed_Backend.h" +#include + +class AP_Airspeed_External : public AP_Airspeed_Backend +{ +public: + AP_Airspeed_External(AP_Airspeed &airspeed, uint8_t instance); + + bool init(void) override { + return true; + } + + void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) override; + + // return the current differential_pressure in Pascal + bool get_differential_pressure(float &pressure) override; + + // temperature not available via analog backend + bool get_temperature(float &temperature) override; + +private: + float sum_pressure; + uint8_t press_count; + float sum_temperature; + uint8_t temperature_count; +}; + +#endif // AP_AIRSPEED_EXTERNAL_ENABLED + diff --git a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp index 37c6b69faa262..749fa9d612164 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp @@ -46,7 +46,7 @@ const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = { // @Param: TYPE // @DisplayName: Airspeed type // @Description: Type of airspeed sensor - // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Airspeed/AP_Airspeed_config.h b/libraries/AP_Airspeed/AP_Airspeed_config.h index 4ba2e4179388a..f12db5cb16c4e 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_config.h +++ b/libraries/AP_Airspeed/AP_Airspeed_config.h @@ -3,6 +3,7 @@ #include #include #include +#include #ifndef AP_AIRSPEED_ENABLED #define AP_AIRSPEED_ENABLED 1 @@ -66,3 +67,7 @@ #ifndef AP_AIRSPEED_HYGROMETER_ENABLE #define AP_AIRSPEED_HYGROMETER_ENABLE (AP_AIRSPEED_ENABLED && BOARD_FLASH_SIZE > 1024) #endif + +#ifndef AP_AIRSPEED_EXTERNAL_ENABLED +#define AP_AIRSPEED_EXTERNAL_ENABLED AP_AIRSPEED_ENABLED && HAL_EXTERNAL_AHRS_ENABLED +#endif diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index b191a6b52b7ca..7e4fd7970b581 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -61,7 +61,6 @@ #define GAIN_CC50 20.0f // LSB/uT #define GAIN_CC100 38.0f #define GAIN_CC200 75.0f -#define UTESLA_TO_MGAUSS 10.0f // uT to mGauss conversion #define TMRC 0x94 // Update rate 150Hz #define CMM 0x71 // read 3 axes and set data ready if 3 axes are ready diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index f75d6ac1d4fd7..0bb0d0af22acb 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -25,8 +25,10 @@ #include "AP_ExternalAHRS_VectorNav.h" #include "AP_ExternalAHRS_MicroStrain5.h" #include "AP_ExternalAHRS_MicroStrain7.h" +#include "AP_ExternalAHRS_InertialLabs.h" #include +#include extern const AP_HAL::HAL &hal; @@ -94,21 +96,31 @@ void AP_ExternalAHRS::init(void) case DevType::None: // nothing to do return; + #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED case DevType::VecNav: backend = new AP_ExternalAHRS_VectorNav(this, state); return; #endif + #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED case DevType::MicroStrain5: backend = new AP_ExternalAHRS_MicroStrain5(this, state); return; #endif + #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED case DevType::MicroStrain7: backend = new AP_ExternalAHRS_MicroStrain7(this, state); return; #endif + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + case DevType::InertialLabs: + backend = new AP_ExternalAHRS_InertialLabs(this, state); + return; +#endif + } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype)); @@ -166,6 +178,19 @@ bool AP_ExternalAHRS::get_location(Location &loc) } WITH_SEMAPHORE(state.sem); loc = state.location; + + if (state.last_location_update_us != 0 && + state.have_velocity) { + // extrapolate position based on velocity to cope with slow backends + const float dt = (AP_HAL::micros() - state.last_location_update_us)*1.0e-6; + if (dt < 1) { + // only extrapolate for 1s max + Vector3p ofs = state.velocity.topostype(); + ofs *= dt; + loc.offset(ofs); + } + } + return true; } @@ -242,6 +267,20 @@ void AP_ExternalAHRS::update(void) if (backend) { backend->update(); } + + /* + if backend has not supplied an origin and AHRS has an origin + then use that origin so we get a common origin for minimum + disturbance when switching backends + */ + WITH_SEMAPHORE(state.sem); + if (!state.have_origin) { + Location origin; + if (AP::ahrs().get_origin(origin)) { + state.origin = origin; + state.have_origin = true; + } + } } // Get model/type name diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index c825b0f609b6b..557f1e9c69994 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -48,10 +48,12 @@ class AP_ExternalAHRS { #endif #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED MicroStrain5 = 2, +#endif +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + InertialLabs = 5, #endif // 3 reserved for AdNav // 4 reserved for CINS - // 5 reserved for InertialLabs // 6 reserved for Trimble #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED MicroStrain7 = 7, @@ -96,6 +98,8 @@ class AP_ExternalAHRS { bool have_origin; bool have_location; bool have_velocity; + + uint32_t last_location_update_us; } state; // accessors for AP_AHRS @@ -154,6 +158,11 @@ class AP_ExternalAHRS { float temperature; } ins_data_message_t; + typedef struct { + float differential_pressure; // Pa + float temperature; // degC + } airspeed_data_message_t; + protected: enum class OPTIONS { @@ -175,6 +184,11 @@ class AP_ExternalAHRS { bool has_sensor(AvailableSensor sensor) const { return (uint16_t(sensors.get()) & uint16_t(sensor)) != 0; } + + // set default of EAHRS_SENSORS + void set_default_sensors(uint16_t _sensors) { + sensors.set_default(_sensors); + } }; namespace AP { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp new file mode 100644 index 0000000000000..0ab6857f63ca3 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -0,0 +1,708 @@ +/* + 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 + */ + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + +#include "AP_ExternalAHRS_InertialLabs.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +// unit status bits +#define ILABS_UNIT_STATUS_ALIGNMENT_FAIL 0x0001 +#define ILABS_UNIT_STATUS_OPERATION_FAIL 0x0002 +#define ILABS_UNIT_STATUS_GYRO_FAIL 0x0004 +#define ILABS_UNIT_STATUS_ACCEL_FAIL 0x0008 +#define ILABS_UNIT_STATUS_MAG_FAIL 0x0010 +#define ILABS_UNIT_STATUS_ELECTRONICS_FAIL 0x0020 +#define ILABS_UNIT_STATUS_GNSS_FAIL 0x0040 +#define ILABS_UNIT_STATUS_RUNTIME_CAL 0x0080 +#define ILABS_UNIT_STATUS_VOLTAGE_LOW 0x0100 +#define ILABS_UNIT_STATUS_VOLTAGE_HIGH 0x0200 +#define ILABS_UNIT_STATUS_X_RATE_HIGH 0x0400 +#define ILABS_UNIT_STATUS_Y_RATE_HIGH 0x0800 +#define ILABS_UNIT_STATUS_Z_RATE_HIGH 0x1000 +#define ILABS_UNIT_STATUS_MAG_FIELD_HIGH 0x2000 +#define ILABS_UNIT_STATUS_TEMP_RANGE_ERR 0x4000 +#define ILABS_UNIT_STATUS_RUNTIME_CAL2 0x8000 + +// unit status2 bits +#define ILABS_UNIT_STATUS2_ACCEL_X_HIGH 0x0001 +#define ILABS_UNIT_STATUS2_ACCEL_Y_HIGH 0x0002 +#define ILABS_UNIT_STATUS2_ACCEL_Z_HIGH 0x0004 +#define ILABS_UNIT_STATUS2_BARO_FAIL 0x0008 +#define ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL 0x0010 +#define ILABS_UNIT_STATUS2_MAGCAL_2D_ACT 0x0020 +#define ILABS_UNIT_STATUS2_MAGCAL_3D_ACT 0x0020 +#define ILABS_UNIT_STATUS2_GNSS_FUSION_OFF 0x0040 +#define ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF 0x0080 +#define ILABS_UNIT_STATUS2_MAG_FUSION_OFF 0x0100 +#define ILABS_UNIT_STATUS2_GNSS_POS_VALID 0x0200 + +// air data status bits +#define ILABS_AIRDATA_INIT_FAIL 0x0001 +#define ILABS_AIRDATA_DIFF_PRESS_INIT_FAIL 0x0002 +#define ILABS_AIRDATA_STATIC_PRESS_FAIL 0x0004 +#define ILABS_AIRDATA_DIFF_PRESS_FAIL 0x0008 +#define ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR 0x0010 +#define ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR 0x0020 +#define ILABS_AIRDATA_PRESS_ALT_FAIL 0x0040 +#define ILABS_AIRDATA_AIRSPEED_FAIL 0x0080 +#define ILABS_AIRDATA_BELOW_THRESHOLD 0x0100 + + +// constructor +AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_frontend, + AP_ExternalAHRS::state_t &_state) : + AP_ExternalAHRS_backend(_frontend, _state) +{ + auto &sm = AP::serialmanager(); + uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); + if (!uart) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "InertialLabs ExternalAHRS no UART"); + return; + } + baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + + // don't offer IMU by default, at 200Hz it is too slow for many aircraft + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | + uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_InertialLabs::update_thread, void), "ILabs", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + AP_HAL::panic("InertialLabs Failed to start ExternalAHRS update thread"); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs ExternalAHRS initialised"); +} + +/* + re-sync buffer on parse failure + */ +void AP_ExternalAHRS_InertialLabs::re_sync(void) +{ + if (buffer_ofs > 3) { + /* + look for the 2 byte header and try to sync to that + */ + const uint16_t header = 0x55AA; + const uint8_t *p = (const uint8_t *)memmem(&buffer[1], buffer_ofs-3, &header, sizeof(header)); + if (p != nullptr) { + const uint16_t n = p - &buffer[0]; + memmove(&buffer[0], p, buffer_ofs - n); + buffer_ofs -= n; + } else { + buffer_ofs = 0; + } + } else { + buffer_ofs = 0; + } +} + +// macro for checking we don't run past end of message buffer +#define CHECK_SIZE(d) need_re_sync = (message_ofs + (msg_len=sizeof(d)) > buffer_end); if (need_re_sync) break + +// lookup a message in the msg_types bitmask to see if we received it in this packet +#define GOT_MSG(mtype) msg_types.get(unsigned(MessageType::mtype)) + +/* + check header is valid + */ +bool AP_ExternalAHRS_InertialLabs::check_header(const ILabsHeader *h) const +{ + return h->magic == 0x55AA && + h->msg_type == 1 && + h->msg_id == 0x95 && + h->msg_len <= sizeof(buffer)-2; +} + +/* + check the UART for more data + returns true if we have consumed potentially valid bytes + */ +bool AP_ExternalAHRS_InertialLabs::check_uart() +{ + WITH_SEMAPHORE(state.sem); + + if (!setup_complete) { + return false; + } + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + if (n + buffer_ofs > sizeof(buffer)) { + n = sizeof(buffer) - buffer_ofs; + } + const ILabsHeader *h = (ILabsHeader *)&buffer[0]; + + if (buffer_ofs < sizeof(ILabsHeader)) { + n = MIN(n, sizeof(ILabsHeader)-buffer_ofs); + } else { + if (!check_header(h)) { + re_sync(); + return false; + } + if (buffer_ofs > h->msg_len+8) { + re_sync(); + return false; + } + n = MIN(n, uint32_t(h->msg_len + 2 - buffer_ofs)); + } + + const ssize_t nread = uart->read(&buffer[buffer_ofs], n); + if (nread != ssize_t(n)) { + re_sync(); + return false; + } + buffer_ofs += n; + + if (buffer_ofs < sizeof(ILabsHeader)) { + return true; + } + + if (!check_header(h)) { + re_sync(); + return false; + } + + if (buffer_ofs < h->msg_len+2) { + /* + see if we can read the rest immediately + */ + const uint16_t needed = h->msg_len+2 - buffer_ofs; + if (uart->available() < needed) { + // need more data + return true; + } + const ssize_t nread2 = uart->read(&buffer[buffer_ofs], needed); + if (nread2 != needed) { + re_sync(); + return false; + } + buffer_ofs += nread2; + } + + // check checksum + const uint16_t crc1 = crc_sum_of_bytes_16(&buffer[2], buffer_ofs-4); + const uint16_t crc2 = le16toh_ptr(&buffer[buffer_ofs-2]); + if (crc1 != crc2) { + re_sync(); + return false; + } + + const uint8_t *buffer_end = &buffer[buffer_ofs]; + const uint16_t payload_size = h->msg_len - 6; + const uint8_t *payload = &buffer[6]; + if (payload_size < 3) { + re_sync(); + return false; + } + const uint8_t num_messages = payload[0]; + if (num_messages == 0 || + num_messages > payload_size-1) { + re_sync(); + return false; + } + const uint8_t *message_ofs = &payload[num_messages+1]; + bool need_re_sync = false; + + // bitmask for what types we get + Bitmask<256> msg_types; + uint32_t now_ms = AP_HAL::millis(); + + for (uint8_t i=0; i= buffer_end) { + re_sync(); + return false; + } + MessageType mtype = (MessageType)payload[1+i]; + ILabsData &u = *(ILabsData*)message_ofs; + uint8_t msg_len = 0; + + msg_types.set(unsigned(mtype)); + + switch (mtype) { + case MessageType::GPS_INS_TIME_MS: { + // this is the GPS tow timestamp in ms for when the IMU data was sampled + CHECK_SIZE(u.gps_time_ms); + state2.gnss_ins_time_ms = u.gps_time_ms; + break; + } + case MessageType::GPS_WEEK: { + CHECK_SIZE(u.gps_week); + gps_data.gps_week = u.gps_week; + break; + } + case MessageType::ACCEL_DATA_HR: { + CHECK_SIZE(u.accel_data_hr); + ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*GRAVITY_MSS*1.0e-6; + break; + } + case MessageType::GYRO_DATA_HR: { + CHECK_SIZE(u.gyro_data_hr); + ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; + break; + } + case MessageType::BARO_DATA: { + CHECK_SIZE(u.baro_data); + baro_data.pressure_pa = u.baro_data.pressure_pa2*2; + break; + } + case MessageType::MAG_DATA: { + CHECK_SIZE(u.mag_data); + mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); + break; + } + case MessageType::ORIENTATION_ANGLES: { + CHECK_SIZE(u.orientation_angles); + state.quat.from_euler(radians(u.orientation_angles.roll*0.01), + radians(u.orientation_angles.pitch*0.01), + radians(u.orientation_angles.yaw*0.01)); + state.have_quaternion = true; + if (last_att_ms == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got link"); + } + last_att_ms = now_ms; + break; + } + case MessageType::VELOCITIES: { + CHECK_SIZE(u.velocity); + state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01; + state.have_velocity = true; + last_vel_ms = now_ms; + break; + } + case MessageType::POSITION: { + CHECK_SIZE(u.position); + state.location.lat = u.position.lat; + state.location.lng = u.position.lon; + state.location.alt = u.position.alt; + state.have_location = true; + state.last_location_update_us = AP_HAL::micros(); + last_pos_ms = now_ms; + break; + } + case MessageType::KF_VEL_COVARIANCE: { + CHECK_SIZE(u.kf_vel_covariance); + state2.kf_vel_covariance = u.kf_vel_covariance.tofloat() * 0.001; + break; + } + case MessageType::KF_POS_COVARIANCE: { + CHECK_SIZE(u.kf_pos_covariance); + state2.kf_pos_covariance = u.kf_pos_covariance.tofloat() * 0.001; + break; + } + case MessageType::UNIT_STATUS: { + CHECK_SIZE(u.unit_status); + state2.unit_status = u.unit_status; + break; + } + case MessageType::GNSS_EXTENDED_INFO: { + CHECK_SIZE(u.gnss_extended_info); + gps_data.fix_type = u.gnss_extended_info.fix_type+1; + state2.gnss_extended_info = u.gnss_extended_info; + break; + } + case MessageType::NUM_SATS: { + CHECK_SIZE(u.num_sats); + gps_data.satellites_in_view = u.num_sats; + break; + } + case MessageType::GNSS_POSITION: { + CHECK_SIZE(u.position); + gps_data.latitude = u.position.lat; + gps_data.longitude = u.position.lon; + gps_data.msl_altitude = u.position.alt; + break; + } + case MessageType::GNSS_VEL_TRACK: { + CHECK_SIZE(u.gnss_vel_track); + Vector2f velxy; + velxy.offset_bearing(u.gnss_vel_track.track_over_ground*0.01, u.gnss_vel_track.hor_speed*0.01); + gps_data.ned_vel_north = velxy.x; + gps_data.ned_vel_east = velxy.y; + gps_data.ned_vel_down = -u.gnss_vel_track.ver_speed*0.01; + break; + } + case MessageType::GNSS_POS_TIMESTAMP: { + CHECK_SIZE(u.gnss_pos_timestamp); + gps_data.ms_tow = u.gnss_pos_timestamp; + break; + } + case MessageType::GNSS_INFO_SHORT: { + CHECK_SIZE(u.gnss_info_short); + state2.gnss_info_short = u.gnss_info_short; + break; + } + case MessageType::GNSS_NEW_DATA: { + CHECK_SIZE(u.gnss_new_data); + state2.gnss_new_data = u.gnss_new_data; + break; + } + case MessageType::GNSS_JAM_STATUS: { + CHECK_SIZE(u.gnss_jam_status); + state2.gnss_jam_status = u.gnss_jam_status; + break; + } + case MessageType::DIFFERENTIAL_PRESSURE: { + CHECK_SIZE(u.differential_pressure); + airspeed_data.differential_pressure = u.differential_pressure*1.0e-4; + break; + } + case MessageType::TRUE_AIRSPEED: { + CHECK_SIZE(u.true_airspeed); + state2.true_airspeed = u.true_airspeed*0.01; + break; + } + case MessageType::WIND_SPEED: { + CHECK_SIZE(u.wind_speed); + state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; + break; + } + case MessageType::AIR_DATA_STATUS: { + CHECK_SIZE(u.air_data_status); + state2.air_data_status = u.air_data_status; + break; + } + case MessageType::SUPPLY_VOLTAGE: { + CHECK_SIZE(u.supply_voltage); + state2.supply_voltage = u.supply_voltage*0.01; + break; + } + case MessageType::TEMPERATURE: { + CHECK_SIZE(u.temperature); + // assume same temperature for baro and airspeed + baro_data.temperature = u.temperature*0.1; + airspeed_data.temperature = u.temperature*0.1; + break; + } + case MessageType::UNIT_STATUS2: { + CHECK_SIZE(u.unit_status2); + state2.unit_status2 = u.unit_status2; + break; + } + } + + if (msg_len == 0) { + // got an unknown message + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: unknown msg 0x%02x", unsigned(mtype)); + buffer_ofs = 0; + return false; + } + message_ofs += msg_len; + + if (msg_len == 0 || need_re_sync) { + re_sync(); + return false; + } + } + + if (h->msg_len != message_ofs-buffer) { + // we had stray bytes at the end of the message + re_sync(); + return false; + } + + if (GOT_MSG(ACCEL_DATA_HR) && + GOT_MSG(GYRO_DATA_HR)) { + AP::ins().handle_external(ins_data); + } + if (GOT_MSG(GPS_INS_TIME_MS) && + GOT_MSG(NUM_SATS) && + GOT_MSG(GNSS_POSITION) && + GOT_MSG(GNSS_NEW_DATA) && + GOT_MSG(GNSS_EXTENDED_INFO) && + state2.gnss_new_data != 0) { + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps_data, instance); + } + if (gps_data.satellites_in_view > 3) { + if (last_gps_ms == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got GPS lock"); + if (!state.have_origin) { + state.origin = Location{ + gps_data.latitude, + gps_data.longitude, + gps_data.msl_altitude, + Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + } + last_gps_ms = now_ms; + } + } + if (GOT_MSG(BARO_DATA) && + GOT_MSG(TEMPERATURE)) { + AP::baro().handle_external(baro_data); + } + if (GOT_MSG(MAG_DATA)) { + AP::compass().handle_external(mag_data); + } +#if AP_AIRSPEED_EXTERNAL_ENABLED && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)) + // only on plane and copter as others do not link AP_Airspeed + if (GOT_MSG(DIFFERENTIAL_PRESSURE) && + GOT_MSG(TEMPERATURE)) { + auto *arsp = AP::airspeed(); + if (arsp != nullptr) { + arsp->handle_external(airspeed_data); + } + } +#endif // AP_AIRSPEED_EXTERNAL_ENABLED + buffer_ofs = 0; + + if (GOT_MSG(POSITION) && + GOT_MSG(ORIENTATION_ANGLES) && + GOT_MSG(VELOCITIES)) { + + float roll, pitch, yaw; + state.quat.to_euler(roll, pitch, yaw); + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB1 + // @Description: InertialLabs AHRS data1 + // @Field: TimeUS: Time since system startup + // @Field: Roll: euler roll + // @Field: Pitch: euler pitch + // @Field: Yaw: euler yaw + // @Field: VN: velocity north + // @Field: VE: velocity east + // @Field: VD: velocity down + // @Field: Lat: latitude + // @Field: Lon: longitude + // @Field: Alt: altitude AMSL + + AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt", + "sdddnnnDUm", + "F000000GG0", + "QffffffLLf", + now_us, + degrees(roll), degrees(pitch), degrees(yaw), + state.velocity.x, state.velocity.y, state.velocity.z, + state.location.lat, state.location.lng, state.location.alt*0.01); + + // @LoggerMessage: ILB2 + // @Description: InertialLabs AHRS data2 + // @Field: TimeUS: Time since system startup + // @Field: PosVarN: position variance north + // @Field: PosVarE: position variance east + // @Field: PosVarD: position variance down + // @Field: VelVarN: velocity variance north + // @Field: VelVarE: velocity variance east + // @Field: VelVarD: velocity variance down + + AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", + "smmmnnn", + "F000000", + "Qffffff", + now_us, + state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z, + state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z); + + // @LoggerMessage: ILB3 + // @Description: InertialLabs AHRS data3 + // @Field: TimeUS: Time since system startup + // @Field: Stat1: unit status1 + // @Field: Stat2: unit status2 + // @Field: FType: fix type + // @Field: SpStat: spoofing status + // @Field: GI1: GNSS Info1 + // @Field: GI2: GNSS Info2 + // @Field: GJS: GNSS jamming status + // @Field: TAS: true airspeed + // @Field: WVN: Wind velocity north + // @Field: WVE: Wind velocity east + // @Field: WVD: Wind velocity down + + AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", + "s-----------", + "F-----------", + "QHHBBBBBffff", + now_us, + state2.unit_status, state2.unit_status2, + state2.gnss_extended_info.fix_type, state2.gnss_extended_info.spoofing_status, + state2.gnss_info_short.info1, state2.gnss_info_short.info2, + state2.gnss_jam_status, + state2.true_airspeed, + state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z); + } + + return true; +} + +void AP_ExternalAHRS_InertialLabs::update_thread() +{ + // Open port in the thread + uart->begin(baudrate, 1024, 512); + + /* + we assume the user has already configured the device + */ + + setup_complete = true; + while (true) { + if (!check_uart()) { + hal.scheduler->delay_microseconds(250); + } + } +} + +// get serial port number for the uart +int8_t AP_ExternalAHRS_InertialLabs::get_port(void) const +{ + if (!uart) { + return -1; + } + return port_num; +}; + +// accessors for AP_AHRS +bool AP_ExternalAHRS_InertialLabs::healthy(void) const +{ + WITH_SEMAPHORE(state.sem); + return AP_HAL::millis() - last_att_ms < 100; +} + +bool AP_ExternalAHRS_InertialLabs::initialised(void) const +{ + if (!setup_complete) { + return false; + } + return true; +} + +bool AP_ExternalAHRS_InertialLabs::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!setup_complete) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs setup failed"); + return false; + } + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs unhealthy"); + return false; + } + WITH_SEMAPHORE(state.sem); + uint32_t now = AP_HAL::millis(); + if (now - last_att_ms > 10 || + now - last_pos_ms > 10 || + now - last_vel_ms > 10) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs not up to date"); + return false; + } + return true; +} + +/* + get filter status. We don't know the meaning of the status bits yet, + so assume all OK if we have GPS lock + */ +void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) const +{ + WITH_SEMAPHORE(state.sem); + uint32_t now = AP_HAL::millis(); + const uint32_t dt_limit = 200; + const uint32_t dt_limit_gps = 500; + memset(&status, 0, sizeof(status)); + const bool init_ok = (state2.unit_status & (ILABS_UNIT_STATUS_ALIGNMENT_FAIL|ILABS_UNIT_STATUS_OPERATION_FAIL))==0; + status.flags.initalized = init_ok; + status.flags.attitude = init_ok && (now - last_att_ms < dt_limit) && init_ok; + status.flags.vert_vel = init_ok && (now - last_vel_ms < dt_limit); + status.flags.vert_pos = init_ok && (now - last_pos_ms < dt_limit); + status.flags.horiz_vel = status.flags.vert_vel; + status.flags.horiz_pos_abs = status.flags.vert_pos; + status.flags.horiz_pos_rel = status.flags.horiz_pos_abs; + status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_abs; + status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; + status.flags.using_gps = (now - last_gps_ms < dt_limit_gps) && + (state2.unit_status & (ILABS_UNIT_STATUS_GNSS_FAIL|ILABS_UNIT_STATUS2_GNSS_FUSION_OFF)) == 0; + status.flags.gps_quality_good = (now - last_gps_ms < dt_limit_gps) && + (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) != 0 && + (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) == 0; + status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL); +} + +// send an EKF_STATUS message to GCS +void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const +{ + // prepare flags + uint16_t flags = 0; + nav_filter_status filterStatus; + get_filter_status(filterStatus); + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; + } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; + } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + if (!filterStatus.flags.initalized) { + flags |= EKF_UNINITIALIZED; + } + + // send message + const float vel_gate = 5; + const float pos_gate = 5; + const float hgt_gate = 5; + const float mag_var = 0; + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + state2.kf_vel_covariance.length()/vel_gate, + state2.kf_pos_covariance.xy().length()/pos_gate, + state2.kf_pos_covariance.z/hgt_gate, + mag_var, 0, 0); +} + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h new file mode 100644 index 0000000000000..bee7d5c28e0cc --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -0,0 +1,226 @@ +/* + 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_INERTIAL_LABS_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, + }; + + /* + 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 gps_time_ms; // ms since start of GPS week + uint16_t gps_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 { + int16_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; + uint16_t unit_status; // set ILABS_UNIT_STATUS_* + gnss_extended_info_t gnss_extended_info; + uint8_t num_sats; + 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; + }; + + 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 + +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 { + Vector3f kf_vel_covariance; + Vector3f kf_pos_covariance; + uint32_t gnss_ins_time_ms; + uint16_t unit_status; + uint16_t unit_status2; + gnss_extended_info_t gnss_extended_info; + gnss_info_short_t gnss_info_short; + uint8_t gnss_new_data; + uint8_t gnss_jam_status; + float differential_pressure; + float true_airspeed; + Vector3f wind_speed; + uint16_t air_data_status; + float supply_voltage; + } state2; + + uint32_t last_att_ms; + uint32_t last_vel_ms; + uint32_t last_pos_ms; + uint32_t last_gps_ms; +}; + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index 141b4ff857e7c..ed6304429c28d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -163,6 +163,7 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const state.location = Location{filter_data.lat, filter_data.lon, gnss_data[gnss_instance].msl_altitude, Location::AltFrame::ABSOLUTE}; state.have_location = true; + state.last_location_update_us = AP_HAL::micros(); } AP_ExternalAHRS::gps_data_message_t gps { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 8e8515b70aa17..13acaf4fc993c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -475,6 +475,7 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) int32_t(pkt1.positionLLA[1] * 1.0e7), int32_t(pkt1.positionLLA[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; + state.last_location_update_us = AP_HAL::micros(); state.have_location = true; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index 07436b7ae4388..02eb1b5319b7d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -53,6 +53,11 @@ class AP_ExternalAHRS_backend { uint16_t get_rate(void) const; bool option_is_set(AP_ExternalAHRS::OPTIONS option) const; + // set default of EAHRS_SENSORS + void set_default_sensors(uint16_t sensors) { + frontend.set_default_sensors(sensors); + } + private: AP_ExternalAHRS &frontend; }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 3b2caa2387046..0119e6857ecea 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -25,3 +25,7 @@ #ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED #define AP_EXTERNAL_AHRS_VECTORNAV_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED +#define AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 9a270445abf71..b7c562f3b47d8 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -230,6 +230,9 @@ void SIMState::fdm_input_local(void) if (microstrain5 != nullptr) { microstrain5->update(); } + if (inertiallabs != nullptr) { + inertiallabs->update(); + } #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 41505a6263d90..4c8350371a70b 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -198,6 +199,9 @@ class AP_HAL::SIMState { // simulated MicroStrain Series 7 system SITL::MicroStrain7 *microstrain7; + // simulated InertialLabs INS-U + SITL::InertialLabs *inertiallabs; + #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 718d80537e182..f05bbf20b1fc7 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -276,6 +276,14 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const } microstrain7 = new SITL::MicroStrain7(); return microstrain7; + + } else if (streq(name, "ILabs")) { + if (inertiallabs != nullptr) { + AP_HAL::panic("Only one InertialLabs INS at a time"); + } + inertiallabs = new SITL::InertialLabs(); + return inertiallabs; + #if HAL_SIM_AIS_ENABLED } else if (streq(name, "AIS")) { if (ais != nullptr) { @@ -445,6 +453,9 @@ void SITL_State_Common::sim_update(void) if (microstrain7 != nullptr) { microstrain7->update(); } + if (inertiallabs != nullptr) { + inertiallabs->update(); + } #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 3a2d722fc5ebd..5d5ea6999db60 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -203,6 +204,9 @@ class HALSITL::SITL_State_Common { // simulated MicroStrain system SITL::MicroStrain7 *microstrain7; + // simulated InertialLabs INS + SITL::InertialLabs *inertiallabs; + #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 985a02c290baa..de81543b485cc 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -599,13 +599,19 @@ uint8_t parity(uint8_t byte) return p; } -// sums the bytes in the supplied buffer, returns that sum mod 256 -// (i.e. shoved into a uint8_t) -uint8_t crc_sum_of_bytes(uint8_t *data, uint16_t count) +// sums the bytes in the supplied buffer, returns that sum mod 0xFFFF +uint16_t crc_sum_of_bytes_16(const uint8_t *data, uint16_t count) { - uint8_t ret = 0; + uint16_t ret = 0; for (uint32_t i=0; i{x,y,z}; } + // convert from right-front-up to front-right-down + // or ENU to NED + Vector3 rfu_to_frd() const { + return Vector3{y,x,-z}; + } + // given a position p1 and a velocity v1 produce a vector // perpendicular to v1 maximising distance from p1. If p1 is the // zero vector the return from the function will always be the diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index fb85273f26ecc..f454515aefab8 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -207,6 +207,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 108:QRTL Mode // @Values{Copter}: 109:use Custom Controller // @Values{Copter, Rover, Plane, Blimp}: 110:KillIMU3 + // @Values{Copter,Plane,Rover,Blimp,Sub,Tracker}: 112:SwitchExternalAHRS // @Values{Plane}: 150:CRUISE Mode // @Values{Copter}: 151:TURTLE Mode // @Values{Copter}: 152:SIMPLE heading reset @@ -700,6 +701,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo case AUX_FUNC::CAMERA_MANUAL_FOCUS: case AUX_FUNC::CAMERA_AUTO_FOCUS: case AUX_FUNC::CAMERA_LENS: + case AUX_FUNC::AHRS_TYPE: run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; default: @@ -1613,6 +1615,14 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos AP::ahrs().request_yaw_reset(); break; + case AUX_FUNC::AHRS_TYPE: { +#if HAL_NAVEKF3_AVAILABLE && HAL_EXTERNAL_AHRS_ENABLED + AP::ahrs().set_ekf_type(ch_flag==AuxSwitchPos::HIGH? AP_AHRS::EKFType::EXTERNAL : AP_AHRS::EKFType::THREE); +#endif + break; + } + + #if HAL_TORQEEDO_ENABLED // clear torqeedo error case AUX_FUNC::TORQEEDO_CLEAR_ERR: { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index bf9d70f5bcf4e..fcf6104773345 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -216,6 +216,7 @@ class RC_Channel { CUSTOM_CONTROLLER = 109, // use Custom Controller KILL_IMU3 = 110, // disable third IMU (for IMU failure testing) LOWEHEISER_STARTER = 111, // allows for manually running starter + AHRS_TYPE = 112, // change AHRS_EKF_TYPE // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp! // also, if you add an option >255, you will need to fix duplicate_options_exist diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 63e4b99441c85..334ea2e0a077e 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -74,13 +74,6 @@ class GPS_Backend { virtual void update_read(); // writing fix information to autopilot (e.g.) virtual void publish(const GPS_Data *d) = 0; - -protected: - - uint8_t instance; - GPS &front; - - class SIM *_sitl; struct GPS_TOW { // Number of weeks since midnight 5-6 January 1980 @@ -90,6 +83,14 @@ class GPS_Backend { }; static GPS_TOW gps_time(); + +protected: + + uint8_t instance; + GPS &front; + + class SIM *_sitl; + static void simulation_timeval(struct timeval *tv); }; diff --git a/libraries/SITL/SIM_InertialLabs.cpp b/libraries/SITL/SIM_InertialLabs.cpp new file mode 100644 index 0000000000000..728c7934f9cf7 --- /dev/null +++ b/libraries/SITL/SIM_InertialLabs.cpp @@ -0,0 +1,124 @@ +/* + 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 . + */ +/* + simulate InertialLabs external AHRS +*/ +#include "SIM_InertialLabs.h" +#include +#include "SIM_GPS.h" + +using namespace SITL; + +InertialLabs::InertialLabs() : SerialDevice::SerialDevice() +{ +} + +void InertialLabs::send_packet(void) +{ + const auto &fdm = _sitl->state; + + pkt.msg_len = sizeof(pkt)-2; + + pkt.accel_data_hr.x = (fdm.yAccel * 1.0e6)/GRAVITY_MSS; + pkt.accel_data_hr.y = (fdm.xAccel * 1.0e6)/GRAVITY_MSS; + pkt.accel_data_hr.z = (-fdm.zAccel * 1.0e6)/GRAVITY_MSS; + + pkt.gyro_data_hr.y = fdm.rollRate*1.0e5; + pkt.gyro_data_hr.x = fdm.pitchRate*1.0e5; + pkt.gyro_data_hr.z = -fdm.yawRate*1.0e5; + + float sigma, delta, theta; + AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta); + pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5; + pkt.baro_data.baro_alt = fdm.altitude; + pkt.temperature = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta); + + pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1; + pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS)*0.1; + pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS)*0.1; + + pkt.orientation_angles.roll = fdm.rollDeg*100; + pkt.orientation_angles.pitch = fdm.pitchDeg*100; + pkt.orientation_angles.yaw = fdm.yawDeg*100; + + pkt.velocity.x = fdm.speedE*100; + pkt.velocity.y = fdm.speedN*100; + pkt.velocity.z = -fdm.speedD*100; + + pkt.position.lat = fdm.latitude*1e7; + pkt.position.lon = fdm.longitude*1e7; + pkt.position.alt = fdm.altitude*1e2; + + pkt.kf_vel_covariance.x = 10; + pkt.kf_vel_covariance.z = 10; + pkt.kf_vel_covariance.z = 10; + + pkt.kf_pos_covariance.x = 20; + pkt.kf_pos_covariance.z = 20; + pkt.kf_pos_covariance.z = 20; + + const auto gps_tow = GPS_Backend::gps_time(); + + pkt.gps_ins_time_ms = gps_tow.ms; + + pkt.gnss_new_data = 0; + + if (packets_sent % gps_frequency == 0) { + // update GPS data at 5Hz + pkt.gps_week = gps_tow.week; + pkt.gnss_pos_timestamp = gps_tow.ms; + pkt.gnss_new_data = 3; + pkt.gps_position.lat = pkt.position.lat; + pkt.gps_position.lon = pkt.position.lon; + pkt.gps_position.alt = pkt.position.alt; + + pkt.num_sats = 32; + pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE)*100; + Vector2d track{fdm.speedN,fdm.speedE}; + pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle()))*100; + pkt.gnss_vel_track.ver_speed = -fdm.speedD*100; + + pkt.gnss_extended_info.fix_type = 2; + } + + pkt.differential_pressure = 0.5*sq(fdm.airspeed+fabsf(rand_float()*0.25))*1.0e4; + pkt.supply_voltage = 12.3*100; + pkt.temperature = 23.4*10; + + const uint8_t *buffer = (const uint8_t *)&pkt; + pkt.crc = crc_sum_of_bytes_16(&buffer[2], sizeof(pkt)-4); + + write_to_autopilot((char *)&pkt, sizeof(pkt)); + + packets_sent++; +} + + +/* + send InertialLabs data + */ +void InertialLabs::update(void) +{ + if (!init_sitl_pointer()) { + return; + } + const uint32_t us_between_packets = 5000; // 200Hz + const uint32_t now = AP_HAL::micros(); + if (now - last_pkt_us >= us_between_packets) { + last_pkt_us = now; + send_packet(); + } + +} diff --git a/libraries/SITL/SIM_InertialLabs.h b/libraries/SITL/SIM_InertialLabs.h new file mode 100644 index 0000000000000..448d5bf48bd19 --- /dev/null +++ b/libraries/SITL/SIM_InertialLabs.h @@ -0,0 +1,123 @@ +//usage: +//PARAMS: +// param set AHRS_EKF_TYPE 11 +// param set EAHRS_TYPE 5 +// param set SERIAL4_PROTOCOL 36 +// param set SERIAL4_BAUD 460800 +// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:ILabs +#pragma once + +#include "SIM_Aircraft.h" + +#include +#include "SIM_SerialDevice.h" + +namespace SITL +{ + +class InertialLabs : public SerialDevice +{ +public: + + InertialLabs(); + + // update state + void update(void); + +private: + void send_packet(void); + + struct PACKED vec3_16_t { + int16_t x,y,z; + }; + struct PACKED vec3_32_t { + int32_t x,y,z; + }; + struct PACKED vec3_u8_t { + uint8_t x,y,z; + }; + struct PACKED vec3_u16_t { + uint16_t 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; + }; + + struct PACKED ILabsPacket { + uint16_t magic = 0x55AA; + uint8_t msg_type = 1; + uint8_t msg_id = 0x95; + uint16_t msg_len; // total packet length-2 + + // send Table4, 27 messages + uint8_t num_messages = 27; + uint8_t messages[27] = { + 0x01, 0x3C, 0x23, 0x21, 0x25, 0x24, 0x07, 0x12, 0x10, 0x58, 0x57, 0x53, 0x4a, + 0x3b, 0x30, 0x32, 0x3e, 0x36, 0x41, 0xc0, 0x28, 0x86, 0x8a, 0x8d, 0x50, + 0x52, 0x5a + }; + uint32_t gps_ins_time_ms; // ms since start of GPS week for IMU data + uint16_t gps_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 { + int16_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; + uint16_t 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, AMSL + } gps_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; + uint16_t crc; + } pkt; + + uint32_t last_pkt_us; + const uint16_t pkt_rate_hz = 200; + const uint16_t gps_rate_hz = 10; + const uint16_t gps_frequency = pkt_rate_hz / gps_rate_hz; + uint32_t packets_sent; +}; + +} +