#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