Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: implement gyro and accel health monitoring

Browse Source 
sensor is healthy if it gave a sample on the last update()
This commit is contained in:
Andrew Tridgell 2014-10-16 13:27:22 +11:00
parent 586fa9a816
commit 3a9a5a9c18
11 changed files with 47 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -926,11 +926,31 @@ void AP_InertialSensor::update(void)
wait_for_sample();
if (!_hil_mode) {
for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
// mark sensors unhealthy and let update() in each backend
// mark them healthy via _rotate_and_offset_gyro() and
// _rotate_and_offset_accel()
_gyro_healthy[i] = false;
_accel_healthy[i] = false;
}
for (int8_t i=0; i<INS_MAX_BACKENDS; i++) {
if (_backends[i] != NULL) {
_backends[i]->update();
}
}
}
// set primary to first healthy accel and gyro
for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i]) {
_primary_gyro = i;
break;
}
}
for (int8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i]) {
_primary_accel = i;
break;
}
}
}
_have_sample = false;

12
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -125,14 +125,14 @@ public:
void set_accel(uint8_t instance, const Vector3f &accel);
// multi-device interface
bool get_gyro_health(uint8_t instance) const { return true; }
bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; }
bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
bool get_gyro_health_all(void) const;
uint8_t get_gyro_count(void) const { return _gyro_count; }
bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
bool gyro_calibrated_ok_all() const;
bool get_accel_health(uint8_t instance) const { return true; }
bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; }
bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
bool get_accel_health_all(void) const;
uint8_t get_accel_count(void) const { return _accel_count; };
@ -226,10 +226,6 @@ private:
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];
// timestamp of latest gyro and accel readings
uint32_t _last_gyro_sample_time_usec[INS_MAX_INSTANCES];
uint32_t _last_accel_sample_time_usec[INS_MAX_INSTANCES];
// product id
AP_Int16 _product_id;
@ -269,6 +265,10 @@ private:
// time between samples in microseconds
uint32_t _sample_period_usec;
// health of gyros and accels
bool _gyro_healthy[INS_MAX_INSTANCES];
bool _accel_healthy[INS_MAX_INSTANCES];
};
#include "AP_InertialSensor_Backend.h"

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -12,18 +12,18 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
/*
rotate gyro vector and add the gyro offset
*/
void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now)
void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro)
{
_imu._gyro[instance] = gyro;
_imu._gyro[instance].rotate(_imu._board_orientation);
_imu._gyro[instance] -= _imu._gyro_offset[instance];
_imu._last_gyro_sample_time_usec[instance] = now;
_imu._gyro_healthy[instance] = true;
}
/*
rotate accel vector, scale and add the accel offset
*/
void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now)
void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel)
{
_imu._accel[instance] = accel;
_imu._accel[instance].rotate(_imu._board_orientation);
@ -33,5 +33,5 @@ void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const
_imu._accel[instance].y *= accel_scale.y;
_imu._accel[instance].z *= accel_scale.z;
_imu._accel[instance] -= _imu._accel_offset[instance];
_imu._last_accel_sample_time_usec[instance] = now;
_imu._accel_healthy[instance] = true;
}

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -58,10 +58,10 @@ protected:
AP_InertialSensor &_imu;
// rotate gyro vector and offset
void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now);
void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
// rotate accel vector, scale and offset
void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now);
void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel);
// backend should fill in its product ID from AP_PRODUCT_ID_*
int16_t _product_id;

5
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -198,7 +198,6 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
bool AP_InertialSensor_Flymaple::update(void)
{
Vector3f accel, gyro;
uint32_t now = hal.scheduler->micros();
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
@ -209,11 +208,11 @@ bool AP_InertialSensor_Flymaple::update(void)
// Adjust for chip scaling to get m/s/s
accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel, now);
_rotate_and_offset_accel(_accel_instance, accel);
// Adjust for chip scaling to get radians/sec
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
_rotate_and_offset_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());

5
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -265,7 +265,6 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
bool AP_InertialSensor_L3G4200D::update(void)
{
Vector3f accel, gyro;
uint32_t now = hal.scheduler->micros();
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
@ -276,11 +275,11 @@ bool AP_InertialSensor_L3G4200D::update(void)
// Adjust for chip scaling to get m/s/s
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_offset_accel(_accel_instance, accel, now);
_rotate_and_offset_accel(_accel_instance, accel);
// Adjust for chip scaling to get radians/sec
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
_rotate_and_offset_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -271,7 +271,6 @@ bool AP_InertialSensor_MPU6000::update( void )
// we have a full set of samples
uint16_t num_samples;
uint32_t now = hal.scheduler->micros();
Vector3f accel, gyro;
hal.scheduler->suspend_timer_procs();
@ -284,10 +283,10 @@ bool AP_InertialSensor_MPU6000::update( void )
hal.scheduler->resume_timer_procs();
gyro *= _gyro_scale / num_samples;
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
_rotate_and_offset_gyro(_gyro_instance, gyro);
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
_rotate_and_offset_accel(_accel_instance, accel, now);
_rotate_and_offset_accel(_accel_instance, accel);
if (_last_filter_hz != _imu.get_filter()) {
if (_spi_sem->take(10)) {

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -1125,7 +1125,6 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
bool AP_InertialSensor_MPU9150::update(void)
{
Vector3f accel, gyro;
uint32_t now = hal.scheduler->micros();
hal.scheduler->suspend_timer_procs();
accel = _accel_filtered;
@ -1134,10 +1133,10 @@ bool AP_InertialSensor_MPU9150::update(void)
hal.scheduler->resume_timer_procs();
accel *= MPU9150_ACCEL_SCALE_2G;
_rotate_and_offset_accel(_accel_instance, accel, now);
_rotate_and_offset_accel(_accel_instance, accel);
gyro *= MPU9150_GYRO_SCALE_2000;
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
_rotate_and_offset_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -266,8 +266,6 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate samp
*/
bool AP_InertialSensor_MPU9250::update( void )
{
uint32_t now = hal.scheduler->micros();
// pull the data from the timer shared data buffer
uint8_t idx = _shared_data_idx;
Vector3f gyro = _shared_data[idx]._gyro_filtered;
@ -288,8 +286,8 @@ bool AP_InertialSensor_MPU9250::update( void )
gyro.rotate(ROTATION_YAW_180);
#endif
_rotate_and_offset_gyro(_gyro_instance, gyro, now);
_rotate_and_offset_accel(_accel_instance, accel, now);
_rotate_and_offset_gyro(_gyro_instance, gyro);
_rotate_and_offset_accel(_accel_instance, accel);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter(_imu.get_filter());

5
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -97,7 +97,6 @@ bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sampl
bool AP_InertialSensor_Oilpan::update()
{
float adc_values[6];
uint32_t now = hal.scheduler->micros();
apm1_adc.Ch6(_sensors, adc_values);
@ -106,14 +105,14 @@ bool AP_InertialSensor_Oilpan::update()
v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
_sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
_sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
_rotate_and_offset_gyro(_gyro_instance, v, now);
_rotate_and_offset_gyro(_gyro_instance, v);
// copy accels to frontend
v(_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));
v *= OILPAN_ACCEL_SCALE_1G;
_rotate_and_offset_accel(_accel_instance, v, now);
_rotate_and_offset_accel(_accel_instance, v);
return true;
}

6
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -124,17 +124,15 @@ bool AP_InertialSensor_PX4::update(void)
// get the latest sample from the sensor drivers
_get_sample();
uint32_t now = hal.scheduler->micros();
for (uint8_t k=0; k<_num_accel_instances; k++) {
Vector3f accel = _accel_in[k];
_rotate_and_offset_accel(_accel_instance[k], accel, now);
_rotate_and_offset_accel(_accel_instance[k], accel);
_last_accel_update_timestamp[k] = _last_accel_timestamp[k];
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
Vector3f gyro = _gyro_in[k];
_rotate_and_offset_gyro(_gyro_instance[k], gyro, now);
_rotate_and_offset_gyro(_gyro_instance[k], gyro);
_last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
}