AP_InertialSensor: make PX4 healthy call _get_sample()

this prevents a false positive during times like arming where we are
not reading the sensors

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -140,7 +140,7 @@ public:
     }
 
     virtual uint16_t error_count(void) const { return 0; }
-    virtual bool healthy(void) const { return true; }
+    virtual bool healthy(void) { return true; }
 
 protected:
 

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -80,7 +80,7 @@ void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
 /**
    try to detect bad accel/gyro sensors
  */
-bool AP_InertialSensor_HIL::healthy(void) const
+bool AP_InertialSensor_HIL::healthy(void)
 {
     uint32_t tnow = hal.scheduler->micros();
     if ((tnow - _last_accel_usec) > 40000) {

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -20,7 +20,7 @@ public:
     bool            wait_for_sample(uint16_t timeout_ms);
     void            set_accel(const Vector3f &accel);
     void            set_gyro(const Vector3f &gyro);
-    bool            healthy(void) const;
+    bool            healthy(void);
 
 protected:
     uint16_t        _init_sensor( Sample_rate sample_rate );

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -34,7 +34,7 @@ public:
     float            	get_delta_time();
 
     uint16_t error_count(void) const { return _error_count; }
-    bool healthy(void) const { return _error_count <= 4; }
+    bool healthy(void) { return _error_count <= 4; }
 
 protected:
     uint16_t                    _init_sensor( Sample_rate sample_rate );

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -131,6 +131,10 @@ void AP_InertialSensor_PX4::_get_sample(void)
     struct accel_report	accel_report;
     struct gyro_report	gyro_report;
 
+    if (_accel_fd == -1 || _gyro_fd == -1) {
+        return;
+    }
+
     while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
         accel_report.timestamp != _last_accel_timestamp) {        
         _accel_in = Vector3f(accel_report.x, accel_report.y, accel_report.z);
@@ -178,9 +182,22 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
 /**
    try to detect bad accel/gyro sensors
  */
-bool AP_InertialSensor_PX4::healthy(void) const
+bool AP_InertialSensor_PX4::healthy(void)
 {
+    if (_sample_time_usec == 0) {
+        // not initialised yet, show as healthy to prevent scary GCS
+        // warnings
+        return true;
+    }
     uint64_t tnow = hrt_absolute_time();
+
+    if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec ||
+        (tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
+        // see if new samples are available
+        _get_sample();
+        tnow = hrt_absolute_time();
+    }
+
     if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec) {
         // accels have not updated
         return false;

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -17,7 +17,12 @@ class AP_InertialSensor_PX4 : public AP_InertialSensor
 {
 public:
 
-    AP_InertialSensor_PX4() : AP_InertialSensor() {}
+    AP_InertialSensor_PX4() : 
+        AP_InertialSensor(),
+        _sample_time_usec(0),
+        _accel_fd(-1),
+        _gyro_fd(-1)
+        {}
 
     /* Concrete implementation of AP_InertialSensor functions: */
     bool            update();
@@ -25,7 +30,7 @@ public:
     float           get_gyro_drift_rate();
     bool            sample_available();
     bool            wait_for_sample(uint16_t timeout_ms);
-    bool            healthy(void) const;
+    bool            healthy(void);
 
 private:
     uint16_t _init_sensor( Sample_rate sample_rate );