aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: prevent INS healthy errors while initialising

Browse Source 
during gyro cal we shouldn't mark the gyro unhealthy
This commit is contained in:
Andrew Tridgell 2013-12-13 20:47:24 +11:00
parent 5ca73785c8
commit 99e2dc87c9
1 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

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