AP_GPS: Support uavcan ardupilot.gnss.Status

Also makes the drive by change to make GPS_Backend::is_configured const

libraries/AP_GPS/AP_GPS.cpp

@@ -1917,6 +1917,15 @@ uint32_t AP_GPS::get_itow(uint8_t instance) const
     return drivers[instance]->get_last_itow();
 }
 
+bool AP_GPS::get_error_codes(uint8_t instance, uint32_t &error_codes) const
+{
+    if (instance >= GPS_MAX_RECEIVERS || drivers[instance] == nullptr) {
+        return false;
+    }
+
+    return drivers[instance]->get_error_codes(error_codes);
+}
+
 // Logging support:
 // Write an GPS packet
 void AP_GPS::Write_GPS(uint8_t i)

libraries/AP_GPS/AP_GPS.h

@@ -525,6 +525,9 @@ public:
     // get iTOW, if supported, zero otherwie
     uint32_t get_itow(uint8_t instance) const;
 
+    bool get_error_codes(uint8_t instance, uint32_t &error_codes) const;
+    bool get_error_codes(uint32_t &error_codes) const { return get_error_codes(primary_instance, error_codes); }
+
 protected:
 
     // configuration parameters

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -535,7 +535,7 @@ void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
     }
 }
 
-bool AP_GPS_SBF::is_configured (void) {
+bool AP_GPS_SBF::is_configured (void) const {
     return (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
              _init_blob_index >= ARRAY_SIZE(_initialisation_blob));
 }

libraries/AP_GPS/AP_GPS_SBF.h

@@ -39,7 +39,7 @@ public:
 
     const char *name() const override { return "SBF"; }
 
-    bool is_configured (void) override;
+    bool is_configured (void) const override;
 
     void broadcast_configuration_failure_reason(void) const override;
 
@@ -54,6 +54,7 @@ public:
 
     bool prepare_for_arming(void) override;
 
+    bool get_error_codes(uint32_t &error_codes) const override { error_codes = RxError; return true; };
 
 private:
 

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -24,10 +24,13 @@
 #include <AP_CANManager/AP_CANManager.h>
 #include <AP_UAVCAN/AP_UAVCAN.h>
 
+#include <AP_Logger/AP_Logger.h>
+
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
 #include <ardupilot/gnss/Heading.hpp>
+#include <ardupilot/gnss/Status.hpp>
 
 extern const AP_HAL::HAL& hal;
 
@@ -37,6 +40,7 @@ UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix);
 UC_REGISTRY_BINDER(Fix2Cb, uavcan::equipment::gnss::Fix2);
 UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary);
 UC_REGISTRY_BINDER(HeadingCb, ardupilot::gnss::Heading);
+UC_REGISTRY_BINDER(StatusCb, ardupilot::gnss::Status);
 
 AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
 HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
@@ -92,6 +96,14 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
         AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
         return;
     }
+
+    uavcan::Subscriber<ardupilot::gnss::Status, StatusCb> *gnss_status;
+    gnss_status = new uavcan::Subscriber<ardupilot::gnss::Status, StatusCb>(*node);
+    const int gnss_status_start_res = gnss_status->start(StatusCb(ap_uavcan, &handle_status_msg_trampoline));
+    if (gnss_status_start_res < 0) {
+        AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
+        return;
+    }
 }
 
 AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
@@ -415,6 +427,23 @@ void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb)
     interim_state.gps_yaw_accuracy = degrees(cb.msg->heading_accuracy_rad);
 }
 
+void AP_GPS_UAVCAN::handle_status_msg(const StatusCb &cb)
+{
+    WITH_SEMAPHORE(sem);
+
+    seen_status = true;
+
+    healthy = cb.msg->healthy;
+    status_flags = cb.msg->status;
+    if (error_code != cb.msg->error_codes) {
+        AP::logger().Write_MessageF("GPS %d: error changed (0x%08x/0x%08x)",
+                                    (unsigned int)(state.instance + 1),
+                                    error_code,
+                                    cb.msg->error_codes);
+        error_code = cb.msg->error_codes;
+    }
+}
+
 void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb)
 {
     WITH_SEMAPHORE(_sem_registry);
@@ -455,6 +484,16 @@ void AP_GPS_UAVCAN::handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t
     }
 }
 
+void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb)
+{
+    WITH_SEMAPHORE(_sem_registry);
+
+    AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
+    if (driver != nullptr) {
+        driver->handle_status_msg(cb);
+    }
+}
+
 // Consume new data and mark it received
 bool AP_GPS_UAVCAN::read(void)
 {
@@ -480,6 +519,35 @@ bool AP_GPS_UAVCAN::read(void)
     return false;
 }
 
+bool AP_GPS_UAVCAN::is_healthy(void) const
+{
+    // if we don't have any health reports, assume it's healthy
+    if (!seen_status) {
+        return true;
+    }
+    return healthy;
+}
+
+bool AP_GPS_UAVCAN::logging_healthy(void) const
+{
+    // if we don't have status, assume it's valid
+    if (!seen_status) {
+        return true;
+    }
+
+    return (status_flags & ardupilot::gnss::Status::STATUS_LOGGING) != 0;
+}
+
+bool AP_GPS_UAVCAN::is_configured(void) const
+{
+    // if we don't have status assume it's configured
+    if (!seen_status) {
+        return true;
+    }
+
+    return (status_flags & ardupilot::gnss::Status::STATUS_ARMABLE) != 0;
+}
+
 /*
   handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink
  */

libraries/AP_GPS/AP_GPS_UAVCAN.h

@@ -29,6 +29,7 @@ class FixCb;
 class Fix2Cb;
 class AuxCb;
 class HeadingCb;
+class StatusCb;
 
 class AP_GPS_UAVCAN : public AP_GPS_Backend {
 public:
@@ -37,6 +38,12 @@ public:
 
     bool read() override;
 
+    bool is_healthy(void) const override;
+
+    bool logging_healthy(void) const override;
+
+    bool is_configured(void) const override;
+
     const char *name() const override { return "UAVCAN"; }
 
     static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
@@ -46,14 +53,18 @@ public:
     static void handle_fix2_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Fix2Cb &cb);
     static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
     static void handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb);
+    static void handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb);
 
     void inject_data(const uint8_t *data, uint16_t len) override;
 
+    bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; };
+
 private:
     void handle_fix_msg(const FixCb &cb);
     void handle_fix2_msg(const Fix2Cb &cb);
     void handle_aux_msg(const AuxCb &cb);
     void handle_heading_msg(const HeadingCb &cb);
+    void handle_status_msg(const StatusCb &cb);
 
     static bool take_registry();
     static void give_registry();
@@ -68,6 +79,11 @@ private:
     bool seen_message;
     bool seen_fix2;
     bool seen_aux;
+    bool seen_status;
+
+    bool healthy;
+    uint32_t status_flags;
+    uint32_t error_code;
 
     // Module Detection Registry
     static struct DetectedModules {

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -117,7 +117,7 @@ public:
 
     static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
 
-    bool is_configured(void) override {
+    bool is_configured(void) const override {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
         if (!gps._auto_config) {
             return true;

libraries/AP_GPS/GPS_Backend.h

@@ -40,7 +40,7 @@ public:
     // Allows external system to identify type of receiver connected.
     virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }
 
-    virtual bool is_configured(void) { return true; }
+    virtual bool is_configured(void) const { return true; }
 
     virtual void inject_data(const uint8_t *data, uint16_t len);
 
@@ -77,6 +77,8 @@ public:
     virtual bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { return false; }
     virtual void clear_RTCMV3(void) {};
 
+    virtual bool get_error_codes(uint32_t &error_codes) const { return false; }
+
     // return iTOW of last message, or zero if not supported
     uint32_t get_last_itow(void) const {
         return _last_itow;