From 4c0ef40d3b1fbcc41a58dfbe02258dfa65bc81c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2024 17:34:38 +1100 Subject: [PATCH] AP_Periph: fixed IMU in periph wait_for_sample() can't handle very low sample rates without a separate delay() by a ms value --- Tools/AP_Periph/imu.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/imu.cpp b/Tools/AP_Periph/imu.cpp index c0c5aaf401..06e961e44b 100644 --- a/Tools/AP_Periph/imu.cpp +++ b/Tools/AP_Periph/imu.cpp @@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal; void AP_Periph_FW::can_imu_update(void) { while (true) { - // sleep for a bit to avoid flooding the CPU - hal.scheduler->delay_microseconds(100); + // we need to delay by a ms value as hal->schedule->delay_microseconds_boost + // used in wait_for_sample() takes uint16_t + const uint32_t delay_ms = 1000U / g.imu_sample_rate; + hal.scheduler->delay(delay_ms); + + if (delay_ms == 0) { + // sleep for a bit to avoid flooding the CPU + hal.scheduler->delay_microseconds(100); + } imu.update(); @@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void) pkt.accelerometer_latest[1] = tmp.y; pkt.accelerometer_latest[2] = tmp.z; + tmp = imu.get_gyro(); + pkt.rate_gyro_latest[0] = tmp.x; + pkt.rate_gyro_latest[1] = tmp.y; + pkt.rate_gyro_latest[2] = tmp.z; + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE]; uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,