Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_HAL/AP_HAL.h>
       4             : #include <AP_Common/Bitmask.h>
       5             : #include <AP_Math/AP_Math.h>
       6             : #include <AP_Logger/AP_Logger_config.h>
       7             : 
       8             : // definitions and macros
       9             : #define SMARTRTL_ACCURACY_DEFAULT        2.0f   // default _ACCURACY parameter value.  Points will be no closer than this distance (in meters) together.
      10             : #define SMARTRTL_POINTS_DEFAULT          300    // default _POINTS parameter value.  High numbers improve path pruning but use more memory and CPU for cleanup. Memory used will be 20bytes * this number.
      11             : #define SMARTRTL_POINTS_MAX              500    // the absolute maximum number of points this library can support.
      12             : #define SMARTRTL_TIMEOUT                 15000  // the time in milliseconds with no points saved to the path (for whatever reason), before SmartRTL is disabled for the flight
      13             : #define SMARTRTL_CLEANUP_POINT_TRIGGER   50     // simplification will trigger when this many points are added to the path
      14             : #define SMARTRTL_CLEANUP_START_MARGIN    10     // routine cleanup algorithms begin when the path array has only this many empty slots remaining
      15             : #define SMARTRTL_CLEANUP_POINT_MIN       10     // cleanup algorithms will remove points if they remove at least this many points
      16             : #define SMARTRTL_SIMPLIFY_EPSILON (_accuracy * 0.5f)
      17             : #define SMARTRTL_SIMPLIFY_STACK_LEN_MULT (2.0f/3.0f)+1  // simplify buffer size as compared to maximum number of points.
      18             :                                                 // The minimum is int((s/2-1)+min(s/2, SMARTRTL_POINTS_MAX-s)), where s = pow(2, floor(log(SMARTRTL_POINTS_MAX)/log(2)))
      19             :                                                 // To avoid this annoying math, a good-enough overestimate is ceil(SMARTRTL_POINTS_MAX*2.0f/3.0f)
      20             : #define SMARTRTL_SIMPLIFY_TIME_US        200    // maximum time (in microseconds) the simplification algorithm will run before returning
      21             : #define SMARTRTL_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
      22             : #define SMARTRTL_PRUNING_LOOP_BUFFER_LEN_MULT 0.25f // pruning loop buffer size as compared to maximum number of points
      23             : #define SMARTRTL_PRUNING_LOOP_TIME_US    200    // maximum time (in microseconds) that the loop finding algorithm will run before returning
      24             : 
      25             : class AP_SmartRTL {
      26             : 
      27             : public:
      28             : 
      29             :     // constructor, destructor
      30             :     AP_SmartRTL(bool example_mode = false);
      31             : 
      32             :     // Return true if SmartRTL is enabled
      33             :     bool enabled() const;
      34             : 
      35             :     // initialise safe rtl including setting up background processes
      36             :     void init();
      37             : 
      38             :     // return true if smart_rtl is usable (it may become unusable if the user took off without GPS lock or the path became too long)
      39          19 :     bool is_active() const { return _active; }
      40             : 
      41             :     // returns number of points on the path
      42             :     uint16_t get_num_points() const;
      43             : 
      44             :     // get a point on the path
      45             :     const Vector3f& get_point(uint16_t index) const { return _path[index]; }
      46             : 
      47             :     // add point to end of path. returns true on success, false on failure (due to failure to take the semaphore)
      48             :     bool add_point(const Vector3p& point);
      49             : 
      50             :     // get next point on the path to home, returns true on success
      51             :     bool pop_point(Vector3p& point);
      52             : 
      53             :     // peek at next point on the path without removing it form the path. Returns true on success
      54             :     // this may fail if the IO thread has taken the path semaphore
      55             :     bool peek_point(Vector3p& point);
      56             : 
      57             :     // clear return path and set return location if position_ok is true.  This should be called as part of the arming procedure
      58             :     // if position_ok is false, SmartRTL will not be available.
      59             :     // example sketches use the method that allows providing vehicle position directly
      60             :     void set_home(bool position_ok);
      61             :     void set_home(bool position_ok, const Vector3p& current_pos);
      62             : 
      63             :     // call this at 3hz (or higher) regardless of what mode the vehicle is in
      64             :     // example sketches use method that allows providing vehicle position directly
      65             :     void update(bool position_ok, bool save_position);
      66             :     void update(bool position_ok, const Vector3p& current_pos);
      67             : 
      68             :     // enum for argument passed to request_through_cleanup
      69             :     enum ThoroughCleanupType {
      70             :         THOROUGH_CLEAN_DEFAULT = 0,     // perform simplify and prune (used by vehicle code)
      71             :         THOROUGH_CLEAN_ALL,             // same as above but used by example sketch
      72             :         THOROUGH_CLEAN_SIMPLIFY_ONLY,   // perform simplify only (used by example sketch)
      73             :         THOROUGH_CLEAN_PRUNE_ONLY,      // perform prune only (used by example sketch)
      74             :     };
      75             : 
      76             :     // triggers thorough cleanup including simplification, pruning and removal of all unnecessary points
      77             :     // returns true if the thorough cleanup was completed, false if it has not yet completed
      78             :     // this method should be called repeatedly until it returns true before initiating the return journey
      79             :     // clean_type should only be set by the example sketch
      80             :     bool request_thorough_cleanup(ThoroughCleanupType clean_type = THOROUGH_CLEAN_DEFAULT);
      81             : 
      82             :     // cancel request for thorough cleanup
      83             :     void cancel_request_for_thorough_cleanup();
      84             : 
      85             :     // run background cleanup - should be run regularly from the IO thread
      86             :     void run_background_cleanup();
      87             : 
      88             :     // returns true if pilot's yaw input should be used to adjust vehicle's heading
      89             :     bool use_pilot_yaw(void) const;
      90             : 
      91             :     // parameter var table
      92             :     static const struct AP_Param::GroupInfo var_info[];
      93             : 
      94             : private:
      95             : 
      96             :     // enums for logging latest actions
      97             :     enum Action : uint8_t {
      98             :         POINT_ADD = 0,
      99             :         POINT_PRUNE = 1,
     100             :         POINT_SIMPLIFY = 2,
     101             :         ADD_FAILED_NO_SEMAPHORE = 3,
     102             :         ADD_FAILED_PATH_FULL = 4,
     103             :         POP_FAILED_NO_SEMAPHORE = 5,
     104             :         PEEK_FAILED_NO_SEMAPHORE = 6,
     105             :         DEACTIVATED_INIT_FAILED = 7,
     106             :         // DEACTIVATED_BAD_POSITION = 8,  unused, but historical
     107             :         DEACTIVATED_BAD_POSITION_TIMEOUT = 9,
     108             :         DEACTIVATED_PATH_FULL_TIMEOUT = 10,
     109             :         DEACTIVATED_PROGRAM_ERROR = 11,
     110             :     };
     111             : 
     112             :     // enum for SRTL_OPTIONS parameter
     113             :     enum class Options : int32_t {
     114             :         // bits 1 and 2 are still available, pilot yaw was mapped to bit 2 for symmetry with auto
     115             :         IgnorePilotYaw    = (1U << 2),
     116             :     };
     117             : 
     118             :     // routine cleanup attempts to remove 10 points (see SMARTRTL_CLEANUP_POINT_MIN definition) by simplification or loop pruning
     119             :     void routine_cleanup(uint16_t path_points_count, uint16_t path_points_complete_limit);
     120             : 
     121             :     // thorough cleanup simplifies and prunes all loops.  returns true if the cleanup was completed.
     122             :     // path_points_count is _path_points_count but passed in to avoid having to take the semaphore
     123             :     bool thorough_cleanup(uint16_t path_points_count, ThoroughCleanupType clean_type);
     124             : 
     125             :     // the two cleanup steps run from the background thread
     126             :     // these are public so that they can be tested by the example sketch
     127             :     void detect_simplifications();
     128             :     void detect_loops();
     129             : 
     130             :     // restart simplify or pruning if new points have been added to path
     131             :     // path_points_count is _path_points_count but passed in to avoid having to take the semaphore
     132             :     void restart_simplify_if_new_points(uint16_t path_points_count);
     133             : 
     134             :     // restart pruning if new points have been simplified
     135             :     void restart_pruning_if_new_points();
     136             : 
     137             :     // restart simplify algorithm so that detect_simplify will check all new points that have been added
     138             :     // to the path since it last completed.
     139             :     // path_points_count is _path_points_count but passed in to avoid having to take the semaphore
     140             :     void restart_simplification(uint16_t path_points_count);
     141             : 
     142             :     // reset simplify algorithm so that it will re-check all points in the path
     143             :     void reset_simplification();
     144             : 
     145             :     // restart pruning algorithm so that detect_loops will check all new points that have been added
     146             :     // to the path since it last completed.
     147             :     // path_points_count is _path_points_count but passed in to avoid having to take the semaphore
     148             :     void restart_pruning(uint16_t path_points_count);
     149             : 
     150             :     // reset pruning algorithm so that it will re-check all points in the path
     151             :     void reset_pruning();
     152             : 
     153             :     // remove all simplify-able points from the path
     154             :     void remove_points_by_simplify_bitmask();
     155             : 
     156             :     // remove loops until at least num_point_to_remove have been removed from path
     157             :     // does not necessarily prune all loops
     158             :     // returns false if it failed to remove points (because it could not take semaphore)
     159             :     bool remove_points_by_loops(uint16_t num_points_to_remove);
     160             : 
     161             :     // add loop to loops array
     162             :     //  returns true if loop added successfully, false on failure (because loop array is full)
     163             :     //  checks if loop overlaps with an existing loop, keeps only the longer loop
     164             :     //  example: segment_a(point2~point3) overlaps with segment_b (point5~point6), add_loop(3,5,midpoint)
     165             :     bool add_loop(uint16_t start_index, uint16_t end_index, const Vector3f& midpoint);
     166             : 
     167             :     // dist_point holds the closest distance reached between 2 line segments, and the point exactly between them
     168             :     typedef struct {
     169             :         float distance;
     170             :         Vector3f midpoint;
     171             :     } dist_point;
     172             : 
     173             :     // get the closest distance between 2 line segments and the point midway between the closest points
     174             :     static dist_point segment_segment_dist(const Vector3f& p1, const Vector3f& p2, const Vector3f& p3, const Vector3f& p4);
     175             : 
     176             :     // de-activate SmartRTL, send warning to GCS and logger
     177             :     void deactivate(Action action, const char *reason);
     178             : 
     179             : #if HAL_LOGGING_ENABLED
     180             :     // logging
     181             :     void log_action(Action action, const Vector3f &point = Vector3f()) const;
     182             : #else
     183             :     void log_action(Action action, const Vector3f &point = Vector3f()) const {}
     184             : #endif
     185             : 
     186             :     // parameters
     187             :     AP_Float _accuracy;
     188             :     AP_Int16 _points_max;
     189             :     AP_Int32 _options;
     190             : 
     191             :     // SmartRTL State Variables
     192             :     bool _active;       // true if SmartRTL is usable.  may become unusable if the path becomes too long to keep in memory, and too convoluted to be cleaned up, SmartRTL will be permanently deactivated (for the remainder of the flight)
     193             :     bool _example_mode; // true when being called from the example sketch, logging and background tasks are disabled
     194             :     bool _home_saved;   // true once home has been saved successfully by the set_home or update methods
     195             :     uint32_t _last_good_position_ms;    // the last system time a last good position was reported. If no position is available for a while, SmartRTL will be disabled.
     196             :     uint32_t _last_position_save_ms;    // the system time a position was saved to the path (used for timeout)
     197             :     uint32_t _thorough_clean_request_ms;// the last system time the thorough cleanup was requested (set by thorough_cleanup method, used by background cleanup)
     198             :     uint32_t _thorough_clean_complete_ms; // set to _thorough_clean_request_ms when the background thread completes the thorough cleanup
     199             :     uint32_t _last_low_space_notify_ms; //last time low on SmartRTL space was notified on Mavlink. Minimum time is required before re-notification to avoid nagging.
     200             :     ThoroughCleanupType _thorough_clean_type;   // used by example sketch to test simplify and prune separately
     201             : 
     202             :     // path variables
     203             :     Vector3f* _path;    // points are stored in meters from EKF origin in NED
     204             :     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
     205             :     uint16_t _path_points_count;// number of points in the path array
     206             :     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
     207             :     HAL_Semaphore _path_sem;   // semaphore for updating path
     208             : 
     209             :     // Simplify
     210             :     // structure and buffer to hold the "to-do list" for the simplify algorithm.
     211             :     typedef struct {
     212             :         uint16_t start;
     213             :         uint16_t finish;
     214             :     } simplify_start_finish_t;
     215             :     struct {
     216             :         bool complete;          // true after simplify_detection has completed
     217             :         bool removal_required;  // true if some simplify-able points have been found on the path, set true by detect_simplifications, set false by remove_points_by_simplify_bitmask
     218             :         uint16_t path_points_count; // copy of _path_points_count taken when the simply algorithm started
     219             :         uint16_t path_points_completed = SMARTRTL_POINTS_MAX; // number of points in that path that have already been simplified and should be ignored
     220             :         simplify_start_finish_t* stack;
     221             :         uint16_t stack_max;     // maximum number of elements in the _simplify_stack array
     222             :         uint16_t stack_count;   // number of elements in _simplify_stack array
     223             :         Bitmask<SMARTRTL_POINTS_MAX> bitmask;  // simplify algorithm clears bits for each point that can be removed
     224             :     } _simplify;
     225             : 
     226             :     // Pruning
     227             :     typedef struct {
     228             :         uint16_t start_index;   // index of the first point in the loop
     229             :         uint16_t end_index;     // index of the last point in the loop
     230             :         Vector3f midpoint;      // midpoint which should replace the first point when the loop is removed
     231             :         float length_squared;   // length squared (in meters) of the loop (used so we can remove the longest loops)
     232             :     } prune_loop_t;
     233             :     struct {
     234             :         bool complete;
     235             :         uint16_t path_points_count;  // copy of _path_points_count taken when the prune algorithm started
     236             :         uint16_t path_points_completed; // number of points in that path that have already been checked for loops and should be ignored
     237             :         uint16_t i;     // loop search's outer loop index
     238             :         uint16_t j;     // loop search's inner loop index
     239             :         prune_loop_t* loops;// the result of the pruning algorithm
     240             :         uint16_t loops_max; // maximum number of elements in the _prunable_loops array
     241             :         uint16_t loops_count;   // number of elements in the _prunable_loops array
     242             :     } _prune;
     243             : 
     244             :     // returns true if the two loops overlap (used within add_loop to determine which loops to keep or throw away)
     245             :     bool loops_overlap(const prune_loop_t& loop1, const prune_loop_t& loop2) const;
     246             : };