Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: fixed IMU in periph 

wait_for_sample() can't handle very low sample rates without a
separate delay() by a ms value
This commit is contained in:
Andrew Tridgell 2024-11-30 17:34:38 +11:00
parent 02ea371eae
commit 4c0ef40d3b
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Tools/AP_Periph/imu.cpp
View File

@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal;
void AP_Periph_FW::can_imu_update(void) void AP_Periph_FW::can_imu_update(void)
{ {
while (true) { while (true) {
// sleep for a bit to avoid flooding the CPU // we need to delay by a ms value as hal->schedule->delay_microseconds_boost
hal.scheduler->delay_microseconds(100); // 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(); imu.update();
@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void)
pkt.accelerometer_latest[1] = tmp.y; pkt.accelerometer_latest[1] = tmp.y;
pkt.accelerometer_latest[2] = tmp.z; 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]; uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE];
uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout()); uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE, canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,