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

AP_RangeFinder: use WITH_SEMAPHORE() 

and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent 530138ded4
commit b1e4434332
4 changed files with 28 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

@ -27,6 +27,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Common/Semaphore.h>
extern const AP_HAL::HAL& hal;
@ -130,12 +131,10 @@ void AP_RangeFinder_MaxsonarI2CXL::_timer(void)
{
uint16_t d;
if (get_reading(d)) {
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
distance = d;
new_distance = true;
state.last_reading_ms = AP_HAL::millis();
_sem->give();
}
WITH_SEMAPHORE(_sem);
distance = d;
new_distance = true;
state.last_reading_ms = AP_HAL::millis();
}
}
@ -144,15 +143,13 @@ void AP_RangeFinder_MaxsonarI2CXL::_timer(void)
*/
void AP_RangeFinder_MaxsonarI2CXL::update(void)
{
if (_sem->take_nonblocking()) {
if (new_distance) {
state.distance_cm = distance;
new_distance = false;
update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 300) {
// if no updates for 0.3 seconds set no-data
set_status(RangeFinder::RangeFinder_NoData);
}
_sem->give();
WITH_SEMAPHORE(_sem);
if (new_distance) {
state.distance_cm = distance;
new_distance = false;
update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 300) {
// if no updates for 0.3 seconds set no-data
set_status(RangeFinder::RangeFinder_NoData);
}
}

27
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

@ -20,6 +20,7 @@
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h>
#include <AP_Common/Semaphore.h>
extern const AP_HAL::HAL& hal;
@ -159,12 +160,13 @@ void AP_RangeFinder_TeraRangerI2C::timer(void)
uint16_t _raw_distance = 0;
uint16_t _distance_cm = 0;
if (collect_raw(_raw_distance) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (collect_raw(_raw_distance)) {
WITH_SEMAPHORE(_sem);
if (process_raw_measure(_raw_distance, _distance_cm)){
accum.sum += _distance_cm;
accum.count++;
}
_sem->give();
}
// and immediately ask for a new reading
measure();
@ -175,16 +177,15 @@ void AP_RangeFinder_TeraRangerI2C::timer(void)
*/
void AP_RangeFinder_TeraRangerI2C::update(void)
{
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (accum.count > 0) {
state.distance_cm = accum.sum / accum.count;
state.last_reading_ms = AP_HAL::millis();
accum.sum = 0;
accum.count = 0;
update_status();
} else {
set_status(RangeFinder::RangeFinder_NoData);
}
_sem->give();
WITH_SEMAPHORE(_sem);
if (accum.count > 0) {
state.distance_cm = accum.sum / accum.count;
state.last_reading_ms = AP_HAL::millis();
accum.sum = 0;
accum.count = 0;
update_status();
} else {
set_status(RangeFinder::RangeFinder_NoData);
}
}

1
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
View File

@ -27,7 +27,6 @@ extern const AP_HAL::HAL& hal;
AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state) :
state(_state)
{
_sem = hal.util->new_semaphore();
}
MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const {

2
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -69,7 +69,7 @@ protected:
RangeFinder::RangeFinder_State &state;
// semaphore for access to shared frontend data
AP_HAL::Semaphore *_sem;
HAL_Semaphore _sem;
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const = 0;
};