Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 13:08:34 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add fast reset for ICM20602 instead of full reset on bad temp sample

Browse Source
This commit is contained in:
bugobliterator 2022-10-22 10:18:46 +05:30 committed by Randy Mackay
parent 7dd5244bad
commit c8f5e3b6b5
2 changed files with 65 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -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;

8
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -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;