Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added healthy check for PX4 and HIL

Browse Source 
used to detect bad accels
This commit is contained in:
Andrew Tridgell 2013-11-07 13:53:59 +11:00
parent 428479b9d5
commit f5299e2e11
9 changed files with 81 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -84,7 +84,7 @@ public:
/// @returns vector of rotational rates in radians/sec
///
const Vector3f &get_gyro(void) const { return _gyro; }
void set_gyro(Vector3f gyro) { _gyro = gyro; }
virtual void set_gyro(const Vector3f &gyro) {}
// set gyro offsets in radians/sec
Vector3f get_gyro_offsets(void) { return _gyro_offset; }
@ -95,7 +95,7 @@ public:
/// @returns vector of current accelerations in m/s/s
///
const Vector3f &get_accel(void) const { return _accel; }
void set_accel(Vector3f accel) { _accel = accel; }
virtual void set_accel(const Vector3f &accel) {}
// get accel offsets in m/s/s
Vector3f get_accel_offsets() { return _accel_offset; }
@ -171,6 +171,9 @@ protected:
// Most recent accelerometer reading obtained by ::update
Vector3f _accel;
// previous accelerometer reading obtained by ::update
Vector3f _previous_accel;
// Most recent gyro reading obtained by ::update
Vector3f _gyro;

2
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -188,6 +188,8 @@ bool AP_InertialSensor_Flymaple::update(void)
_delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
_last_update_usec = _last_gyro_timestamp;
_previous_accel = _accel;
_accel = _accel_filtered;
_accel_samples = 0;

40
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -5,9 +5,7 @@
const extern AP_HAL::HAL& hal;
AP_InertialSensor_HIL::AP_InertialSensor_HIL() : AP_InertialSensor() {
Vector3f accels;
accels.z = -GRAVITY_MSS;
set_accel(accels);
_accel.z = -GRAVITY_MSS;
}
uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) {
@ -65,3 +63,39 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
}
return false;
}
void AP_InertialSensor_HIL::set_accel(const Vector3f &accel)
{
_previous_accel = _accel;
_accel = accel;
_last_accel_usec = hal.scheduler->micros();
}
void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
{
_gyro = gyro;
_last_gyro_usec = hal.scheduler->micros();
}
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_HIL::healthy(void) const
{
uint32_t tnow = hal.scheduler->micros();
if ((tnow - _last_accel_usec) > 40000) {
// accels have not updated
return false;
}
if ((tnow - _last_gyro_usec) > 40000) {
// gyros have not updated
return false;
}
if (fabs(_accel.x) > 30 && fabs(_accel.y) > 30 && fabs(_accel.z) > 30 &&
(_previous_accel - _accel).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure
return false;
}
return true;
}

5
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -18,12 +18,17 @@ public:
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
void set_accel(const Vector3f &accel);
void set_gyro(const Vector3f &gyro);
bool healthy(void) const;
protected:
uint16_t _init_sensor( Sample_rate sample_rate );
uint32_t _sample_period_ms;
uint32_t _last_update_ms;
uint32_t _delta_time_usec;
uint32_t _last_accel_usec;
uint32_t _last_gyro_usec;
};
#endif // __AP_INERTIAL_SENSOR_STUB_H__

2
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -236,6 +236,8 @@ bool AP_InertialSensor_L3G4200D::update(void)
}
Vector3f accel_scale = _accel_scale.get();
_previous_accel = _accel;
hal.scheduler->suspend_timer_procs();
_accel = _accel_filtered;
_gyro = _gyro_filtered;

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -252,6 +252,8 @@ bool AP_InertialSensor_MPU6000::update( void )
return false;
}
_previous_accel = _accel;
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);

2
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -89,6 +89,8 @@ bool AP_InertialSensor_Oilpan::update()
_gyro.z *= _gyro_gain_z;
_gyro -= gyro_offset;
_previous_accel = _accel;
_accel = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
_sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));

25
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -90,6 +90,8 @@ bool AP_InertialSensor_PX4::update(void)
// get the latest sample from the sensor drivers
_get_sample();
_previous_accel = _accel;
_accel = _accel_in;
_gyro = _gyro_in;
@ -173,5 +175,28 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
return false;
}
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_PX4::healthy(void) const
{
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec) {
// accels have not updated
return false;
}
if ((tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
// gyros have not updated
return false;
}
if (fabs(_accel.x) > 30 && fabs(_accel.y) > 30 && fabs(_accel.z) > 30 &&
(_previous_accel - _accel).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure of the LSM303d
return false;
}
return true;
}
#endif // CONFIG_HAL_BOARD

1
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -25,6 +25,7 @@ public:
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
bool healthy(void) const;
private:
uint16_t _init_sensor( Sample_rate sample_rate );