Tools/AP_Periph/imu.cpp

   1 #include "AP_Periph.h"
   2
   3 #ifdef HAL_PERIPH_ENABLE_IMU
   4 #include <dronecan_msgs.h>
   5
   6 extern const AP_HAL::HAL &hal;
   7
   8 /*
   9   update CAN magnetometer
  10  */
  11 void AP_Periph_FW::can_imu_update(void)
  12 {
  13     while (true) {
  14         // we need to delay by a ms value as hal->schedule->delay_microseconds_boost
  15         // used in wait_for_sample() takes uint16_t
  16         const uint32_t delay_ms = 1000U / g.imu_sample_rate;
  17         hal.scheduler->delay(delay_ms);
  18
  19         if (delay_ms == 0) {
  20             // sleep for a bit to avoid flooding the CPU
  21             hal.scheduler->delay_microseconds(100);
  22         }
  23
  24         imu.update();
  25
  26         if (!imu.healthy()) {
  27             continue;
  28         }
  29
  30         uavcan_equipment_ahrs_RawIMU pkt {};
  31
  32         Vector3f tmp;
  33         imu.get_delta_velocity(tmp, pkt.integration_interval);
  34         pkt.accelerometer_integral[0] = tmp.x;
  35         pkt.accelerometer_integral[1] = tmp.y;
  36         pkt.accelerometer_integral[2] = tmp.z;
  37
  38         imu.get_delta_angle(tmp, pkt.integration_interval);
  39         pkt.rate_gyro_integral[0] = tmp.x;
  40         pkt.rate_gyro_integral[1] = tmp.y;
  41         pkt.rate_gyro_integral[2] = tmp.z;
  42
  43         tmp = imu.get_accel();
  44         pkt.accelerometer_latest[0] = tmp.x;
  45         pkt.accelerometer_latest[1] = tmp.y;
  46         pkt.accelerometer_latest[2] = tmp.z;
  47
  48         tmp = imu.get_gyro();
  49         pkt.rate_gyro_latest[0] = tmp.x;
  50         pkt.rate_gyro_latest[1] = tmp.y;
  51         pkt.rate_gyro_latest[2] = tmp.z;
  52
  53         uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE];
  54         uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout());
  55         canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE,
  56                          UAVCAN_EQUIPMENT_AHRS_RAWIMU_ID,
  57                          CANARD_TRANSFER_PRIORITY_HIGH,
  58                          &buffer[0],
  59                          total_size);
  60     }
  61 }
  62 #endif // HAL_PERIPH_ENABLE_IMU