Baro: add altitude sanity check

healthy flag made protected
healthy accessor fn added which also check latest calculated altitude
was ok

libraries/AP_Baro/AP_Baro.cpp

@@ -67,7 +67,7 @@ void AP_Baro::calibrate()
 
     {
         uint32_t tstart = hal.scheduler->millis();
-        while (ground_pressure == 0 || !healthy) {
+        while (ground_pressure == 0 || !_flags.healthy) {
             read();         // Get initial data from absolute pressure sensor
             if (hal.scheduler->millis() - tstart > 500) {
                 hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
@@ -89,7 +89,7 @@ void AP_Baro::calibrate()
                 hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
                         "for more than 500ms in AP_Baro::calibrate [2]\r\n"));
             }
-        } while (!healthy);
+        } while (!_flags.healthy);
         ground_pressure     = get_pressure();
         ground_temperature  = get_temperature();
 
@@ -106,7 +106,7 @@ void AP_Baro::calibrate()
                 hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
                         "for more than 500ms in AP_Baro::calibrate [3]\r\n"));
             }
-        } while (!healthy);
+        } while (!_flags.healthy);
         ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f);
         ground_temperature = (ground_temperature * 0.8f) + 
             (get_temperature() * 0.2f);
@@ -168,10 +168,19 @@ float AP_Baro::get_altitude(void)
     }
 
     float pressure = get_pressure();
-    _altitude = get_altitude_difference(_ground_pressure, pressure);
+    float alt = get_altitude_difference(_ground_pressure, pressure);
 
+    // record that we have consumed latest data
     _last_altitude_t = _last_update;
 
+    // sanity check altitude
+    if (isnan(alt) || isinf(alt)) {
+        _flags.alt_ok = false;
+    } else {
+        _altitude = alt;
+        _flags.alt_ok = true;
+    }
+
     // ensure the climb rate filter is updated
     _climb_rate_filter.update(_altitude, _last_update);
 

libraries/AP_Baro/AP_Baro.h

@@ -10,12 +10,17 @@
 class AP_Baro
 {
 public:
-    bool                    healthy;
-
     AP_Baro() {
+        // initialise flags
+        _flags.healthy = false;
+        _flags.alt_ok = false;
+
 		AP_Param::setup_object_defaults(this, var_info);
     }
 
+    // healthy - returns true if sensor and derived altitude are good
+    bool healthy() const { return _flags.healthy && _flags.alt_ok; }
+
     virtual bool            init()=0;
     virtual uint8_t         read() = 0;
 
@@ -76,6 +81,12 @@ public:
     static const struct AP_Param::GroupInfo        var_info[];
 
 protected:
+
+    struct Baro_flags {
+        uint8_t healthy :1;             // true if sensor is healthy
+        uint8_t alt_ok  :1;             // true if calculated altitude is ok
+    } _flags;
+
     uint32_t                            _last_update; // in ms
     uint8_t                             _pressure_samples;