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

AP_RangeFinder: use WITH_SEMAPHORE()

Browse Source 
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

9
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp
View File

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

11
libraries/AP_RangeFinder/AP_RangeFinder_TeraRangerI2C.cpp
View File

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

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) : AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state) :
state(_state) state(_state)
{ {
_sem = hal.util->new_semaphore();
} }
MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const { 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; RangeFinder::RangeFinder_State &state;
// semaphore for access to shared frontend data // 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; virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const = 0;
}; };