CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/AP_Periph/imu.cpp
Views: 1798
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

63


64