mirror of https://github.com/ArduPilot/ardupilot
AP_SmartRTL: use WITH_SEMAPHORE()
and removed usage of hal.util->new_semaphore()
This commit is contained in:
parent
b1e4434332
commit
046bcfa0a9
|
@ -95,12 +95,6 @@ void AP_SmartRTL::init()
|
|||
return;
|
||||
}
|
||||
|
||||
// create semaphore
|
||||
_path_sem = hal.util->new_semaphore();
|
||||
if (_path_sem == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// allocate arrays
|
||||
_path = (Vector3f*)calloc(_points_max, sizeof(Vector3f));
|
||||
|
||||
|
@ -144,14 +138,14 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
|
|||
}
|
||||
|
||||
// get semaphore
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
if (!_path_sem.take_nonblocking()) {
|
||||
log_action(SRTL_POP_FAILED_NO_SEMAPHORE);
|
||||
return false;
|
||||
}
|
||||
|
||||
// check we have another point
|
||||
if (_path_points_count == 0) {
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -161,7 +155,7 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
|
|||
// record count of last point popped
|
||||
_path_points_completed_limit = _path_points_count;
|
||||
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -287,7 +281,7 @@ void AP_SmartRTL::cancel_request_for_thorough_cleanup()
|
|||
bool AP_SmartRTL::add_point(const Vector3f& point)
|
||||
{
|
||||
// get semaphore
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
if (!_path_sem.take_nonblocking()) {
|
||||
log_action(SRTL_ADD_FAILED_NO_SEMAPHORE, point);
|
||||
return false;
|
||||
}
|
||||
|
@ -296,14 +290,14 @@ bool AP_SmartRTL::add_point(const Vector3f& point)
|
|||
if (_path_points_count > 0) {
|
||||
const Vector3f& last_pos = _path[_path_points_count-1];
|
||||
if (last_pos.distance_squared(point) < sq(_accuracy.get())) {
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// check we have space in the path
|
||||
if (_path_points_count >= _path_points_max) {
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
log_action(SRTL_ADD_FAILED_PATH_FULL, point);
|
||||
return false;
|
||||
}
|
||||
|
@ -312,7 +306,7 @@ bool AP_SmartRTL::add_point(const Vector3f& point)
|
|||
_path[_path_points_count++] = point;
|
||||
log_action(SRTL_POINT_ADD, point);
|
||||
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -324,14 +318,14 @@ void AP_SmartRTL::run_background_cleanup()
|
|||
}
|
||||
|
||||
// get semaphore
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
if (!_path_sem.take_nonblocking()) {
|
||||
return;
|
||||
}
|
||||
// local copy of _path_points_count and _path_points_completed_limit
|
||||
const uint16_t path_points_count = _path_points_count;
|
||||
const uint16_t path_points_completed_limit = _path_points_completed_limit;
|
||||
_path_points_completed_limit = SMARTRTL_POINTS_MAX;
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
|
||||
// check if thorough cleanup is required
|
||||
if (_thorough_clean_request_ms > 0) {
|
||||
|
@ -623,7 +617,7 @@ void AP_SmartRTL::reset_pruning()
|
|||
void AP_SmartRTL::remove_points_by_simplify_bitmask()
|
||||
{
|
||||
// get semaphore before modifying path
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
if (!_path_sem.take_nonblocking()) {
|
||||
return;
|
||||
}
|
||||
uint16_t dest = 1;
|
||||
|
@ -648,7 +642,7 @@ void AP_SmartRTL::remove_points_by_simplify_bitmask()
|
|||
deactivate(SRTL_DEACTIVATED_PROGRAM_ERROR, "program error");
|
||||
}
|
||||
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
|
||||
// flag point removal is complete
|
||||
_simplify.bitmask.setall();
|
||||
|
@ -666,7 +660,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
|
|||
}
|
||||
|
||||
// get semaphore before modifying path
|
||||
if (!_path_sem->take_nonblocking()) {
|
||||
if (!_path_sem.take_nonblocking()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -692,7 +686,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
|
|||
} else {
|
||||
// this is an error that should never happen so deactivate
|
||||
deactivate(SRTL_DEACTIVATED_PROGRAM_ERROR, "program error");
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
// we return true so thorough_cleanup does not get stuck
|
||||
return true;
|
||||
}
|
||||
|
@ -712,7 +706,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove)
|
|||
_prune.loops_count--;
|
||||
}
|
||||
|
||||
_path_sem->give();
|
||||
_path_sem.give();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -184,7 +184,7 @@ private:
|
|||
uint16_t _path_points_max; // after the array has been allocated, we will need to know how big it is. We can't use the parameter, because a user could change the parameter in-flight
|
||||
uint16_t _path_points_count;// number of points in the path array
|
||||
uint16_t _path_points_completed_limit; // set by main thread to the path_point_count when a point is popped. used by simplify and prune algorithms to detect path shrinking
|
||||
AP_HAL::Semaphore *_path_sem; // semaphore for updating path
|
||||
HAL_Semaphore _path_sem; // semaphore for updating path
|
||||
|
||||
// Simplify
|
||||
// structure and buffer to hold the "to-do list" for the simplify algorithm.
|
||||
|
|
Loading…
Reference in New Issue