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

2


3
#if AP_AIRSPEED_DRONECAN_ENABLED

4


5
#include <AP_CANManager/AP_CANManager.h>

6
#include <AP_DroneCAN/AP_DroneCAN.h>

7
#include <AP_BoardConfig/AP_BoardConfig.h>

8


9
extern const AP_HAL::HAL& hal;

10


11
#define LOG_TAG "AirSpeed"

12


13
AP_Airspeed_DroneCAN::DetectedModules AP_Airspeed_DroneCAN::_detected_modules[];

14
HAL_Semaphore AP_Airspeed_DroneCAN::_sem_registry;

15


16
bool AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)

17
{

18
    const auto driver_index = ap_dronecan->get_driver_index();

19


20
    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, driver_index) != nullptr)

21
#if AP_AIRSPEED_HYGROMETER_ENABLE

22
        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, driver_index) != nullptr)

23
#endif

24
    ;

25
}

26


27
AP_Airspeed_Backend* AP_Airspeed_DroneCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid)

28
{

29
    WITH_SEMAPHORE(_sem_registry);

30


31
    AP_Airspeed_DroneCAN* backend = nullptr;

32


33
    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {

34
        if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) {

35
            const auto bus_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,

36
                                                            _detected_modules[i].ap_dronecan->get_driver_index(),

37
                                                            _detected_modules[i].node_id, 0);

38
            if (previous_devid != 0 && previous_devid != bus_id) {

39
                // match with previous ID only

40
                continue;

41
            }

42
            backend = NEW_NOTHROW AP_Airspeed_DroneCAN(_frontend, _instance);

43
            if (backend == nullptr) {

44
                AP::can().log_text(AP_CANManager::LOG_INFO,

45
                                   LOG_TAG,

46
                                   "Failed register DroneCAN Airspeed Node %d on Bus %d\n",

47
                                   _detected_modules[i].node_id,

48
                                   _detected_modules[i].ap_dronecan->get_driver_index());

49
            } else {

50
                _detected_modules[i].driver = backend;

51
                AP::can().log_text(AP_CANManager::LOG_INFO,

52
                                   LOG_TAG,

53
                                   "Registered DroneCAN Airspeed Node %d on Bus %d\n",

54
                                   _detected_modules[i].node_id,

55
                                   _detected_modules[i].ap_dronecan->get_driver_index());

56
                backend->set_bus_id(bus_id);

57
            }

58
            break;

59
        }

60
    }

61


62
    return backend;

63
}

64


65
AP_Airspeed_DroneCAN* AP_Airspeed_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)

66
{

67
    if (ap_dronecan == nullptr) {

68
        return nullptr;

69
    }

70


71
    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {

72
        if (_detected_modules[i].driver != nullptr &&

73
            _detected_modules[i].ap_dronecan == ap_dronecan &&

74
            _detected_modules[i].node_id == node_id ) {

75
            return _detected_modules[i].driver;

76
        }

77
    }

78


79
    bool detected = false;

80
    for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {

81
        if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) {

82
            // detected

83
            detected = true;

84
            break;

85
        }

86
    }

87


88
    if (!detected) {

89
        for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {

90
            if (_detected_modules[i].ap_dronecan == nullptr) {

91
                _detected_modules[i].ap_dronecan = ap_dronecan;

92
                _detected_modules[i].node_id = node_id;

93
                break;

94
            }

95
        }

96
    }

97


98
    return nullptr;

99
}

100


101
void AP_Airspeed_DroneCAN::handle_airspeed(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_RawAirData &msg)

102
{

103
    WITH_SEMAPHORE(_sem_registry);

104


105
    AP_Airspeed_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);

106


107
    if (driver != nullptr) {

108
        WITH_SEMAPHORE(driver->_sem_airspeed);

109
        driver->_pressure = msg.differential_pressure;

110
        if (!isnan(msg.static_air_temperature) &&

111
            msg.static_air_temperature > 0) {

112
            driver->_temperature = KELVIN_TO_C(msg.static_air_temperature);

113
            driver->_have_temperature = true;

114
        }

115
        driver->_last_sample_time_ms = AP_HAL::millis();

116
    }

117
}

118


119
#if AP_AIRSPEED_HYGROMETER_ENABLE

120
void AP_Airspeed_DroneCAN::handle_hygrometer(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_hygrometer_Hygrometer &msg)

121
{

122
    WITH_SEMAPHORE(_sem_registry);

123


124
    AP_Airspeed_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);

125


126
    if (driver != nullptr) {

127
        WITH_SEMAPHORE(driver->_sem_airspeed);

128
        driver->_hygrometer.temperature = KELVIN_TO_C(msg.temperature);

129
        driver->_hygrometer.humidity = msg.humidity;

130
        driver->_hygrometer.last_sample_ms = AP_HAL::millis();

131
    }

132
}

133
#endif // AP_AIRSPEED_HYGROMETER_ENABLE

134


135
bool AP_Airspeed_DroneCAN::init()

136
{

137
    // always returns true

138
    return true;

139
}

140


141
bool AP_Airspeed_DroneCAN::get_differential_pressure(float &pressure)

142
{

143
    WITH_SEMAPHORE(_sem_airspeed);

144


145
    if ((AP_HAL::millis() - _last_sample_time_ms) > 250) {

146
        return false;

147
    }

148


149
    pressure = _pressure;

150


151
    return true;

152
}

153


154
bool AP_Airspeed_DroneCAN::get_temperature(float &temperature)

155
{

156
    if (!_have_temperature) {

157
        return false;

158
    }

159
    WITH_SEMAPHORE(_sem_airspeed);

160


161
    if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {

162
        return false;

163
    }

164


165
    temperature = _temperature;

166


167
    return true;

168
}

169


170
#if AP_AIRSPEED_HYGROMETER_ENABLE

171
/*

172
  return hygrometer data if available

173
 */

174
bool AP_Airspeed_DroneCAN::get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity)

175
{

176
    if (_hygrometer.last_sample_ms == 0) {

177
        return false;

178
    }

179
    WITH_SEMAPHORE(_sem_airspeed);

180
    last_sample_ms = _hygrometer.last_sample_ms;

181
    temperature = _hygrometer.temperature;

182
    humidity = _hygrometer.humidity;

183
    return true;

184
}

185
#endif // AP_AIRSPEED_HYGROMETER_ENABLE

186
#endif // AP_AIRSPEED_DRONECAN_ENABLED

187


188