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

AP_InertialSensor: add option to enable fast fifo reset on ICM20602

Browse Source
This commit is contained in:
bugobliterator 2022-11-01 05:32:00 +05:30 committed by Andrew Tridgell
parent ff5cc48441
commit 4351f83c35
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -47,6 +47,10 @@ extern const AP_HAL::HAL& hal;
#define debug(fmt, args ...) do {printf("MPU: " fmt "\n", ## args); } while(0) #define debug(fmt, args ...) do {printf("MPU: " fmt "\n", ## args); } while(0)
#endif #endif
#ifndef HAL_ENABLE_FAST_FIFO_RESET_ICM20602
#define HAL_ENABLE_FAST_FIFO_RESET_ICM20602 0
#endif
/* /*
EXT_SYNC allows for frame synchronisation with an external device EXT_SYNC allows for frame synchronisation with an external device
such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
@ -239,7 +243,7 @@ void AP_InertialSensor_Invensense::start()
// ICM20602 has a bug where sometimes the data gets a huge offset // 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 // this seems to be fixed by doing a quick FIFO reset via USR_CTRL
// reg // reg
_enable_fast_fifo_reset = true; _enable_fast_fifo_reset = HAL_ENABLE_FAST_FIFO_RESET_ICM20602;
_enable_offset_checking = true; _enable_offset_checking = true;
break; break;
case Invensense_ICM20601: case Invensense_ICM20601:
@ -449,6 +453,7 @@ bool AP_InertialSensor_Invensense::update() /* front end */
_publish_temperature(_accel_instance, _temp_filtered); _publish_temperature(_accel_instance, _temp_filtered);
#if HAL_ENABLE_FAST_FIFO_RESET_ICM20602
if (fast_reset_count) { if (fast_reset_count) {
// check if we have reported in the last 1 seconds or // check if we have reported in the last 1 seconds or
// fast_reset_count changed // fast_reset_count changed
@ -470,7 +475,7 @@ bool AP_InertialSensor_Invensense::update() /* front end */
} }
#endif #endif
} }
#endif
return true; return true;
} }
@ -634,10 +639,13 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
// use temperature to detect FIFO corruption // use temperature to detect FIFO corruption
int16_t t2 = int16_val(data, 3); int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) { if (!_check_raw_temp(t2)) {
#if HAL_ENABLE_FAST_FIFO_RESET_ICM20602
if (_enable_fast_fifo_reset) { if (_enable_fast_fifo_reset) {
_fast_fifo_reset(); _fast_fifo_reset();
ret = false; ret = false;
} else { } else
#endif
{
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);
} }