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

2


3
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER

4


5
/*

6
  rangefinder support

7
 */

8


9
#include <dronecan_msgs.h>

10


11
#ifndef AP_PERIPH_PROBE_CONTINUOUS

12
#define AP_PERIPH_PROBE_CONTINUOUS 0

13
#endif

14


15
extern const AP_HAL::HAL &hal;

16


17
/*

18
  update CAN rangefinder

19
 */

20
void AP_Periph_FW::can_rangefinder_update(void)

21
{

22
    if (rangefinder.get_type(0) == RangeFinder::Type::NONE) {

23
        return;

24
    }

25
#if AP_PERIPH_PROBE_CONTINUOUS

26
    // We only allow continuous probing for rangefinders while vehicle is disarmed. Probing is currently inefficient and leads to longer loop times.

27
    if ((rangefinder.num_sensors() == 0) && !hal.util->get_soft_armed() && option_is_set(PeriphOptions::PROBE_CONTINUOUS)) {

28
        uint32_t now = AP_HAL::millis();

29
        static uint32_t last_probe_ms;

30
        if (now - last_probe_ms >= 1000) {

31
            last_probe_ms = now;

32
            rangefinder.init(ROTATION_NONE);

33
        }

34
    }

35
#endif

36
    uint32_t now = AP_HAL::millis();

37
    if (g.rangefinder_max_rate > 0 &&

38
        now - last_rangefinder_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {

39
        // limit to max rate

40
        return;

41
    }

42
    last_rangefinder_update_ms = now;

43


44
    // update all rangefinder instances

45
    rangefinder.update();

46


47
    // cycle through each rangefinder instance to find one to send

48
    // equipment.range_sensor only uses 3 CAN frames so we just send all available sensor measurements.

49
    for (uint8_t i = 0; i <= rangefinder.num_sensors(); i++) {

50


51
        if (rangefinder.get_type(i) == RangeFinder::Type::NONE) {

52
            continue;

53
        }

54


55
        AP_RangeFinder_Backend *backend = rangefinder.get_backend(i);

56
        if (backend == nullptr) {

57
            continue;

58
        }

59


60
        RangeFinder::Status status = backend->status();

61
        if (status <= RangeFinder::Status::NoData) {

62
            // don't send any data for this instance

63
            continue;

64
        }

65


66
        const uint32_t sample_ms = backend->last_reading_ms();

67
        if (last_rangefinder_sample_ms[i] == sample_ms) {

68
            // don't same the same reading again

69
            continue;

70
        }

71
        last_rangefinder_sample_ms[i] = sample_ms;

72


73
        uavcan_equipment_range_sensor_Measurement pkt {};

74
        pkt.sensor_id = rangefinder.get_address(i);

75


76
        switch (status) {

77
        case RangeFinder::Status::OutOfRangeLow:

78
            pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;

79
            break;

80
        case RangeFinder::Status::OutOfRangeHigh:

81
            pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;

82
            break;

83
        case RangeFinder::Status::Good:

84
            pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;

85
            break;

86
        default:

87
            pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED;

88
            break;

89
        }

90


91
        switch (backend->get_mav_distance_sensor_type()) {

92
        case MAV_DISTANCE_SENSOR_LASER:

93
            pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;

94
            break;

95
        case MAV_DISTANCE_SENSOR_ULTRASOUND:

96
            pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR;

97
            break;

98
        case MAV_DISTANCE_SENSOR_RADAR:

99
            pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR;

100
            break;

101
        default:

102
            pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED;

103
            break;

104
        }

105


106
        float dist_m = backend->distance();

107
        pkt.range = dist_m;

108


109
        uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE];

110
        uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout());

111


112
        canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,

113
                        UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,

114
                        CANARD_TRANSFER_PRIORITY_LOW,

115
                        &buffer[0],

116
                        total_size);

117


118
    }

119
}

120


121
#endif // HAL_PERIPH_ENABLE_RANGEFINDER

122


123