mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: add fast reset for ICM20602 instead of full reset on bad temp sample
This commit is contained in:
parent
7dd5244bad
commit
c8f5e3b6b5
|
@ -27,6 +27,7 @@
|
|||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#include "AP_InertialSensor_Invensense.h"
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -195,6 +196,16 @@ void AP_InertialSensor_Invensense::_fifo_reset(bool log_error)
|
|||
notify_gyro_fifo_reset(_gyro_instance);
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Invensense::_fast_fifo_reset()
|
||||
{
|
||||
fast_reset_count++;
|
||||
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl | BIT_USER_CTRL_FIFO_RESET);
|
||||
|
||||
notify_accel_fifo_reset(_accel_instance);
|
||||
notify_gyro_fifo_reset(_gyro_instance);
|
||||
}
|
||||
|
||||
|
||||
bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
|
||||
{
|
||||
return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C;
|
||||
|
@ -225,6 +236,10 @@ void AP_InertialSensor_Invensense::start()
|
|||
case Invensense_ICM20602:
|
||||
gdev = DEVTYPE_INS_ICM20602;
|
||||
adev = DEVTYPE_INS_ICM20602;
|
||||
// ICM20602 has a bug where sometimes the data gets a huge offset
|
||||
// this seems to be fixed by doing a quick FIFO reset via USR_CTRL
|
||||
// reg
|
||||
_enable_fast_fifo_reset = true;
|
||||
_enable_offset_checking = true;
|
||||
break;
|
||||
case Invensense_ICM20601:
|
||||
|
@ -434,6 +449,28 @@ bool AP_InertialSensor_Invensense::update() /* front end */
|
|||
|
||||
_publish_temperature(_accel_instance, _temp_filtered);
|
||||
|
||||
if (fast_reset_count) {
|
||||
// check if we have reported in the last 1 seconds or
|
||||
// fast_reset_count changed
|
||||
#if HAL_GCS_ENABLED && BOARD_FLASH_SIZE > 1024
|
||||
if (AP_HAL::millis() - last_fast_reset_count_report_ms > 1000) {
|
||||
last_fast_reset_count_report_ms = AP_HAL::millis();
|
||||
char param_name[sizeof("IMUx_RST")];
|
||||
if (_gyro_instance <= 9) {
|
||||
snprintf(param_name, sizeof(param_name), "IMU%u_RST", _gyro_instance);
|
||||
} else {
|
||||
snprintf(param_name, sizeof(param_name), "IMUx_RST");
|
||||
}
|
||||
gcs().send_named_float(param_name, fast_reset_count);
|
||||
}
|
||||
#endif
|
||||
#if HAL_LOGGING_ENABLED
|
||||
if (last_fast_reset_count != fast_reset_count) {
|
||||
AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -547,11 +584,16 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
|
|||
|
||||
int16_t t2 = int16_val(data, 3);
|
||||
if (!_check_raw_temp(t2)) {
|
||||
if (!hal.scheduler->in_expected_delay()) {
|
||||
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
||||
if (_enable_fast_fifo_reset) {
|
||||
_fast_fifo_reset();
|
||||
return false;
|
||||
} else {
|
||||
if (!hal.scheduler->in_expected_delay()) {
|
||||
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
||||
}
|
||||
_fifo_reset(true);
|
||||
return false;
|
||||
}
|
||||
_fifo_reset(true);
|
||||
return false;
|
||||
}
|
||||
float temp = t2 * temp_sensitivity + temp_zero;
|
||||
|
||||
|
@ -589,14 +631,19 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
|
|||
for (uint8_t i = 0; i < n_samples; i++) {
|
||||
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
|
||||
|
||||
// use temperatue to detect FIFO corruption
|
||||
// use temperature to detect FIFO corruption
|
||||
int16_t t2 = int16_val(data, 3);
|
||||
if (!_check_raw_temp(t2)) {
|
||||
if (!hal.scheduler->in_expected_delay()) {
|
||||
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
||||
if (_enable_fast_fifo_reset) {
|
||||
_fast_fifo_reset();
|
||||
ret = false;
|
||||
} else {
|
||||
if (!hal.scheduler->in_expected_delay()) {
|
||||
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
||||
}
|
||||
_fifo_reset(true);
|
||||
ret = false;
|
||||
}
|
||||
_fifo_reset(true);
|
||||
ret = false;
|
||||
break;
|
||||
}
|
||||
tsum += t2;
|
||||
|
@ -727,7 +774,7 @@ void AP_InertialSensor_Invensense::_read_fifo()
|
|||
|
||||
if (_fast_sampling) {
|
||||
if (!_accumulate_sensor_rate_sampling(rx, n)) {
|
||||
if (!hal.scheduler->in_expected_delay()) {
|
||||
if (!hal.scheduler->in_expected_delay() && !_enable_fast_fifo_reset) {
|
||||
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
|
||||
}
|
||||
break;
|
||||
|
|
|
@ -86,6 +86,8 @@ private:
|
|||
|
||||
void _set_filter_register(void);
|
||||
void _fifo_reset(bool log_error) __RAMFUNC__;
|
||||
void _fast_fifo_reset() __RAMFUNC__;
|
||||
|
||||
bool _has_auxiliary_bus();
|
||||
|
||||
/* Read samples from FIFO (FIFO enabled) */
|
||||
|
@ -129,12 +131,18 @@ private:
|
|||
LowPassFilter2pFloat _temp_filter;
|
||||
uint32_t last_reset_ms;
|
||||
uint8_t reset_count;
|
||||
uint8_t fast_reset_count;
|
||||
uint8_t last_fast_reset_count;
|
||||
uint32_t last_fast_reset_count_report_ms;
|
||||
|
||||
enum Rotation _rotation;
|
||||
|
||||
// enable checking of unexpected resets of offsets
|
||||
bool _enable_offset_checking;
|
||||
|
||||
// enable fast fifo reset instead of full fifo reset
|
||||
bool _enable_fast_fifo_reset;
|
||||
|
||||
// ICM-20602 y offset register. See usage for explanation
|
||||
uint8_t _saved_y_ofs_high;
|
||||
|
||||
|
|
Loading…
Reference in New Issue