Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: uavcan: use WITH_SEMAPHORE in place of give/take _registry

This commit is contained in:
Peter Barker 2019-02-15 12:37:08 +11:00 committed by Andrew Tridgell
parent 79481028b4
commit 78f05492d7
2 changed files with 6 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Baro/AP_Baro_UAVCAN.cpp
View File

@ -55,21 +55,10 @@ void AP_Baro_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
}
}
bool AP_Baro_UAVCAN::take_registry()
{
return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
}
void AP_Baro_UAVCAN::give_registry()
{
_sem_registry.give();
}
AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro)
{
if (!take_registry()) {
return nullptr;
}
WITH_SEMAPHORE(_sem_registry);
AP_Baro_UAVCAN* backend = nullptr;
for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) {
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) {
@ -94,7 +83,6 @@ AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro)
break;
}
}
give_registry();
return backend;
}
@ -137,10 +125,10 @@ AP_Baro_UAVCAN* AP_Baro_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t
void AP_Baro_UAVCAN::handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb)
{
if (take_registry()) {
WITH_SEMAPHORE(_sem_registry);
AP_Baro_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, true);
if (driver == nullptr) {
give_registry();
return;
}
{
@ -148,24 +136,20 @@ void AP_Baro_UAVCAN::handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, cons
driver->_pressure = cb.msg->static_pressure;
driver->new_pressure = true;
}
give_registry();
}
}
void AP_Baro_UAVCAN::handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb)
{
if (take_registry()) {
WITH_SEMAPHORE(_sem_registry);
AP_Baro_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, false);
if (driver == nullptr) {
give_registry();
return;
}
{
WITH_SEMAPHORE(driver->_sem_baro);
driver->_temperature = cb.msg->static_temperature - C_TO_KELVIN;
}
give_registry();
}
}
// Read the sensor

2
libraries/AP_Baro/AP_Baro_UAVCAN.h
View File

@ -25,8 +25,6 @@ public:
static void handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb);
private:
static bool take_registry();
static void give_registry();
uint8_t _instance;