From 99e2dc87c9662f697a3d40e563dd817b5068a98e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 13 Dec 2013 20:47:24 +1100
Subject: [PATCH] AP_InertialSensor: prevent INS healthy errors while
 initialising

during gyro cal we shouldn't mark the gyro unhealthy
---
 .../AP_InertialSensor_PX4.cpp                  | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index 29be4230fe..2cb3c162d5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -15,6 +15,8 @@ const extern AP_HAL::HAL& hal;
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_hrt.h>
 
+#include <AP_Notify.h>
+
 uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
 {
     // assumes max 2 instances
@@ -78,14 +80,14 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
 // multi-device interface
 bool AP_InertialSensor_PX4::get_gyro_health(uint8_t instance) const
 {
-    if (instance >= _num_gyro_instances) {
-        return false;
-    }
-    if (_sample_time_usec == 0) {
+    if (_sample_time_usec == 0 || AP_Notify::flags.initialising) {
         // not initialised yet, show as healthy to prevent scary GCS
         // warnings
         return true;
     }
+    if (instance >= _num_gyro_instances) {
+        return false;
+    }
     uint64_t tnow = hrt_absolute_time();
 
     if ((tnow - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) {
@@ -102,14 +104,14 @@ uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const
 
 bool AP_InertialSensor_PX4::get_accel_health(uint8_t k) const
 {
-    if (k >= _num_accel_instances) {
-        return false;
-    }
-    if (_sample_time_usec == 0) {
+    if (_sample_time_usec == 0 || AP_Notify::flags.initialising) {
         // not initialised yet, show as healthy to prevent scary GCS
         // warnings
         return true;
     }
+    if (k >= _num_accel_instances) {
+        return false;
+    }
     uint64_t tnow = hrt_absolute_time();
 
     if ((tnow - _last_accel_timestamp[k]) > 2*_sample_time_usec) {