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

AP_InertialSensor: allow FIFO rate logging for hires sampling

Browse Source
This commit is contained in:
Andy Piper 2024-04-30 19:44:50 +01:00 committed by Andrew Tridgell
parent 10c189d28d
commit 471079d5b6
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -468,6 +468,9 @@ static inline float uint20_to_float(uint8_t msb, uint8_t bits, uint8_t lsb)
bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHighRes *data, uint8_t n_samples) bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHighRes *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 FIFODataHighRes &d = data[i]; const FIFODataHighRes &d = data[i];
@ -488,6 +491,9 @@ bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHi
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