Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_SafeRTL: fixes from peer review

Browse Source 
- ACCURACY parameter description
- SafeRTL comments reduced to fit 80 characters
- moved a Comment down a line
- renamed definition to SAFERTL_PRUNING_LOOP_TIME_US
- comment change of "algorithm" to "algorithms"
- removed destructor (same could be done for AP_Airspeed then)
- updated GCS message to, "failed to save initial point"
- "update" comment modified from "a couple" to "several"
- added "const" ahead of "now = AP_HAL::millis()"
- added new deactivate method
- const run_background_cleanup's path_points_count
- removed unnecessary "return } else {"
- const potential_amount_to_simplify
- s/as/has in comments
- zero_points_by_simplify_bitmask starts from path[1] so as to never remove the start point
- remove_empty_points gets for loop in place of while, inverted clauses
- clarified dist_point returns value FLT_MAX in distance field
- lots of "const" added to segment_segment_dist, removed unnecessary else
- reference in Log_action.  Can't do it because want default
- ::update() comments add "3hz"
This commit is contained in:
Randy Mackay 2017-08-31 15:00:43 +09:00
parent ed0203e53c
commit e0da7ed09c
2 changed files with 92 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

166
libraries/AP_SafeRTL/AP_SafeRTL.cpp
View File

@ -17,8 +17,8 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_SafeRTL::var_info[] = {
// @Param: ACCURACY
// @DisplayName: SafeRTL _accuracy
// @Description: SafeRTL _accuracy. The minimum distance between points.
// @DisplayName: SafeRTL accuracy
// @Description: SafeRTL accuracy. The minimum distance between points.
// @Units: m
// @Range: 0 10
// @User: Advanced
@ -36,37 +36,40 @@ const AP_Param::GroupInfo AP_SafeRTL::var_info[] = {
};
/*
* This library is used for the Safe Return-to-Launch feature. The vehicle's position
* (aka "bread crumbs") are stored into an array in memory at regular intervals.
* After a certain number of bread crumbs have been stored and space within the array
* is low, clean-up algorithms are run to reduce the total number of points.
* When Safe-RTL is initiated by the vehicle code, a more thorough cleanup runs and
* the resulting path is fed into navigation controller to return the vehicle to home.
* This library is used for the Safe Return-to-Launch feature. The vehicle's
* position (aka "bread crumbs") are stored into an array in memory at
* regular intervals. After a certain number of bread crumbs have been
* stored and space within the array is low, clean-up algorithms are run to
* reduce the total number of points. When Safe-RTL is initiated by the
* vehicle code, a more thorough cleanup runs and the resulting path is fed
* into navigation controller to return the vehicle to home.
*
* The cleanup consists of two parts, pruning and simplification:
*
* 1. Pruning calculates the closest distance between two line segments formed by
* two pairs of sequential points, and then cuts out anything between two points
* when their line segments get close. This algorithm will never compare two
* consecutive line segments. Obviously the segments (p1,p2) and (p2,p3) will
* get very close (they touch), but there would be nothing to trim between them.
* 1. Pruning calculates the closest distance between two line segments formed
* by two pairs of sequential points, and then cuts out anything between two
* points when their line segments get close. This algorithm will never
* compare two consecutive line segments. Obviously the segments (p1,p2) and
* (p2,p3) will get very close (they touch), but there would be nothing to
* trim between them.
*
* 2. Simplification uses the Ramer-Douglas-Peucker algorithm. See Wikipedia for
* a more complete description.
* 2. Simplification uses the Ramer-Douglas-Peucker algorithm. See Wikipedia
* for a more complete description.
*
* The simplification and pruning algorithms run in the background and do not alter
* the path in memory. Two definitions, SAFERTL_SIMPLIFY_TIME_US and
* SAFERTL_LOOP_TIME_US are used to limit how long each algorithm will be run
* before they save their state and return.
* The simplification and pruning algorithms run in the background and do not
* alter the path in memory. Two definitions, SAFERTL_SIMPLIFY_TIME_US and
* SAFERTL_PRUNING_LOOP_TIME_US are used to limit how long each algorithm will
* be run before they save their state and return.
*
* Both algorithm are "anytime algorithms" meaning they can be interrupted before
* they complete which is helpful when memory is filling up and we just need to
* quickly identify a handful of points which can be deleted.
* Both algorithms are "anytime algorithms" meaning they can be interrupted
* before they complete which is helpful when memory is filling up and we just
* need to quickly identify a handful of points which can be deleted.
*
* Once the algorithms have completed the _simplify_complete and _prune_complete
* flags are set to true. The "thorough cleanup" procedure which is run as the
* vehicle initiates RTL, waits for these flags to become true. This can force
* the vehicle to pause for a few seconds before initiating the return journey.
* Once the algorithms have completed the _simplify_complete and
* _prune_complete flags are set to true. The "thorough cleanup" procedure,
* which is run as the vehicle initiates the SafeRTL flight mode, waits for
* these flags to become true. This can force the vehicle to pause for a few
* seconds before initiating the return journey.
*/
AP_SafeRTL::AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode) :
@ -77,11 +80,6 @@ AP_SafeRTL::AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode) :
_simplify_bitmask.setall();
}
AP_SafeRTL::~AP_SafeRTL()
{
delete _path_sem;
}
// initialise safe rtl including setting up background processes
void AP_SafeRTL::init()
{
@ -122,7 +120,7 @@ void AP_SafeRTL::init()
// create semaphore
_path_sem = hal.util->new_semaphore();
// when running the example sketch, we want the cleanup tasks to run when we tell them to, no in the background (so that they can be timed.)
// when running the example sketch, we want the cleanup tasks to run when we tell them to, not in the background (so that they can be timed.)
if (!_example_mode){
// register background cleanup to run in IO thread
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SafeRTL::run_background_cleanup, void));
@ -186,16 +184,13 @@ void AP_SafeRTL::reset_path(bool position_ok, const Vector3f& current_pos)
// de-activate if no position at take-off
if (!position_ok) {
_active = false;
log_action(SRTL_DEACTIVATED_BAD_POSITION);
gcs().send_text(MAV_SEVERITY_WARNING, "SafeRTL deactivated: bad position");
deactivate(SRTL_DEACTIVATED_BAD_POSITION, "bad position");
return;
}
// save current position as first point in path
if (!add_point(current_pos)) {
_active = false;
gcs().send_text(MAV_SEVERITY_WARNING, "SafeRTL deactivated: failed to save point");
deactivate(SRTL_ADD_FAILED_NO_SEMAPHORE, "failed to save initial point");
return;
}
@ -204,7 +199,7 @@ void AP_SafeRTL::reset_path(bool position_ok, const Vector3f& current_pos)
_active = true;
}
// call this a couple of times per second regardless of what mode the vehicle is in
// call this at 3hz (or higher) regardless of what mode the vehicle is in
void AP_SafeRTL::update(bool position_ok, bool save_position)
{
if (!_active || !position_ok || !save_position) {
@ -223,7 +218,7 @@ void AP_SafeRTL::update(bool position_ok, const Vector3f& current_pos)
}
if (position_ok) {
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
_last_good_position_ms = now;
// add the point
if (add_point(current_pos)) {
@ -233,17 +228,13 @@ void AP_SafeRTL::update(bool position_ok, const Vector3f& current_pos)
// check for timeout due to bad position
if (AP_HAL::millis() - _last_good_position_ms > SAFERTL_TIMEOUT) {
_active = false;
log_action(SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT);
gcs().send_text(MAV_SEVERITY_WARNING,"SafeRTL deactivated: bad position");
deactivate(SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT, "bad position");
return;
}
// check for timeout due to failure to save points to path (most likely due to buffer filling up)
if (AP_HAL::millis() - _last_position_save_ms > SAFERTL_TIMEOUT) {
_active = false;
log_action(SRTL_DEACTIVATED_PATH_FULL_TIMEOUT);
gcs().send_text(MAV_SEVERITY_WARNING,"SafeRTL deactivated: buffer full");
deactivate(SRTL_DEACTIVATED_PATH_FULL_TIMEOUT, "buffer full");
}
}
@ -330,7 +321,7 @@ void AP_SafeRTL::run_background_cleanup()
return;
}
// local copy of _path_points_count
uint16_t path_points_count = _path_points_count;
const uint16_t path_points_count = _path_points_count;
_path_sem->give();
// check if thorough cleanup is required
@ -345,11 +336,11 @@ void AP_SafeRTL::run_background_cleanup()
// we do not perform any further detection or cleanup until the requester acknowledges
// they have what they need by setting _thorough_clean_request_ms back to zero
return;
} else {
// ensure clean complete time is zero
_thorough_clean_complete_ms = 0;
}
// ensure clean complete time is zero
_thorough_clean_complete_ms = 0;
// check if path array is nearly full, if yes we should do a routine cleanup (i.e. remove 10 points)
bool path_nearly_full = path_points_count >= MAX(_path_points_max - SAFERTL_CLEANUP_START_MARGIN, 0);
if (path_nearly_full) {
@ -379,7 +370,7 @@ void AP_SafeRTL::run_background_cleanup()
// the calls to remove_empty_points causes the detect_ algorithms to begin their calculations from scratch
void AP_SafeRTL::routine_cleanup()
{
uint16_t potential_amount_to_simplify = _simplify_bitmask.size() - _simplify_bitmask.count();
const uint16_t potential_amount_to_simplify = _simplify_bitmask.size() - _simplify_bitmask.count();
// if simplifying will remove more than 10 points, just do it
if (potential_amount_to_simplify >= SAFERTL_CLEANUP_POINT_MIN) {
@ -548,7 +539,7 @@ void AP_SafeRTL::detect_loops()
const uint32_t start_time_us = AP_HAL::micros();
// run for defined amount of time
while (AP_HAL::micros() - start_time_us < SAFERTL_LOOP_TIME_US) {
while (AP_HAL::micros() - start_time_us < SAFERTL_PRUNING_LOOP_TIME_US) {
// advance inner loop
_prune_j++;
@ -596,7 +587,7 @@ void AP_SafeRTL::reset_if_new_points(uint16_t path_points_count)
}
// reset simplification algorithm so that it will re-check all points in the path
// should be called if the existing path is altered for example when a loop as been removed
// should be called if the existing path has been altered, for example when a loop as been removed
void AP_SafeRTL::reset_simplification(uint16_t path_points_count)
{
_simplify_complete = false;
@ -620,7 +611,7 @@ void AP_SafeRTL::reset_pruning(uint16_t path_points_count)
// set all points that can be removed to zero
void AP_SafeRTL::zero_points_by_simplify_bitmask()
{
for (uint16_t i = 0; i < _path_points_count; i++) {
for (uint16_t i = 1; i < _path_points_count; i++) {
if (!_simplify_bitmask.get(i)) {
if (!_path[i].is_zero()) {
log_action(SRTL_POINT_SIMPLIFY, _path[i]);
@ -657,14 +648,14 @@ void AP_SafeRTL::zero_points_by_loops(uint16_t points_to_delete)
*/
void AP_SafeRTL::remove_empty_points()
{
uint16_t src = 0;
uint16_t dest = 0;
uint16_t dest = 1;
uint16_t removed = 0;
while (++src < _path_points_count) { // never removes the first point
if (!_path[src].is_zero()) {
_path[++dest] = _path[src];
} else {
for (uint16_t src = 1; src < _path_points_count; src++) {
if (_path[src].is_zero()) {
removed++;
} else {
_path[dest] = _path[src];
dest++;
}
}
_path_points_count -= removed;
@ -678,21 +669,22 @@ void AP_SafeRTL::remove_empty_points()
* Returns the closest distance in 3D space between any part of two input segments, defined from p1 to p2 and from p3 to p4.
* Also returns the point which is halfway between
*
* Limitation: This function does not work for parallel lines. In this case, it will return FLT_MAX. This does not matter for the path cleanup algorithm because
* the pruning will still occur fine between the first parallel segment and a segment which is directly before or after the second segment.
* Limitation: This function does not work for parallel lines. In this case, dist_point.distance will be FLT_MAX.
* This does not matter for the path cleanup algorithm because the pruning will still occur fine between the first
* parallel segment and a segment which is directly before or after the second segment.
*/
AP_SafeRTL::dist_point AP_SafeRTL::segment_segment_dist(const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4)
{
Vector3f line1 = p2-p1;
Vector3f line2 = p4-p3;
Vector3f line_start_diff = p1-p3; // from the beginning of the second line to the beginning of the first line
const Vector3f line1 = p2-p1;
const Vector3f line2 = p4-p3;
const Vector3f line_start_diff = p1-p3; // from the beginning of the second line to the beginning of the first line
// these don't really have a physical representation. They're only here to break up the longer formulas below.
float a = line1*line1;
float b = line1*line2;
float c = line2*line2;
float d = line1*line_start_diff;
float e = line2*line_start_diff;
const float a = line1*line1;
const float b = line1*line2;
const float c = line2*line2;
const float d = line1*line_start_diff;
const float e = line2*line_start_diff;
// the parameter for the position on line1 and line2 which define the closest points.
float t1 = 0.0f;
@ -702,24 +694,32 @@ AP_SafeRTL::dist_point AP_SafeRTL::segment_segment_dist(const Vector3f &p1, cons
// could always be pruned start/end of the previous/subsequent line segment
if (is_zero((a*c)-(b*b))) {
return {FLT_MAX, Vector3f(0.0f, 0.0f, 0.0f)};
} else {
t1 = (b*e-c*d)/(a*c-b*b);
t2 = (a*e-b*d)/(a*c-b*b);
// restrict both parameters between 0 and 1.
t1 = constrain_float(t1, 0.0f, 1.0f);
t2 = constrain_float(t2, 0.0f, 1.0f);
// difference between two closest points
Vector3f dP = line_start_diff+line1*t1-line2*t2;
Vector3f midpoint = (p1+line1*t1 + p3+line2*t2)/2.0f;
return {dP.length(), midpoint};
}
t1 = (b*e-c*d)/(a*c-b*b);
t2 = (a*e-b*d)/(a*c-b*b);
// restrict both parameters between 0 and 1.
t1 = constrain_float(t1, 0.0f, 1.0f);
t2 = constrain_float(t2, 0.0f, 1.0f);
// difference between two closest points
const Vector3f dP = line_start_diff+line1*t1-line2*t2;
const Vector3f midpoint = (p1+line1*t1 + p3+line2*t2)/2.0f;
return {dP.length(), midpoint};
}
// de-activate SafeRTL, send warning to GCS and log to dataflash
void AP_SafeRTL::deactivate(SRTL_Actions action, const char *reason)
{
_active = false;
log_action(action);
gcs().send_text(MAV_SEVERITY_WARNING, "SafeRTL deactivated: %s", reason);
}
// logging
void AP_SafeRTL::log_action(SRTL_Actions action, const Vector3f point)
void AP_SafeRTL::log_action(SRTL_Actions action, const Vector3f &point)
{
if (!_example_mode) {
DataFlash_Class::instance()->Log_Write_SRTL(_active, _path_points_count, _path_points_max, action, point);

16
libraries/AP_SafeRTL/AP_SafeRTL.h
View File

@ -19,10 +19,10 @@
#define SAFERTL_SIMPLIFY_STACK_LEN_MULT (2.0f/3.0f)+1 // simplify buffer size as compared to maximum number of points.
// The minimum is int((s/2-1)+min(s/2, SAFERTL_POINTS_MAX-s)), where s = pow(2, floor(log(SAFERTL_POINTS_MAX)/log(2)))
// To avoid this annoying math, a good-enough overestimate is ceil(SAFERTL_POINTS_MAX*2.0f/3.0f)
#define SAFERTL_SIMPLIFY_TIME_US 200 // maximum time (in microseconds) the simplification algorithm will run before returning
#define SAFERTL_SIMPLIFY_TIME_US 200 // maximum time (in microseconds) the simplification algorithm will run before returning
#define SAFERTL_PRUNING_DELTA (_accuracy * 0.99) // How many meters apart must two points be, such that we can assume that there is no obstacle between them. must be smaller than _ACCURACY parameter
#define SAFERTL_PRUNING_LOOP_BUFFER_LEN_MULT 0.25f // pruning loop buffer size as compared to maximum number of points
#define SAFERTL_LOOP_TIME_US 300 // maximum time (in microseconds) that the loop finding algorithm will run before returning
#define SAFERTL_PRUNING_LOOP_TIME_US 200 // maximum time (in microseconds) that the loop finding algorithm will run before returning
class AP_SafeRTL {
@ -30,7 +30,6 @@ public:
// constructor, destructor
AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode = false);
~AP_SafeRTL();
// initialise safe rtl including setting up background processes
void init();
@ -47,13 +46,13 @@ public:
// get next point on the path to home, returns true on success
bool pop_point(Vector3f& point);
// clear return path and set return location is position_ok is true. This should be called as part of the arming procedure
// clear return path and set return location if position_ok is true. This should be called as part of the arming procedure
// if position_ok is false, SafeRTL will not be available.
// example sketches use method that allows providing vehicle position directly
// example sketches use the method that allows providing vehicle position directly
void reset_path(bool position_ok);
void reset_path(bool position_ok, const Vector3f& current_pos);
// call this a couple of times per second regardless of what mode the vehicle is in
// call this at 3hz (or higher) regardless of what mode the vehicle is in
// example sketches use method that allows providing vehicle position directly
void update(bool position_ok, bool save_position);
void update(bool position_ok, const Vector3f& current_pos);
@ -142,8 +141,11 @@ private:
// get the closest distance between 2 line segments and the point midway between the closest points
static dist_point segment_segment_dist(const Vector3f& p1, const Vector3f& p2, const Vector3f& p3, const Vector3f& p4);
// de-activate SafeRTL, send warning to GCS and log to dataflash
void deactivate(SRTL_Actions action, const char *reason);
// logging
void log_action(SRTL_Actions action, const Vector3f point = Vector3f());
void log_action(SRTL_Actions action, const Vector3f &point = Vector3f());
// external references
const AP_AHRS& _ahrs;