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

AP_InertialSensor: added optional FIFO rate logging to invensensev3 driver

Browse Source 
this is useful for tracking down and confirming the stuck gyro issue
on the ICM42688
This commit is contained in:
Andrew Tridgell 2023-10-22 08:43:46 +11:00
parent 792d8a4cb8
commit b6c9ac2569
3 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -319,6 +319,8 @@ private:
// logging // logging
void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const __RAMFUNC__; // Write GYR data packet: raw gyro data
protected:
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp=false) const __RAMFUNC__; // Write GYR data packet: raw gyro data
}; };

10
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -137,6 +137,9 @@ extern const AP_HAL::HAL& hal;
#define INV3_ID_ICM42670 0x67 #define INV3_ID_ICM42670 0x67
#define INV3_ID_ICM45686 0xE9 #define INV3_ID_ICM45686 0xE9
// enable logging at FIFO rate for debugging
#define INV3_ENABLE_FIFO_LOGGING 0
/* /*
really nice that this sensor has an option to request little-endian really nice that this sensor has an option to request little-endian
data data
@ -336,6 +339,9 @@ void AP_InertialSensor_Invensensev3::accumulate()
bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, uint8_t n_samples) bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, uint8_t n_samples)
{ {
#if INV3_ENABLE_FIFO_LOGGING
const uint64_t tstart = AP_HAL::micros64();
#endif
for (uint8_t i = 0; i < n_samples; i++) { for (uint8_t i = 0; i < n_samples; i++) {
const FIFOData &d = data[i]; const FIFOData &d = data[i];
@ -352,6 +358,10 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
accel *= accel_scale; accel *= accel_scale;
gyro *= gyro_scale; gyro *= gyro_scale;
#if INV3_ENABLE_FIFO_LOGGING
Write_GYR(gyro_instance, tstart+(i*backend_period_us), gyro, true);
#endif
const float temp = d.temperature * temp_sensitivity + temp_zero; const float temp = d.temperature * temp_sensitivity + temp_zero;
// these four calls are about 40us // these four calls are about 40us

4
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View File

@ -20,9 +20,9 @@ void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t
} }
// Write GYR data packet: raw gyro data // Write GYR data packet: raw gyro data
void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp) const
{ {
const uint64_t now = AP_HAL::micros64(); const uint64_t now = use_sample_timestamp?sample_us:AP_HAL::micros64();
const struct log_GYR pkt{ const struct log_GYR pkt{
LOG_PACKET_HEADER_INIT(LOG_GYR_MSG), LOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
time_us : now, time_us : now,