Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: replace libuavcan with libcanard based driver

Browse Source
This commit is contained in:
bugobliterator 2023-01-04 12:44:30 +11:00 committed by Andrew Tridgell
parent 7112d156ed
commit 54df802d59
6 changed files with 39 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Baro/AP_Baro.cpp
View File

@ -605,10 +605,13 @@ void AP_Baro::init(void)
if (sitl == nullptr) { if (sitl == nullptr) {
AP_HAL::panic("No SITL pointer"); 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++) { for(uint8_t i = 0; i < sitl->baro_count; i++) {
ADD_BACKEND(new AP_Baro_SITL(*this)); ADD_BACKEND(new AP_Baro_SITL(*this));
} }
#endif #endif
#endif
#if AP_BARO_UAVCAN_ENABLED #if AP_BARO_UAVCAN_ENABLED
// Detect UAVCAN Modules, try as many times as there are driver slots // Detect UAVCAN Modules, try as many times as there are driver slots

1
libraries/AP_Baro/AP_Baro.h
View File

@ -28,6 +28,7 @@ class AP_Baro
{ {
friend class AP_Baro_Backend; friend class AP_Baro_Backend;
friend class AP_Baro_SITL; // for access to sensors[] friend class AP_Baro_SITL; // for access to sensors[]
friend class AP_Baro_UAVCAN; // for access to sensors[]
public: public:
AP_Baro(); AP_Baro();

16
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

@ -29,17 +29,17 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
void AP_Baro_SITL::temperature_adjustment(float &p, float &T) void AP_Baro_SITL::temperature_adjustment(float &p, float &T)
{ {
const float tsec = AP_HAL::millis() * 0.001f; const float tsec = AP_HAL::millis() * 0.001f;
const float T_sensor = T + _sitl->temp_board_offset; const float T_sensor = T + AP::sitl()->temp_board_offset;
const float tconst = _sitl->temp_tconst; const float tconst = AP::sitl()->temp_tconst;
if (tsec < 23 * tconst) { // time which past the equation below equals T_sensor within approx. 1E-9 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); T = T_sensor - (T_sensor - T0) * expf(-tsec / tconst);
} }
else { else {
T = T_sensor; 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 const float Tzero = 30.0f; // start baro adjustment at 30C
if (is_positive(baro_factor)) { if (is_positive(baro_factor)) {
// this produces a pressure change with temperature that // this produces a pressure change with temperature that
@ -129,7 +129,7 @@ void AP_Baro_SITL::_timer()
#endif #endif
// add in correction for wind effects // add in correction for wind effects
p += wind_pressure_correction(); p += wind_pressure_correction(_instance);
_recent_press = p; _recent_press = p;
_recent_temp = T; _recent_temp = T;
@ -157,12 +157,12 @@ void AP_Baro_SITL::update(void)
/* /*
return pressure correction for wind based on SIM_BARO_WCF parameters 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 // 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; float error = 0.0;
const float sqx = sq(airspeed_vec_bf.x); const float sqx = sq(airspeed_vec_bf.x);

12
libraries/AP_Baro/AP_Baro_SITL.h
View File

@ -14,6 +14,12 @@ public:
void update() override; 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: protected:
void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = healthy(instance); }; 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; static const uint8_t _buffer_length = 50;
VectorN<readings_baro, _buffer_length> _buffer; VectorN<readings_baro, _buffer_length> _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 // is the barometer usable for flight
bool healthy(uint8_t instance); bool healthy(uint8_t instance);

42
libraries/AP_Baro/AP_Baro_UAVCAN.cpp
View File

@ -3,19 +3,14 @@
#if AP_BARO_UAVCAN_ENABLED #if AP_BARO_UAVCAN_ENABLED
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#include "AP_Baro_SITL.h"
#include <uavcan/equipment/air_data/StaticPressure.hpp> #include <AP_Vehicle/AP_Vehicle_Type.h>
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define LOG_TAG "Baro" #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[]; AP_Baro_UAVCAN::DetectedModules AP_Baro_UAVCAN::_detected_modules[];
HAL_Semaphore AP_Baro_UAVCAN::_sem_registry; 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) { if (ap_uavcan == nullptr) {
return; return;
} }
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_pressure, ap_uavcan->get_driver_index()) == nullptr) {
auto* node = ap_uavcan->get_node(); AP_BoardConfig::allocation_error("pressure_sub");
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, PressureCb> *pressure_listener;
pressure_listener = new uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, PressureCb>(*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");
} }
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, TemperatureCb> *temperature_listener; if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_temperature, ap_uavcan->get_driver_index()) == nullptr) {
temperature_listener = new uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, TemperatureCb>(*node); AP_BoardConfig::allocation_error("temperature_sub");
// 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");
} }
} }
@ -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; AP_Baro_UAVCAN* driver;
{ {
WITH_SEMAPHORE(_sem_registry); 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) { if (driver == nullptr) {
return; return;
} }
} }
{ {
WITH_SEMAPHORE(driver->_sem_baro); 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; 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; AP_Baro_UAVCAN* driver;
{ {
WITH_SEMAPHORE(_sem_registry); 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) { if (driver == nullptr) {
return; return;
} }
} }
{ {
WITH_SEMAPHORE(driver->_sem_baro); WITH_SEMAPHORE(driver->_sem_baro);
driver->_temperature = KELVIN_TO_C(cb.msg->static_temperature); driver->_temperature = KELVIN_TO_C(msg.static_temperature);
} }
} }

14
libraries/AP_Baro/AP_Baro_UAVCAN.h
View File

@ -5,9 +5,9 @@
#if AP_BARO_UAVCAN_ENABLED #if AP_BARO_UAVCAN_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_UAVCAN/AP_UAVCAN.h>
#if AP_TEST_DRONECAN_DRIVERS
class PressureCb; #include <SITL/SITL.h>
class TemperatureCb; #endif
class AP_Baro_UAVCAN : public AP_Baro_Backend { class AP_Baro_UAVCAN : public AP_Baro_Backend {
public: 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_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new);
static AP_Baro_Backend* probe(AP_Baro &baro); 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_pressure(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg);
static void handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb); 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: private:
static void _update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count); static void _update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count);