diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 78b9f23237..b4d8ae9e58 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -655,24 +655,5 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, } } -/** - default versions of multi-device accessor functions - */ -bool AP_InertialSensor::get_gyro_health(uint8_t instance) const -{ - if (instance != 0) { - return false; - } - return healthy(); -} - -bool AP_InertialSensor::get_accel_health(uint8_t instance) const -{ - if (instance != 0) { - return false; - } - return healthy(); -} - #endif // __AVR_ATmega1280__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 389de2fbfa..d19738fce6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -110,11 +110,11 @@ public: virtual void set_accel(const Vector3f &accel) {} // multi-device interface - virtual bool get_gyro_health(uint8_t instance) const; + virtual bool get_gyro_health(uint8_t instance) const { return true; } bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } virtual uint8_t get_gyro_count(void) const { return 1; }; - virtual bool get_accel_health(uint8_t instance) const; + virtual bool get_accel_health(uint8_t instance) const { return true; } bool get_accel_health(void) const { return get_accel_health(_get_primary_accel()); } virtual uint8_t get_accel_count(void) const { return 1; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 7bf4534763..56af873e11 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -32,6 +32,8 @@ public: uint16_t error_count(void) const { return _error_count; } bool healthy(void) const { return _error_count <= 4; } + bool get_gyro_health(uint8_t instance) const { return healthy(); } + bool get_accel_health(uint8_t instance) const { return healthy(); } protected: uint16_t _init_sensor( Sample_rate sample_rate );