From f10a4b04ae72ac4f4ade3a669b6f1bf8d1845482 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 10 Nov 2013 15:35:28 +1100
Subject: [PATCH] AP_InertialSensor: make PX4 healthy call _get_sample()

this prevents a false positive during times like arming where we are
not reading the sensors
---
 .../AP_InertialSensor/AP_InertialSensor.h     |  2 +-
 .../AP_InertialSensor_HIL.cpp                 |  2 +-
 .../AP_InertialSensor/AP_InertialSensor_HIL.h |  2 +-
 .../AP_InertialSensor_MPU6000.h               |  2 +-
 .../AP_InertialSensor_PX4.cpp                 | 19 ++++++++++++++++++-
 .../AP_InertialSensor/AP_InertialSensor_PX4.h |  9 +++++++--
 6 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 7967886d0a..95946ebced 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/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:
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
index c29d7bd4c7..faa1381673 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
+++ b/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) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
index 37f47d1d71..5870ce0344 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
+++ b/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 );
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index e9c7dbc6ea..a164be13d0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/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 );
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index ed213aec24..5ae52020ec 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/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;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
index 8cefa991e9..5a39f3d3cf 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
+++ b/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 );