5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00

AP_Baro: replace libuavcan with libcanard based driver

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

View File

@ -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

View File

@ -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();

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)
{
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);

View File

@ -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<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
bool healthy(uint8_t instance);

View File

@ -3,19 +3,14 @@
#if AP_BARO_UAVCAN_ENABLED
#include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/equipment/air_data/StaticPressure.hpp>
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include "AP_Baro_SITL.h"
#include <AP_Vehicle/AP_Vehicle_Type.h>
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<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");
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_pressure, ap_uavcan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("pressure_sub");
}
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, TemperatureCb> *temperature_listener;
temperature_listener = new uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, TemperatureCb>(*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);
}
}

View File

@ -5,9 +5,9 @@
#if AP_BARO_UAVCAN_ENABLED
#include <AP_UAVCAN/AP_UAVCAN.h>
class PressureCb;
class TemperatureCb;
#if AP_TEST_DRONECAN_DRIVERS
#include <SITL/SITL.h>
#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);