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) {