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:
parent
7112d156ed
commit
54df802d59
@ -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
|
||||
|
@ -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();
|
||||
|
@ -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);
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
Loading…
Reference in New Issue
Block a user