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/compass.cpp
Views: 1798
1
#include "AP_Periph.h"

2


3
#ifdef HAL_PERIPH_ENABLE_MAG

4


5
/*

6
  magnetometer support

7
 */

8


9
#include <dronecan_msgs.h>

10


11
#ifndef AP_PERIPH_MAG_MAX_RATE

12
#define AP_PERIPH_MAG_MAX_RATE 25U

13
#endif

14


15
#ifndef AP_PERIPH_PROBE_CONTINUOUS

16
#define AP_PERIPH_PROBE_CONTINUOUS 0

17
#endif

18


19
#ifndef AP_PERIPH_MAG_HIRES

20
#define AP_PERIPH_MAG_HIRES 0

21
#endif

22


23
extern const AP_HAL::HAL &hal;

24


25
/*

26
  update CAN magnetometer

27
 */

28
void AP_Periph_FW::can_mag_update(void)

29
{

30
    if (!compass.available()) {

31
        return;

32
    }

33


34
#if AP_PERIPH_MAG_MAX_RATE > 0

35
    // don't flood the bus with very high rate magnetometers

36
    const uint32_t now_ms = AP_HAL::millis();

37
    if (now_ms - last_mag_update_ms < (1000U / AP_PERIPH_MAG_MAX_RATE)) {

38
        return;

39
    }

40
#endif

41


42
    compass.read();

43


44
#if AP_PERIPH_PROBE_CONTINUOUS

45
    if (option_is_set(PeriphOptions::PROBE_CONTINUOUS) && !hal.util->get_soft_armed() && (compass.get_count() == 0)) {

46
        static uint32_t last_probe_ms;

47
        uint32_t now = AP_HAL::millis();

48
        if (now - last_probe_ms >= 1000) {

49
            last_probe_ms = now;

50
            compass.init();

51
        }

52
    }

53
#endif

54


55
    if (last_mag_update_ms == compass.last_update_ms()) {

56
        return;

57
    }

58
    if (!compass.healthy()) {

59
        return;

60
    }

61


62
    last_mag_update_ms = compass.last_update_ms();

63
    Vector3f field_Ga = compass.get_field() * 0.001;

64


65
#if !AP_PERIPH_MAG_HIRES

66
    // normal message, float16 values

67
    uavcan_equipment_ahrs_MagneticFieldStrength pkt {};

68


69
    for (uint8_t i=0; i<3; i++) {

70
        pkt.magnetic_field_ga[i] = field_Ga[i];

71
    }

72
    

73
    uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE];

74
    uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout());

75


76
    canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE,

77
                    UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID,

78
                    CANARD_TRANSFER_PRIORITY_LOW,

79
                    &buffer[0],

80
                    total_size);

81
#else

82
    // High resolution magnetometer, for magnetic surveys

83
    dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes pkt {};

84


85
    for (uint8_t i=0; i<3; i++) {

86
        pkt.magnetic_field_ga[i] = field_Ga[i];

87
    }

88
    

89
    uint8_t buffer[DRONECAN_SENSORS_MAGNETOMETER_MAGNETICFIELDSTRENGTHHIRES_MAX_SIZE];

90
    uint16_t total_size = dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes_encode(&pkt, buffer, !periph.canfdout());

91


92
    canard_broadcast(DRONECAN_SENSORS_MAGNETOMETER_MAGNETICFIELDSTRENGTHHIRES_SIGNATURE,

93
                     DRONECAN_SENSORS_MAGNETOMETER_MAGNETICFIELDSTRENGTHHIRES_ID,

94
                     CANARD_TRANSFER_PRIORITY_LOW,

95
                     &buffer[0],

96
                     total_size);

97
#endif // AP_PERIPH_MAG_HIRES

98
}

99


100
#endif // HAL_PERIPH_ENABLE_MAG

101


102