AP_Baro: make backends responsible for setting their health

This resolves a problem in SITL where the barometer would
not change value for prolonged period of time, making it
go unhealthy

libraries/AP_Baro/AP_Baro.cpp

@@ -512,17 +512,10 @@ void AP_Baro::update(void)
 
     if (!_hil_mode) {
         for (uint8_t i=0; i<_num_drivers; i++) {
-            drivers[i]->update();
+            drivers[i]->backend_update(i);
         }
     }
 
-    // consider a sensor as healthy if it has had an update in the
-    // last 0.5 seconds and values are non-zero and have changed within the last 2 seconds
-    uint32_t now = AP_HAL::millis();
-    for (uint8_t i=0; i<_num_sensors; i++) {
-        sensors[i].healthy = (now - sensors[i].last_update_ms < BARO_TIMEOUT_MS) && (now - sensors[i].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) && !is_zero(sensors[i].pressure);
-    }
-
     for (uint8_t i=0; i<_num_sensors; i++) {
         if (sensors[i].healthy) {
             // update altitude calculation

libraries/AP_Baro/AP_Baro.h

@@ -21,6 +21,7 @@ class AP_Baro_Backend;
 class AP_Baro
 {
     friend class AP_Baro_Backend;
+    friend class AP_Baro_SITL; // for access to sensors[]
 
 public:
     // constructor

libraries/AP_Baro/AP_Baro_Backend.cpp

@@ -9,6 +9,33 @@ AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) :
     _sem = hal.util->new_semaphore();    
 }
 
+void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
+{
+    if (instance >= _frontend._num_sensors) {
+        return;
+    }
+    if (!_sem->take_nonblocking()) {
+        return;
+    }
+
+    // consider a sensor as healthy if it has had an update in the
+    // last 0.5 seconds and values are non-zero and have changed within the last 2 seconds
+    const uint32_t now = AP_HAL::millis();
+    _frontend.sensors[instance].healthy =
+        (now - _frontend.sensors[instance].last_update_ms < BARO_TIMEOUT_MS) &&
+        (now - _frontend.sensors[instance].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) &&
+        !is_zero(_frontend.sensors[instance].pressure);
+
+    _sem->give();
+}
+
+void AP_Baro_Backend::backend_update(uint8_t instance)
+{
+    update();
+    update_healthy_flag(instance);
+}
+
+
 /*
   copy latest data to the frontend from a backend
  */

libraries/AP_Baro/AP_Baro_Backend.h

@@ -20,6 +20,8 @@ public:
     // callback for UAVCAN messages
     virtual void handle_baro_msg(float pressure, float temperature) {}
 
+    void backend_update(uint8_t instance);
+
 protected:
     // reference to frontend object
     AP_Baro &_frontend;
@@ -28,4 +30,7 @@ protected:
 
     // semaphore for access to shared frontend data
     AP_HAL::Semaphore *_sem;    
+
+    virtual void update_healthy_flag(uint8_t instance);
+
 };

libraries/AP_Baro/AP_Baro_SITL.h

@@ -12,6 +12,10 @@ public:
 
     void update() override;
 
+protected:
+
+    void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = true; }
+
 private:
     uint8_t instance;
     SITL::SITL *sitl;