ardupilot/Tools/AP_Periph/imu.cpp

51 lines
1.4 KiB
C++

#include "AP_Periph.h"
#ifdef HAL_PERIPH_ENABLE_IMU
#include <dronecan_msgs.h>
extern const AP_HAL::HAL &hal;
/*
update CAN magnetometer
*/
void AP_Periph_FW::can_imu_update(void)
{
while (true) {
// sleep for a bit to avoid flooding the CPU
hal.scheduler->delay_microseconds(100);
imu.update();
if (!imu.healthy()) {
continue;
}
uavcan_equipment_ahrs_RawIMU pkt {};
Vector3f tmp;
imu.get_delta_velocity(tmp, pkt.integration_interval);
pkt.accelerometer_integral[0] = tmp.x;
pkt.accelerometer_integral[1] = tmp.y;
pkt.accelerometer_integral[2] = tmp.z;
imu.get_delta_angle(tmp, pkt.integration_interval);
pkt.rate_gyro_integral[0] = tmp.x;
pkt.rate_gyro_integral[1] = tmp.y;
pkt.rate_gyro_integral[2] = tmp.z;
tmp = imu.get_accel();
pkt.accelerometer_latest[0] = tmp.x;
pkt.accelerometer_latest[1] = tmp.y;
pkt.accelerometer_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,
UAVCAN_EQUIPMENT_AHRS_RAWIMU_ID,
CANARD_TRANSFER_PRIORITY_HIGH,
&buffer[0],
total_size);
}
}
#endif // HAL_PERIPH_ENABLE_IMU