mirror of https://github.com/ArduPilot/ardupilot
63 lines
1.9 KiB
C++
63 lines
1.9 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) {
|
|
// 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();
|
|
|
|
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;
|
|
|
|
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,
|
|
UAVCAN_EQUIPMENT_AHRS_RAWIMU_ID,
|
|
CANARD_TRANSFER_PRIORITY_HIGH,
|
|
&buffer[0],
|
|
total_size);
|
|
}
|
|
}
|
|
#endif // HAL_PERIPH_ENABLE_IMU
|