diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 2d7371ae43..9c1f3665d5 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -605,10 +605,13 @@ void AP_Baro::init(void) if (sitl == nullptr) { AP_HAL::panic("No SITL pointer"); } +#if !AP_TEST_DRONECAN_DRIVERS + // use dronecan instances instead of SITL instances for(uint8_t i = 0; i < sitl->baro_count; i++) { ADD_BACKEND(new AP_Baro_SITL(*this)); } #endif +#endif #if AP_BARO_UAVCAN_ENABLED // Detect UAVCAN Modules, try as many times as there are driver slots diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 3d423164cd..d61a43b9b1 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -28,6 +28,7 @@ class AP_Baro { friend class AP_Baro_Backend; friend class AP_Baro_SITL; // for access to sensors[] + friend class AP_Baro_UAVCAN; // for access to sensors[] public: AP_Baro(); diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index 1dd4e0e15b..9b41e33645 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -29,17 +29,17 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) : void AP_Baro_SITL::temperature_adjustment(float &p, float &T) { const float tsec = AP_HAL::millis() * 0.001f; - const float T_sensor = T + _sitl->temp_board_offset; - const float tconst = _sitl->temp_tconst; + const float T_sensor = T + AP::sitl()->temp_board_offset; + const float tconst = AP::sitl()->temp_tconst; if (tsec < 23 * tconst) { // time which past the equation below equals T_sensor within approx. 1E-9 - const float T0 = _sitl->temp_start; + const float T0 = AP::sitl()->temp_start; T = T_sensor - (T_sensor - T0) * expf(-tsec / tconst); } else { T = T_sensor; } - const float baro_factor = _sitl->temp_baro_factor; + const float baro_factor = AP::sitl()->temp_baro_factor; const float Tzero = 30.0f; // start baro adjustment at 30C if (is_positive(baro_factor)) { // this produces a pressure change with temperature that @@ -129,7 +129,7 @@ void AP_Baro_SITL::_timer() #endif // add in correction for wind effects - p += wind_pressure_correction(); + p += wind_pressure_correction(_instance); _recent_press = p; _recent_temp = T; @@ -157,12 +157,12 @@ void AP_Baro_SITL::update(void) /* return pressure correction for wind based on SIM_BARO_WCF parameters */ -float AP_Baro_SITL::wind_pressure_correction(void) +float AP_Baro_SITL::wind_pressure_correction(uint8_t instance) { - const auto &bp = _sitl->baro[_instance]; + const auto &bp = AP::sitl()->baro[instance]; // correct for static pressure position errors - const Vector3f &airspeed_vec_bf = _sitl->state.velocity_air_bf; + const Vector3f &airspeed_vec_bf = AP::sitl()->state.velocity_air_bf; float error = 0.0; const float sqx = sq(airspeed_vec_bf.x); diff --git a/libraries/AP_Baro/AP_Baro_SITL.h b/libraries/AP_Baro/AP_Baro_SITL.h index 5fe90a985c..68ba04d016 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.h +++ b/libraries/AP_Baro/AP_Baro_SITL.h @@ -14,6 +14,12 @@ public: void update() override; + // adjust for simulated board temperature + static void temperature_adjustment(float &p, float &T); + + // adjust for wind effects + static float wind_pressure_correction(uint8_t instance); + protected: void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = healthy(instance); }; @@ -32,12 +38,6 @@ private: static const uint8_t _buffer_length = 50; VectorN _buffer; - // adjust for simulated board temperature - void temperature_adjustment(float &p, float &T); - - // adjust for wind effects - float wind_pressure_correction(void); - // is the barometer usable for flight bool healthy(uint8_t instance); diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp index 0c7c46e5cc..0a2a41ca80 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -3,19 +3,14 @@ #if AP_BARO_UAVCAN_ENABLED #include -#include - -#include -#include +#include +#include "AP_Baro_SITL.h" +#include extern const AP_HAL::HAL& hal; #define LOG_TAG "Baro" -//UAVCAN Frontend Registry Binder -UC_REGISTRY_BINDER(PressureCb, uavcan::equipment::air_data::StaticPressure); -UC_REGISTRY_BINDER(TemperatureCb, uavcan::equipment::air_data::StaticTemperature); - AP_Baro_UAVCAN::DetectedModules AP_Baro_UAVCAN::_detected_modules[]; HAL_Semaphore AP_Baro_UAVCAN::_sem_registry; @@ -31,23 +26,12 @@ void AP_Baro_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) if (ap_uavcan == nullptr) { return; } - - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *pressure_listener; - pressure_listener = new uavcan::Subscriber(*node); - // Msg Handler - const int pressure_listener_res = pressure_listener->start(PressureCb(ap_uavcan, &handle_pressure)); - if (pressure_listener_res < 0) { - AP_HAL::panic("UAVCAN Baro subscriber start problem\n\r"); + if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_pressure, ap_uavcan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("pressure_sub"); } - uavcan::Subscriber *temperature_listener; - temperature_listener = new uavcan::Subscriber(*node); - // Msg Handler - const int temperature_listener_res = temperature_listener->start(TemperatureCb(ap_uavcan, &handle_temperature)); - if (temperature_listener_res < 0) { - AP_HAL::panic("UAVCAN Baro subscriber start problem\n\r"); + if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_temperature, ap_uavcan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("temperature_sub"); } } @@ -137,36 +121,36 @@ void AP_Baro_UAVCAN::_update_and_wrap_accumulator(float *accum, float val, uint8 } } -void AP_Baro_UAVCAN::handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb) +void AP_Baro_UAVCAN::handle_pressure(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg) { AP_Baro_UAVCAN* driver; { WITH_SEMAPHORE(_sem_registry); - driver = get_uavcan_backend(ap_uavcan, node_id, true); + driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id, true); if (driver == nullptr) { return; } } { WITH_SEMAPHORE(driver->_sem_baro); - _update_and_wrap_accumulator(&driver->_pressure, cb.msg->static_pressure, &driver->_pressure_count, 32); + _update_and_wrap_accumulator(&driver->_pressure, msg.static_pressure, &driver->_pressure_count, 32); driver->new_pressure = true; } } -void AP_Baro_UAVCAN::handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb) +void AP_Baro_UAVCAN::handle_temperature(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticTemperature &msg) { AP_Baro_UAVCAN* driver; { WITH_SEMAPHORE(_sem_registry); - driver = get_uavcan_backend(ap_uavcan, node_id, false); + driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id, false); if (driver == nullptr) { return; } } { WITH_SEMAPHORE(driver->_sem_baro); - driver->_temperature = KELVIN_TO_C(cb.msg->static_temperature); + driver->_temperature = KELVIN_TO_C(msg.static_temperature); } } diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.h b/libraries/AP_Baro/AP_Baro_UAVCAN.h index 2edcc2e871..38c73eb2d8 100644 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.h +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.h @@ -5,9 +5,9 @@ #if AP_BARO_UAVCAN_ENABLED #include - -class PressureCb; -class TemperatureCb; +#if AP_TEST_DRONECAN_DRIVERS +#include +#endif class AP_Baro_UAVCAN : public AP_Baro_Backend { public: @@ -19,9 +19,11 @@ public: static AP_Baro_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new); static AP_Baro_Backend* probe(AP_Baro &baro); - static void handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb); - static void handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb); - + static void handle_pressure(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg); + static void handle_temperature(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticTemperature &msg); +#if AP_TEST_DRONECAN_DRIVERS + void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = !AP::sitl()->baro[instance].disable; }; +#endif private: static void _update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count);