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

2


3
#if AP_BARO_DRONECAN_ENABLED

4


5
#include <AP_CANManager/AP_CANManager.h>

6
#include <AP_BoardConfig/AP_BoardConfig.h>

7
#include "AP_Baro_SITL.h"

8
#include <AP_Vehicle/AP_Vehicle_Type.h>

9


10
extern const AP_HAL::HAL& hal;

11


12
#define LOG_TAG "Baro"

13


14
AP_Baro_DroneCAN::DetectedModules AP_Baro_DroneCAN::_detected_modules[];

15
HAL_Semaphore AP_Baro_DroneCAN::_sem_registry;

16


17
/*

18
  constructor - registers instance at top Baro driver

19
 */

20
AP_Baro_DroneCAN::AP_Baro_DroneCAN(AP_Baro &baro) :

21
    AP_Baro_Backend(baro)

22
{}

23


24
bool AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)

25
{

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

27


28
    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, driver_index) != nullptr)

29
        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, driver_index) != nullptr)

30
    ;

31
}

32


33
AP_Baro_Backend* AP_Baro_DroneCAN::probe(AP_Baro &baro)

34
{

35
    WITH_SEMAPHORE(_sem_registry);

36


37
    AP_Baro_DroneCAN* backend = nullptr;

38
    for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {

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

40
            backend = NEW_NOTHROW AP_Baro_DroneCAN(baro);

41
            if (backend == nullptr) {

42
                AP::can().log_text(AP_CANManager::LOG_ERROR,

43
                            LOG_TAG,

44
                            "Failed register DroneCAN Baro Node %d on Bus %d\n",

45
                            _detected_modules[i].node_id,

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

47
            } else {

48
                _detected_modules[i].driver = backend;

49
                backend->_pressure = 0;

50
                backend->_pressure_count = 0;

51
                backend->_ap_dronecan = _detected_modules[i].ap_dronecan;

52
                backend->_node_id = _detected_modules[i].node_id;

53


54
                backend->_instance = backend->_frontend.register_sensor();

55
                backend->set_bus_id(backend->_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,

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

57
                                                                                    backend->_node_id, 0));

58


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

60
                            LOG_TAG,

61
                            "Registered DroneCAN Baro Node %d on Bus %d\n",

62
                            _detected_modules[i].node_id,

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

64
            }

65
            break;

66
        }

67
    }

68
    return backend;

69
}

70


71
AP_Baro_DroneCAN* AP_Baro_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, bool create_new)

72
{

73
    if (ap_dronecan == nullptr) {

74
        return nullptr;

75
    }

76
    for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {

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

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

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

80
            return _detected_modules[i].driver;

81
        }

82
    }

83
    

84
    if (create_new) {

85
        bool already_detected = false;

86
        //Check if there's an empty spot for possible registration

87
        for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {

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

89
                //Already Detected

90
                already_detected = true;

91
                break;

92
            }

93
        }

94
        if (!already_detected) {

95
            for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {

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

97
                    _detected_modules[i].ap_dronecan = ap_dronecan;

98
                    _detected_modules[i].node_id = node_id;

99
                    break;

100
                }

101
            }

102
        }

103
    }

104


105
    return nullptr;

106
}

107


108


109
void AP_Baro_DroneCAN::_update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count)

110
{

111
    *accum += val;

112
    *count += 1;

113
    if (*count == max_count) {

114
        *count = max_count / 2;

115
        *accum = *accum / 2;

116
    }

117
}

118


119
void AP_Baro_DroneCAN::handle_pressure(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg)

120
{

121
    AP_Baro_DroneCAN* driver;

122
    {

123
        WITH_SEMAPHORE(_sem_registry);

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

125
        if (driver == nullptr) {

126
            return;

127
        }

128
    }

129
    {

130
        WITH_SEMAPHORE(driver->_sem_baro);

131
        _update_and_wrap_accumulator(&driver->_pressure, msg.static_pressure, &driver->_pressure_count, 32);

132
        driver->new_pressure = true;

133
    }

134
}

135


136
void AP_Baro_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticTemperature &msg)

137
{

138
    AP_Baro_DroneCAN* driver;

139
    {

140
        WITH_SEMAPHORE(_sem_registry);

141
        driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, false);

142
        if (driver == nullptr) {

143
            return;

144
        }

145
    }

146
    {

147
        WITH_SEMAPHORE(driver->_sem_baro);

148
        driver->_temperature = KELVIN_TO_C(msg.static_temperature);

149
    }

150
}

151


152
// Read the sensor

153
void AP_Baro_DroneCAN::update(void)

154
{

155
    float pressure = 0;

156


157
    WITH_SEMAPHORE(_sem_baro);

158
    if (new_pressure) {

159
        if (_pressure_count != 0) {

160
            pressure = _pressure / _pressure_count;

161
            _pressure_count = 0;

162
            _pressure = 0;

163
        }

164
        _copy_to_frontend(_instance, pressure, _temperature);

165


166


167
        _frontend.set_external_temperature(_temperature);

168
        new_pressure = false;

169
    }

170
}

171


172
#endif // AP_BARO_DRONECAN_ENABLED

173


174