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

AP_InertialSensor: trigger internal error on persistent IMU reset

Browse Source 
this will give users a warning when they are getting the Invensense
IMU FIFO reset issue
This commit is contained in:
Andrew Tridgell 2020-09-01 15:42:43 +10:00
parent b20279e452
commit cfc59497f6
2 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -22,6 +22,7 @@
#include <stdio.h> #include <stdio.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_InternalError/AP_InternalError.h>
#include "AP_InertialSensor_Invensense.h" #include "AP_InertialSensor_Invensense.h"
@ -147,8 +148,21 @@ bool AP_InertialSensor_Invensense::_init()
return success; return success;
} }
void AP_InertialSensor_Invensense::_fifo_reset() void AP_InertialSensor_Invensense::_fifo_reset(bool log_error)
{ {
uint32_t now = AP_HAL::millis();
if (log_error &&
!hal.scheduler->in_expected_delay() &&
now - last_reset_ms < 10000) {
reset_count++;
if (reset_count == 10) {
// 10 resets, each happening within 10s, triggers an internal error
INTERNAL_ERROR(AP_InternalError::error_t::imu_reset);
reset_count = 0;
}
}
last_reset_ms = now;
uint8_t user_ctrl = _last_stat_user_ctrl; uint8_t user_ctrl = _last_stat_user_ctrl;
user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN); user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
@ -184,7 +198,7 @@ void AP_InertialSensor_Invensense::start()
hal.scheduler->delay(1); hal.scheduler->delay(1);
// always use FIFO // always use FIFO
_fifo_reset(); _fifo_reset(false);
// grab the used instances // grab the used instances
enum DevTypes gdev, adev; enum DevTypes gdev, adev;
@ -433,7 +447,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
if (!hal.scheduler->in_expected_delay()) { if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
} }
_fifo_reset(); _fifo_reset(true);
return false; return false;
} }
float temp = t2 * temp_sensitivity + temp_zero; float temp = t2 * temp_sensitivity + temp_zero;
@ -478,7 +492,7 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
if (!hal.scheduler->in_expected_delay()) { if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2); debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
} }
_fifo_reset(); _fifo_reset(true);
ret = false; ret = false;
break; break;
} }
@ -625,7 +639,7 @@ void AP_InertialSensor_Invensense::_read_fifo()
if (need_reset) { if (need_reset) {
//debug("fifo reset n_samples %u", bytes_read/MPU_SAMPLE_SIZE); //debug("fifo reset n_samples %u", bytes_read/MPU_SAMPLE_SIZE);
_fifo_reset(); _fifo_reset(false);
} }
check_registers: check_registers:

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -86,7 +86,7 @@ private:
bool _check_whoami(); bool _check_whoami();
void _set_filter_register(void); void _set_filter_register(void);
void _fifo_reset(); void _fifo_reset(bool log_error);
bool _has_auxiliary_bus(); bool _has_auxiliary_bus();
/* Read samples from FIFO (FIFO enabled) */ /* Read samples from FIFO (FIFO enabled) */
@ -125,6 +125,8 @@ private:
float _fifo_accel_scale; float _fifo_accel_scale;
float _fifo_gyro_scale; float _fifo_gyro_scale;
LowPassFilter2pFloat _temp_filter; LowPassFilter2pFloat _temp_filter;
uint32_t last_reset_ms;
uint8_t reset_count;
enum Rotation _rotation; enum Rotation _rotation;