From 856cf121c4281677e84c7901815f1238e7eeb288 Mon Sep 17 00:00:00 2001 From: Daniel Cook Date: Tue, 26 Mar 2024 13:42:33 +1100 Subject: [PATCH] AP_ExternalAHRS: Added Airspeed Aiding for AdNav Devices and fixed some bugs --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 10 ++- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 3 + .../AP_ExternalAHRS_AdvancedNavigation.cpp | 86 +++++++++++++++++-- .../AP_ExternalAHRS_AdvancedNavigation.h | 37 ++++++-- .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 7 ++ 5 files changed, 130 insertions(+), 13 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index dc8b1e0ea70029..691ddcb21b3b20 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -73,6 +73,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @DisplayName: External AHRS options // @Description: External AHRS options bitmask // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag. + // @Bitmask: 1:Provide airspeed aiding to Advanced Navigation device from airspeed sensors. // @User: Standard AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0), @@ -89,7 +90,14 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Units: Hz // @User: Standard AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10), - + + // @Param: _ARSP_ERR_20MS + // @DisplayName: Airspeed error at 20m/s + // @Description: Estimate of error in airspeed when vehicle travelling at 20m/s + // @Units: m/s + // @User: Advanced + AP_GROUPINFO("_ASER_20MS", 6, AP_ExternalAHRS, arsp_err_20ms, 1.0), + AP_GROUPEND }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index bc8f6fd84ad675..fbaa5e1eebec37 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -34,6 +34,7 @@ class AP_ExternalAHRS { public: friend class AP_ExternalAHRS_backend; friend class AP_ExternalAHRS_VectorNav; + friend class AP_ExternalAHRS_AdvancedNavigation; AP_ExternalAHRS(); @@ -175,6 +176,7 @@ class AP_ExternalAHRS { enum class OPTIONS { VN_UNCOMP_IMU = 1U << 0, + AN_ARSP_AID = 1U << 1, }; bool option_is_set(OPTIONS option) const { return (options.get() & int32_t(option)) != 0; } @@ -186,6 +188,7 @@ class AP_ExternalAHRS { AP_Int16 log_rate; AP_Int16 options; AP_Int16 sensors; + AP_Float arsp_err_20ms; static AP_ExternalAHRS *_singleton; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp index eeea1ecc5bd22c..30b0c84d8e46a9 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -18,10 +18,10 @@ #define ALLOW_DOUBLE_MATH_FUNCTIONS -#include "AP_ExternalAHRS_AdvancedNavigation.h" +#include "AP_ExternalAHRS_config.h" #if AP_EXTERNAL_AHRS_ADNAV_ENABLED - +#include "AP_ExternalAHRS_AdvancedNavigation.h" #include #include #include @@ -36,6 +36,7 @@ #define AN_TIMEOUT 5000 //ms #define AN_MAXIMUM_PACKET_PERIODS 50 #define AN_GNSS_PACKET_RATE 5 +#define AN_AIRSPEED_AIDING_FREQUENCY 20 #define an_packet_pointer(packet) packet->header #define an_packet_size(packet) (packet->length + AN_PACKET_HEADER_SIZE)*sizeof(uint8_t) @@ -136,7 +137,8 @@ int AP_ExternalAHRS_AdvancedNavigation_Decoder::decode_packet(uint8_t* out_buffe // constructor AP_ExternalAHRS_AdvancedNavigation::AP_ExternalAHRS_AdvancedNavigation(AP_ExternalAHRS *_frontend, AP_ExternalAHRS::state_t &_state) : - AP_ExternalAHRS_backend(_frontend, _state) + AP_ExternalAHRS_backend(_frontend, _state), + dal (AP::dal()) { auto &sm = AP::serialmanager(); _uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); @@ -161,7 +163,20 @@ AP_ExternalAHRS_AdvancedNavigation::AP_ExternalAHRS_AdvancedNavigation(AP_Extern AP_HAL::panic("Failed to start ExternalAHRS update thread"); } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised"); + uint32_t tstart = AP_HAL::millis(); + + while (!_last_device_info_pkt_ms) + { + const uint32_t tnow = AP_HAL::millis(); + if (tnow - tstart >= AN_TIMEOUT) + { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Advanced Navigation Device Unresponsive"); + tstart = tnow; + } + hal.scheduler->delay(50); + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised: %s", get_name()); } void AP_ExternalAHRS_AdvancedNavigation::update() @@ -182,6 +197,13 @@ void AP_ExternalAHRS_AdvancedNavigation::update_thread(void) // Sleep the scheduler hal.scheduler->delay_microseconds(1000); + // Send Aiding data to the device + if (option_is_set(AP_ExternalAHRS::OPTIONS::AN_ARSP_AID)) { + if (!send_airspeed_aiding()) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ExternalAHRS: Unable to send airspeed aiding."); + } + } + // Collect the requested packets from the UART manager if (!get_packets()) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Error Receiving Packets"); @@ -413,6 +435,7 @@ void AP_ExternalAHRS_AdvancedNavigation::get_filter_status(nav_filter_status &st status.flags.vert_pos = true; status.flags.attitude = true; + status.flags.vert_vel = true; if (_device_status.filter.b.gnss_fix_type > gnss_fix_none) { status.flags.horiz_vel = true; @@ -425,8 +448,6 @@ void AP_ExternalAHRS_AdvancedNavigation::get_filter_status(nav_filter_status &st if (_device_status.filter.b.gnss_fix_type > gnss_fix_2d) { status.flags.gps_quality_good = true; - status.flags.vert_pos = true; - status.flags.vert_vel = true; } } @@ -623,6 +644,59 @@ bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(const AN_FILTER_OPTI return true; } +// Returns true on successful transmit of packet or if called prior to requirement of sending packet. +bool AP_ExternalAHRS_AdvancedNavigation::send_airspeed_aiding(){ + + uint32_t now = AP_HAL::millis(); + const AP_DAL_Airspeed *arsp = dal.airspeed(); + const AP_DAL_Baro &bar = dal.baro(); + uint32_t period_airspeed_aiding_ms = 1000/AN_AIRSPEED_AIDING_FREQUENCY; + if (now < _last_ext_air_data_sent_ms + period_airspeed_aiding_ms) { + return true; + } + + if (arsp == nullptr || !arsp->healthy() || !bar.healthy(bar.get_primary())) { + return false; + } + + AN_PACKET packet; + float airspeed = arsp->get_airspeed(); + + _last_ext_air_data_sent_ms = now; + + packet.payload.ext_air_data.true_airspeed = airspeed * dal.get_EAS2TAS(); + packet.payload.ext_air_data.airspeed_delay = (AP_HAL::millis() - arsp->last_update_ms()) / 1000; + packet.payload.ext_air_data.airspeed_standard_deviation = get_airspeed_error(airspeed); + packet.payload.ext_air_data.barometric_altitude = bar.get_altitude(); + packet.payload.ext_air_data.baro_delay = (AP_HAL::millis() - bar.get_last_update()) / 1000; + packet.payload.ext_air_data.barometric_standard_deviation = get_pressure_error(); + packet.payload.ext_air_data.flags.b.airspeed_set_valid = true; + packet.payload.ext_air_data.flags.b.barometric_altitude_set_valid = true; + packet.update_checks(packet_id_external_air_data, sizeof(packet.payload.ext_air_data)); + + // Check for space in the tx buffer + if (_uart->txspace() < packet.packet_size()) { + return false; + } + _uart->write(packet.raw_pointer(), packet.packet_size()); + + return true; +} + +// Method to return estimate of airspeed error based upon the current airspeed given error at 20m/s. +float AP_ExternalAHRS_AdvancedNavigation::get_airspeed_error(float airspeed) +{ + float nominal_pressure = 0.5 * sq(airspeed); + float adj_airspeed = sqrt(2 * nominal_pressure + get_pressure_error()); + return adj_airspeed - airspeed; +} + +// Method to give an estimate of the pressure error from the parameter for airspeed error at 20m/s +float AP_ExternalAHRS_AdvancedNavigation::get_pressure_error(void) +{ + return (0.5*(sq(20+airspeed_err_20ms()))) - (0.5*sq(20)); +} + void AP_ExternalAHRS_AdvancedNavigation::handle_packet() { // get current time diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h index 7c166ffca2bac1..3f004367e7eb3a 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h @@ -19,10 +19,15 @@ #pragma once -#include "AP_ExternalAHRS_backend.h" +#include "AP_ExternalAHRS_config.h" #if AP_EXTERNAL_AHRS_ADNAV_ENABLED +#include "AP_ExternalAHRS_backend.h" +#include +#include +#include + #define AN_PACKET_HEADER_SIZE 5 #define AN_MAXIMUM_PACKET_SIZE 255 #define AN_DECODE_BUFFER_SIZE 2*(AN_MAXIMUM_PACKET_SIZE+AN_PACKET_HEADER_SIZE) @@ -30,9 +35,6 @@ #define AN_START_STATE_PACKETS 20 #define AN_START_CONFIGURATION_PACKETS 180 -#include -#include - class AP_ExternalAHRS_AdvancedNavigation_Decoder { @@ -107,6 +109,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend private: AP_ExternalAHRS_AdvancedNavigation_Decoder _decoder; + AP_DAL &dal; typedef enum { packet_id_acknowledge, @@ -246,7 +249,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend device_id_air_data_unit, device_id_spatial_fog_dual = 16, device_id_motus, - device_id_gnss_compass, + device_id_gnss_compass = 19, device_id_certus = 26, device_id_aries, device_id_boreas_d90, @@ -274,7 +277,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend vehicle_type_race_car } vehicle_type_e; - struct PACKED AN_ACKNOWLEGE { + struct PACKED AN_ACKNOWLEDGE { uint8_t id_acknowledged; uint16_t crc_acknowledged; uint8_t result; @@ -391,6 +394,23 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend uint8_t sbas_satellites; } *_last_satellites; + struct PACKED AN_EXTERNAL_AIR_DATA { + float baro_delay; + float airspeed_delay; + float barometric_altitude; + float true_airspeed; + float barometric_standard_deviation; + float airspeed_standard_deviation; + union { + uint8_t r; + struct{ + uint8_t barometric_altitude_set_valid :1; + uint8_t airspeed_set_valid :1; + uint8_t barometric_altitude_reference_reset :1; + }b; + } flags; + }; + struct PACKED AN_PERIOD { uint8_t id; uint32_t packet_period; @@ -432,6 +452,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend AN_RAW_SENSORS raw_sensors; AN_RAW_GNSS raw_gnss; AN_SATELLITES satellites; + AN_EXTERNAL_AIR_DATA ext_air_data; AN_PACKET_PERIODS packet_periods; AN_FILTER_OPTIONS filter_options; } payload; @@ -492,6 +513,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend uint32_t _last_state_pkt_ms; uint32_t _last_device_info_pkt_ms; uint32_t _last_raw_gnss_pkt_ms; + uint32_t _last_ext_air_data_sent_ms; uint32_t _device_id; uint32_t _hardware_rev; @@ -506,6 +528,9 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend bool get_baro_capability(void) const; bool set_filter_options(bool gnss_en, vehicle_type_e vehicle_type, bool permanent = false); bool set_filter_options(AN_FILTER_OPTIONS options_packet); + bool send_airspeed_aiding(void); + float get_airspeed_error(float airspeed); + float get_pressure_error(void); void handle_packet(); }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index ed987a01ced3aa..a476f507a87520 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -58,6 +58,13 @@ class AP_ExternalAHRS_backend { frontend.set_default_sensors(sensors); } + /* + return the airspeed error at 20ms + */ + float airspeed_err_20ms(void) const { + return frontend.arsp_err_20ms; + } + /* return true if the GNSS is disabled */