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

AP_InertialSensor: zero accumulators on time step

Browse Source 
this zeros the delta angle and delta velocity accumulators when a
sensor is unavailable for a period of 0.1s. This prevents garbage
values being passed into the EKF when a sensor dies and then becomes
available again some time later
This commit is contained in:
Andrew Tridgell 2019-07-05 11:51:30 +10:00
parent 3444e82ef9
commit 09d3392aee
1 changed files with 29 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -155,6 +155,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance], _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
_imu._gyro_raw_sample_rates[instance]); _imu._gyro_raw_sample_rates[instance]);
uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
/* /*
we have two classes of sensors. FIFO based sensors produce data we have two classes of sensors. FIFO based sensors produce data
at a very predictable overall rate, but the data comes in at a very predictable overall rate, but the data comes in
@ -165,6 +167,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
*/ */
if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) { if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f; dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f;
_imu._gyro_last_sample_us[instance] = sample_us;
} else { } else {
// don't accept below 100Hz // don't accept below 100Hz
if (_imu._gyro_raw_sample_rates[instance] < 100) { if (_imu._gyro_raw_sample_rates[instance] < 100) {
@ -172,8 +175,8 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
} }
dt = 1.0f / _imu._gyro_raw_sample_rates[instance]; dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
_imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
} }
_imu._gyro_last_sample_us[instance] = sample_us;
#if AP_MODULE_SUPPORTED #if AP_MODULE_SUPPORTED
// call gyro_sample hook if any // call gyro_sample hook if any
@ -181,8 +184,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
#endif #endif
// push gyros if optical flow present // push gyros if optical flow present
if (hal.opticalflow) if (hal.opticalflow) {
hal.opticalflow->push_gyro(gyro.x, gyro.y, dt); hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
}
// compute delta angle // compute delta angle
Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt; Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
@ -199,6 +203,16 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
{ {
WITH_SEMAPHORE(_sem); WITH_SEMAPHORE(_sem);
uint64_t now = AP_HAL::micros64();
if (now - last_sample_us > 100000U) {
// zero accumulator if sensor was unhealthy for 0.1s
_imu._delta_angle_acc[instance].zero();
_imu._delta_angle_acc_dt[instance] = 0;
dt = 0;
delta_angle.zero();
}
// integrate delta angle accumulator // integrate delta angle accumulator
// the angles and coning corrections are accumulated separately in the // the angles and coning corrections are accumulated separately in the
// referenced paper, but in simulation little difference was found between // referenced paper, but in simulation little difference was found between
@ -305,6 +319,8 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
_update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance], _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
_imu._accel_raw_sample_rates[instance]); _imu._accel_raw_sample_rates[instance]);
uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
/* /*
we have two classes of sensors. FIFO based sensors produce data we have two classes of sensors. FIFO based sensors produce data
at a very predictable overall rate, but the data comes in at a very predictable overall rate, but the data comes in
@ -315,6 +331,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
*/ */
if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) { if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f; dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f;
_imu._accel_last_sample_us[instance] = sample_us;
} else { } else {
// don't accept below 100Hz // don't accept below 100Hz
if (_imu._accel_raw_sample_rates[instance] < 100) { if (_imu._accel_raw_sample_rates[instance] < 100) {
@ -322,8 +339,8 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
} }
dt = 1.0f / _imu._accel_raw_sample_rates[instance]; dt = 1.0f / _imu._accel_raw_sample_rates[instance];
_imu._accel_last_sample_us[instance] = AP_HAL::micros64();
} }
_imu._accel_last_sample_us[instance] = sample_us;
#if AP_MODULE_SUPPORTED #if AP_MODULE_SUPPORTED
// call accel_sample hook if any // call accel_sample hook if any
@ -335,6 +352,15 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
{ {
WITH_SEMAPHORE(_sem); WITH_SEMAPHORE(_sem);
uint64_t now = AP_HAL::micros64();
if (now - last_sample_us > 100000U) {
// zero accumulator if sensor was unhealthy for 0.1s
_imu._delta_velocity_acc[instance].zero();
_imu._delta_velocity_acc_dt[instance] = 0;
dt = 0;
}
// delta velocity // delta velocity
_imu._delta_velocity_acc[instance] += accel * dt; _imu._delta_velocity_acc[instance] += accel * dt;
_imu._delta_velocity_acc_dt[instance] += dt; _imu._delta_velocity_acc_dt[instance] += dt;