aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: enumeration and multiple interfaces support

Browse Source
This commit is contained in:
Eugene Shamaev 2017-05-06 12:13:05 +03:00 committed by Francisco Ferreira
parent 8f2306fd19
commit 6e6efa7e1b
3 changed files with 79 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Baro/AP_Baro.cpp
View File

@ -25,6 +25,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_Baro_SITL.h"
@ -394,6 +395,16 @@ void AP_Baro::init(void)
return;
#endif
#if HAL_WITH_UAVCAN
bool added;
do {
added = _add_backend(AP_Baro_UAVCAN::probe(*this));
if (_num_drivers == BARO_MAX_DRIVERS || _num_sensors == BARO_MAX_INSTANCES) {
return;
}
} while (added);
#endif
#if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
@ -479,19 +490,6 @@ void AP_Baro::init(void)
std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR))));
#endif
}
#if HAL_WITH_UAVCAN
// If there is place left - allocate one UAVCAN based baro
if ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr))
{
if(_num_drivers < BARO_MAX_DRIVERS && _num_sensors < BARO_MAX_INSTANCES)
{
printf("Creating AP_Baro_UAVCAN\n\r");
drivers[_num_drivers] = new AP_Baro_UAVCAN(*this);
_num_drivers++;
}
}
#endif
if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
AP_BoardConfig::sensor_config_error("Baro: unable to initialise driver");
@ -617,3 +615,4 @@ void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction)
sensors[instance].p_correction = p_correction;
}
}

81
libraries/AP_Baro/AP_Baro_UAVCAN.cpp
View File

@ -4,6 +4,7 @@
#include "AP_Baro_UAVCAN.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <sys/types.h>
#include <sys/stat.h>
@ -12,9 +13,7 @@
extern const AP_HAL::HAL& hal;
#define debug_baro_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0)
// There is limitation to use only one UAVCAN barometer now.
#define debug_baro_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
/*
constructor - registers instance at top Baro driver
@ -22,33 +21,50 @@ extern const AP_HAL::HAL& hal;
AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) :
AP_Baro_Backend(baro)
{
_instance = _frontend.register_sensor();
if (hal.can_mgr != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
if (ap_uavcan != nullptr) {
// Give time to receive some packets from CAN if baro sensor is present
// This way it will get calibrated correctly
hal.scheduler->delay(1000);
ap_uavcan->register_baro_listener(this, 1);
debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r");
}
}
_sem_baro = hal.util->new_semaphore();
}
AP_Baro_UAVCAN::~AP_Baro_UAVCAN()
{
if (hal.can_mgr != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
if (ap_uavcan != nullptr) {
ap_uavcan->remove_baro_listener(this);
debug_baro_uavcan(2, "AP_Baro_UAVCAN destructed\n\r");
if (_initialized) {
if (hal.can_mgr[_manager] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[_manager]->get_UAVCAN();
if (ap_uavcan != nullptr) {
ap_uavcan->remove_baro_listener(this);
debug_baro_uavcan(2, "AP_Baro_UAVCAN destructed\n\r");
}
}
}
}
AP_Baro_Backend *AP_Baro_UAVCAN::probe(AP_Baro &baro)
{
AP_Baro_UAVCAN *sensor = nullptr;
if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
if (uavcan != nullptr) {
uint8_t freebaro = uavcan->find_smallest_free_baro_node();
if (freebaro != UINT8_MAX) {
sensor = new AP_Baro_UAVCAN(baro);
if (sensor->register_uavcan_baro(i, freebaro)) {
debug_baro_uavcan(2, "AP_Baro_UAVCAN probed, drv: %d, node: %d\n\r", i, freebaro);
return sensor;
} else {
delete sensor;
sensor = nullptr;
}
}
}
}
}
}
return sensor;
}
// Read the sensor
void AP_Baro_UAVCAN::update(void)
{
@ -70,5 +86,28 @@ void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature)
}
}
bool AP_Baro_UAVCAN::register_uavcan_baro(uint8_t mgr, uint8_t node)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
_manager = mgr;
if (ap_uavcan->register_baro_listener_to_node(this, node))
{
_instance = _frontend.register_sensor();
debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r");
_initialized = true;
return true;
}
}
}
return false;
}
#endif // HAL_WITH_UAVCAN

7
libraries/AP_Baro/AP_Baro_UAVCAN.h
View File

@ -8,16 +8,23 @@ public:
AP_Baro_UAVCAN(AP_Baro &);
~AP_Baro_UAVCAN() override;
static AP_Baro_Backend *probe(AP_Baro &baro);
void update() override;
// This method is called from UAVCAN thread
virtual void handle_baro_msg(float pressure, float temperature) override;
bool register_uavcan_baro(uint8_t mgr, uint8_t node);
private:
uint8_t _instance;
float _pressure;
float _temperature;
uint64_t _last_timestamp;
uint8_t _manager;
bool _initialized;
AP_HAL::Semaphore *_sem_baro;
};