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

2


3
#ifdef HAL_PERIPH_ENABLE_PROXIMITY

4


5
/*

6
  proximity support

7
 */

8


9
#include <dronecan_msgs.h>

10


11
void AP_Periph_FW::can_proximity_update()

12
{

13
    if (proximity.get_type(0) == AP_Proximity::Type::None) {

14
        return;

15
    }

16


17
    uint32_t now = AP_HAL::millis();

18
    static uint32_t last_update_ms;

19
    if (g.proximity_max_rate > 0 &&

20
        now - last_update_ms < 1000/g.proximity_max_rate) {

21
        // limit to max rate

22
        return;

23
    }

24
    last_update_ms = now;

25
    proximity.update();

26
    AP_Proximity::Status status = proximity.get_status();

27
    if (status <= AP_Proximity::Status::NoData) {

28
        // don't send any data

29
        return;

30
    }

31


32
    ardupilot_equipment_proximity_sensor_Proximity pkt {};

33


34
    const uint8_t obstacle_count = proximity.get_obstacle_count();

35


36
    // if no objects return

37
    if (obstacle_count == 0) {

38
        return;

39
    }

40


41
    // calculate maximum roll, pitch values from objects

42
    for (uint8_t i=0; i<obstacle_count; i++) {

43
        if (!proximity.get_obstacle_info(i, pkt.yaw, pkt.pitch, pkt.distance)) {

44
            // not a valid obstacle

45
            continue;

46
        }

47


48
        pkt.sensor_id = proximity.get_address(0);

49


50
        switch (status) {

51
        case AP_Proximity::Status::NotConnected:

52
            pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED;

53
            break;

54
        case AP_Proximity::Status::Good:

55
            pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD;

56
            break;

57
        case AP_Proximity::Status::NoData:

58
        default:

59
            pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA;

60
            break;

61
        }

62


63
        uint8_t buffer[ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_MAX_SIZE];

64
        uint16_t total_size = ardupilot_equipment_proximity_sensor_Proximity_encode(&pkt, buffer, !periph.canfdout());

65


66
        canard_broadcast(ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_SIGNATURE,

67
                        ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_ID,

68
                        CANARD_TRANSFER_PRIORITY_LOW,

69
                        &buffer[0],

70
                        total_size);

71


72
    }

73
}

74


75
#endif // HAL_PERIPH_ENABLE_PROXIMITY

76


77