Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: cope with race conditioning popping points on SmartRTL return

Browse Source 
There is a race with the cleanup thread.  While thin, it only has to
happen once.  After this patch the race would have to happen... a lot.

Co-authored-by: jasclarke308 <jasclarke308@gmail.com>
This commit is contained in:
Peter Barker 2020-10-23 11:28:41 +11:00 committed by Andrew Tridgell
parent 03746c8145
commit 65f95325c3
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/mode.h
View File

@ -1101,6 +1101,10 @@ private:
void land(); void land();
SmartRTLState smart_rtl_state = SmartRTL_PathFollow; SmartRTLState smart_rtl_state = SmartRTL_PathFollow;
// keep track of how long we have failed to get another return
// point while following our path home. If we take too long we
// may choose to land the vehicle.
uint32_t path_follow_last_pop_fail_ms;
}; };

18
ArduCopter/mode_smart_rtl.cpp
View File

@ -69,6 +69,7 @@ void ModeSmartRTL::wait_cleanup_run()
// check if return path is computed and if yes, begin journey home // check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) { if (g2.smart_rtl.request_thorough_cleanup()) {
path_follow_last_pop_fail_ms = 0;
smart_rtl_state = SmartRTL_PathFollow; smart_rtl_state = SmartRTL_PathFollow;
} }
} }
@ -87,7 +88,10 @@ void ModeSmartRTL::path_follow_run()
// if we are close to current target point, switch the next point to be our target. // if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) { if (wp_nav->reached_wp_destination()) {
Vector3f next_point; Vector3f next_point;
// this pop_point can fail if the IO task currently has the
// path semaphore.
if (g2.smart_rtl.pop_point(next_point)) { if (g2.smart_rtl.pop_point(next_point)) {
path_follow_last_pop_fail_ms = 0;
bool fast_waypoint = true; bool fast_waypoint = true;
if (g2.smart_rtl.get_num_points() == 0) { if (g2.smart_rtl.get_num_points() == 0) {
// this is the very last point, add 2m to the target alt and move to pre-land state // this is the very last point, add 2m to the target alt and move to pre-land state
@ -98,8 +102,18 @@ void ModeSmartRTL::path_follow_run()
// send target to waypoint controller // send target to waypoint controller
wp_nav->set_wp_destination_NED(next_point); wp_nav->set_wp_destination_NED(next_point);
wp_nav->set_fast_waypoint(fast_waypoint); wp_nav->set_fast_waypoint(fast_waypoint);
} else { } else if (g2.smart_rtl.get_num_points() == 0) {
// this can only happen if we fail to get the semaphore which should never happen but just in case, land // We should never get here; should always have at least
// two points and the "zero points left" is handled above.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
smart_rtl_state = SmartRTL_PreLandPosition;
} else if (path_follow_last_pop_fail_ms == 0) {
// first time we've failed to pop off (ever, or after a success)
path_follow_last_pop_fail_ms = AP_HAL::millis();
} else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10000) {
// we failed to pop a point off for 10 seconds. This is
// almost certainly a bug.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
smart_rtl_state = SmartRTL_PreLandPosition; smart_rtl_state = SmartRTL_PreLandPosition;
} }
} }