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

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

This commit is contained in:
Peter Barker 2019-02-15 12:43:56 +11:00 committed by Andrew Tridgell
parent 7a674254e5
commit 5b06d01402
2 changed files with 6 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -70,21 +70,10 @@ void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
} }
} }
bool AP_Compass_UAVCAN::take_registry()
{
return _sem_registry.take(HAL_SEMAPHORE_BLOCK_FOREVER);
}
void AP_Compass_UAVCAN::give_registry()
{
_sem_registry.give();
}
AP_Compass_Backend* AP_Compass_UAVCAN::probe() AP_Compass_Backend* AP_Compass_UAVCAN::probe()
{ {
if (!take_registry()) { WITH_SEMAPHORE(_sem_registry);
return nullptr;
}
AP_Compass_UAVCAN* driver = nullptr; AP_Compass_UAVCAN* driver = nullptr;
for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) { for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) {
if (!_detected_modules[i].driver && _detected_modules[i].ap_uavcan) { if (!_detected_modules[i].driver && _detected_modules[i].ap_uavcan) {
@ -103,7 +92,6 @@ AP_Compass_Backend* AP_Compass_UAVCAN::probe()
break; break;
} }
} }
give_registry();
return driver; return driver;
} }
@ -181,7 +169,8 @@ void AP_Compass_UAVCAN::handle_mag_msg(const Vector3f &mag)
void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb) void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb)
{ {
if (take_registry()) { WITH_SEMAPHORE(_sem_registry);
Vector3f mag_vector; Vector3f mag_vector;
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0); AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0);
if (driver != nullptr) { if (driver != nullptr) {
@ -190,13 +179,12 @@ void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node
mag_vector[2] = cb.msg->magnetic_field_ga[2]; mag_vector[2] = cb.msg->magnetic_field_ga[2];
driver->handle_mag_msg(mag_vector); driver->handle_mag_msg(mag_vector);
} }
give_registry();
}
} }
void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb) void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb)
{ {
if (take_registry()) { WITH_SEMAPHORE(_sem_registry);
Vector3f mag_vector; Vector3f mag_vector;
uint8_t sensor_id = cb.msg->sensor_id; uint8_t sensor_id = cb.msg->sensor_id;
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id); AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id);
@ -206,8 +194,6 @@ void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t no
mag_vector[2] = cb.msg->magnetic_field_ga[2]; mag_vector[2] = cb.msg->magnetic_field_ga[2];
driver->handle_mag_msg(mag_vector); driver->handle_mag_msg(mag_vector);
} }
give_registry();
}
} }
void AP_Compass_UAVCAN::read(void) void AP_Compass_UAVCAN::read(void)

2
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

@ -26,8 +26,6 @@ private:
// callback for UAVCAN messages // callback for UAVCAN messages
void handle_mag_msg(const Vector3f &mag); void handle_mag_msg(const Vector3f &mag);
static bool take_registry();
static void give_registry();
static AP_Compass_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id); static AP_Compass_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id);
uint8_t _instance; uint8_t _instance;