AP_ExternalAHRS: Added Airspeed Aiding for AdNav Devices and fixed so…

…me bugs

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
 };
 

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;
 

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 <AP_Math/AP_Math.h>
 #include <AP_Baro/AP_Baro.h>
 #include <AP_Compass/AP_Compass.h>
@@ -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

libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h

@@ -19,20 +19,22 @@
 
 #pragma once
 
-#include "AP_ExternalAHRS_backend.h"
+#include "AP_ExternalAHRS_config.h"
 
 #if AP_EXTERNAL_AHRS_ADNAV_ENABLED
 
+#include "AP_ExternalAHRS_backend.h"
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_DAL/AP_DAL.h>
+
 #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)
 #define AN_START_SYSTEM_PACKETS 0
 #define AN_START_STATE_PACKETS 20
 #define AN_START_CONFIGURATION_PACKETS 180
 
-#include <GCS_MAVLink/GCS_MAVLink.h>
-#include <AP_Math/AP_Math.h>
-
 
 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();
 };
 

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
      */