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/libraries/AP_DAL/AP_DAL_RangeFinder.cpp
Views: 1798
1
#include "AP_DAL_RangeFinder.h"

2


3
#include <AP_Logger/AP_Logger.h>

4


5
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>

6


7
#if AP_RANGEFINDER_ENABLED

8


9
#include "AP_DAL.h"

10
#include <AP_BoardConfig/AP_BoardConfig.h>

11
#include <AP_Vehicle/AP_Vehicle_Type.h>

12
#include <AP_InternalError/AP_InternalError.h>

13


14
AP_DAL_RangeFinder::AP_DAL_RangeFinder()

15
{

16
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)

17
    _RRNH.num_sensors = AP::rangefinder()->num_sensors();

18
    _RRNI = NEW_NOTHROW log_RRNI[_RRNH.num_sensors];

19
    _backend = NEW_NOTHROW AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];

20
    if (!_RRNI || !_backend) {

21
        goto failed;

22
    }

23
    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {

24
        _RRNI[i].instance = i;

25
    }

26
    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {

27
        // this avoids having to discard a const....

28
        _backend[i] = NEW_NOTHROW AP_DAL_RangeFinder_Backend(_RRNI[i]);

29
        if (!_backend[i]) {

30
            goto failed;

31
        }

32
    }

33
    return;

34
failed:

35
    AP_BoardConfig::allocation_error("DAL backends");

36
#endif

37
}

38


39
int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const

40
{

41
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)

42
    const auto *rangefinder = AP::rangefinder();

43


44
    if (orientation != ROTATION_PITCH_270) {

45
        // the EKF only asks for this from a specific orientation.  Thankfully.

46
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);

47
        return rangefinder->ground_clearance_cm_orient(orientation);

48
    }

49
#endif

50


51
    return _RRNH.ground_clearance_cm;

52
}

53


54
int16_t AP_DAL_RangeFinder::max_distance_cm_orient(enum Rotation orientation) const

55
{

56
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)

57
    if (orientation != ROTATION_PITCH_270) {

58
        const auto *rangefinder = AP::rangefinder();

59
        // the EKF only asks for this from a specific orientation.  Thankfully.

60
        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);

61
        return rangefinder->max_distance_cm_orient(orientation);

62
    }

63
#endif

64


65
    return _RRNH.max_distance_cm;

66
}

67


68
void AP_DAL_RangeFinder::start_frame()

69
{

70
    const auto *rangefinder = AP::rangefinder();

71
    if (rangefinder == nullptr) {

72
        return;

73
    }

74


75
    const log_RRNH old = _RRNH;

76


77
    // EKF only asks for this *down*.

78
    _RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);

79
    _RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270);

80


81
    WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old);

82


83
    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {

84
        auto *backend = rangefinder->get_backend(i);

85
        if (backend == nullptr) {

86
            continue;

87
        }

88
        _backend[i]->start_frame(backend);

89
    }

90
}

91


92


93


94


95
AP_DAL_RangeFinder_Backend::AP_DAL_RangeFinder_Backend(struct log_RRNI &RRNI) :

96
    _RRNI(RRNI)

97
{

98
}

99


100
void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {

101
    const log_RRNI old = _RRNI;

102
    _RRNI.orientation = backend->orientation();

103
    _RRNI.status = (uint8_t)backend->status();

104
    _RRNI.pos_offset = backend->get_pos_offset();

105
    _RRNI.distance_cm = backend->distance_cm();

106
    WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);

107
}

108


109
// return true if we have a range finder with the specified orientation

110
bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const

111
{

112
    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {

113
        if (_RRNI[i].orientation == orientation) {

114
            return true;

115
        }

116
    }

117
    return false;

118
}

119


120


121
AP_DAL_RangeFinder_Backend *AP_DAL_RangeFinder::get_backend(uint8_t id) const

122
{

123
   if (id >= RANGEFINDER_MAX_INSTANCES) {

124
       INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);

125
       return nullptr;

126
   }

127
   if (id >= _RRNH.num_sensors) {

128
        return nullptr;

129
    }

130


131
   return _backend[id];

132
}

133


134
void AP_DAL_RangeFinder::handle_message(const log_RRNH &msg)

135
{

136
    _RRNH = msg;

137
    if (_RRNH.num_sensors > 0 && _RRNI == nullptr) {

138
        _RRNI = NEW_NOTHROW log_RRNI[_RRNH.num_sensors];

139
        _backend = NEW_NOTHROW AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];

140
    }

141
}

142


143
void AP_DAL_RangeFinder::handle_message(const log_RRNI &msg)

144
{

145
    if (_RRNI != nullptr && msg.instance < _RRNH.num_sensors) {

146
        _RRNI[msg.instance] = msg;

147
        if (_backend != nullptr && _backend[msg.instance] == nullptr) {

148
            _backend[msg.instance] = NEW_NOTHROW AP_DAL_RangeFinder_Backend(_RRNI[msg.instance]);

149
        }

150
    }

151
}

152


153
#endif  // AP_RANGEFINDER_ENABLED

154


155