mirror of https://github.com/ArduPilot/ardupilot
51 lines
1.4 KiB
C++
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
|