Rover: add SmartRTL mode

called at 3hz from scheduler

APMrover2/APMrover2.cpp

@@ -72,6 +72,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(update_trigger,         50,    600),
     SCHED_TASK(gcs_failsafe_check,     10,    600),
     SCHED_TASK(compass_accumulate,     50,    900),
+    SCHED_TASK(smart_rtl_update,        3,    100),
     SCHED_TASK(update_notify,          50,    300),
     SCHED_TASK(one_second_loop,         1,   3000),
     SCHED_TASK(compass_cal_update,     50,    100),

APMrover2/Parameters.cpp

@@ -241,7 +241,7 @@ const AP_Param::Info Rover::var_info[] = {
 
     // @Param: MODE1
     // @DisplayName: Mode1
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
     GSCALAR(mode1,           "MODE1",         MANUAL),
@@ -249,35 +249,35 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: MODE2
     // @DisplayName: Mode2
     // @Description: Driving mode for switch position 2 (1231 to 1360)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode2,           "MODE2",         MANUAL),
 
     // @Param: MODE3
     // @DisplayName: Mode3
     // @Description: Driving mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode3,           "MODE3",         MANUAL),
 
     // @Param: MODE4
     // @DisplayName: Mode4
     // @Description: Driving mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode4,           "MODE4",         MANUAL),
 
     // @Param: MODE5
     // @DisplayName: Mode5
     // @Description: Driving mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode5,           "MODE5",         MANUAL),
 
     // @Param: MODE6
     // @DisplayName: Mode6
     // @Description: Driving mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode6,           "MODE6",         MANUAL),
 
@@ -513,6 +513,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ACRO_TURN_RATE", 12, ParametersG2, acro_turn_rate, 180.0f),
 
+    // @Group: SRTL_
+    // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
+    AP_SUBGROUPINFO(smart_rtl, "SRTL_", 13, ParametersG2, AP_SmartRTL),
+
     AP_GROUPEND
 };
 
@@ -524,7 +528,8 @@ ParametersG2::ParametersG2(void)
 #endif
     beacon(rover.serial_manager),
     motors(rover.ServoRelayEvents),
-    attitude_control(rover.ahrs)
+    attitude_control(rover.ahrs),
+    smart_rtl(rover.ahrs)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -319,6 +319,9 @@ public:
 
     // acro mode turn rate maximum
     AP_Float acro_turn_rate;
+
+    // Safe RTL library
+    AP_SmartRTL smart_rtl;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -67,6 +67,7 @@
 #include <AP_VisualOdom/AP_VisualOdom.h>
 #include <AP_WheelEncoder/AP_WheelEncoder.h>
 #include <APM_Control/AR_AttitudeControl.h>
+#include <AP_SmartRTL/AP_SmartRTL.h>
 #include <DataFlash/DataFlash.h>
 #include <Filter/AverageFilter.h>                   // Mode Filter from Filter library
 #include <Filter/Butter.h>                          // Filter library - butterworth filter
@@ -111,6 +112,7 @@ public:
     friend class ModeSteering;
     friend class ModeManual;
     friend class ModeRTL;
+    friend class ModeSmartRTL;
 
     Rover(void);
 
@@ -384,6 +386,7 @@ private:
     ModeAuto mode_auto;
     ModeSteering mode_steering;
     ModeRTL mode_rtl;
+    ModeSmartRTL mode_smartrtl;
 
     // cruise throttle and speed learning
     struct {
@@ -568,6 +571,7 @@ private:
     void change_arm_state(void);
     bool arm_motors(AP_Arming::ArmingMethod method);
     bool disarm_motors(void);
+    void smart_rtl_update();
 
     // test.cpp
     void print_hit_enter();

APMrover2/control_modes.cpp

@@ -24,6 +24,9 @@ Mode *Rover::control_mode_from_num(const enum mode num)
     case RTL:
         ret = &mode_rtl;
         break;
+    case SMART_RTL:
+        ret = &mode_smartrtl;
+        break;
     case GUIDED:
        ret = &mode_guided;
         break;

APMrover2/defines.h

@@ -33,6 +33,7 @@ enum mode {
     HOLD         = 4,
     AUTO         = 10,
     RTL          = 11,
+    SMART_RTL    = 12,
     GUIDED       = 15,
     INITIALISING = 16
 };

APMrover2/mode.h

@@ -338,6 +338,40 @@ protected:
     bool _enter() override;
 };
 
+class ModeSmartRTL : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return SMART_RTL; }
+    const char *name4() const override { return "SRTL"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    // attributes of the mode
+    bool is_autopilot_mode() const override { return true; }
+    bool failsafe_throttle_suppress() const override { return false; }
+
+    float get_distance_to_destination() const override { return _distance_to_destination; }
+    bool reached_destination() override { return smart_rtl_state == SmartRTL_StopAtHome; }
+
+    // save current position for use by the smart_rtl flight mode
+    void save_position(bool save_pos);
+
+protected:
+
+    // Safe RTL states
+    enum SmartRTLState {
+        SmartRTL_WaitForPathCleanup,
+        SmartRTL_PathFollow,
+        SmartRTL_StopAtHome,
+        SmartRTL_Failure
+    } smart_rtl_state;
+
+    bool _enter() override;
+    bool _load_point;
+};
+
 
 class ModeSteering : public Mode
 {

APMrover2/mode_smart_rtl.cpp

@@ -0,0 +1,79 @@
+#include "mode.h"
+#include "Rover.h"
+
+bool ModeSmartRTL::_enter()
+{
+    // SmartRTL requires EKF (not DCM)
+    Location ekf_origin;
+    if (!ahrs.get_origin(ekf_origin)) {
+        return false;
+    }
+
+    // refuse to enter SmartRTL if smart RTL's home has not been set
+    if (!g2.smart_rtl.is_active()) {
+        return false;
+    }
+
+    // RTL never reverses
+    rover.set_reverse(false);
+
+    // init state
+    smart_rtl_state = SmartRTL_WaitForPathCleanup;
+    _reached_destination = false;
+
+    return true;
+}
+
+void ModeSmartRTL::update()
+{
+    switch (smart_rtl_state) {
+        case SmartRTL_WaitForPathCleanup:
+            // check if return path is computed and if yes, begin journey home
+            if (g2.smart_rtl.request_thorough_cleanup()) {
+                smart_rtl_state = SmartRTL_PathFollow;
+                _load_point = true;
+            }
+            // Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely)
+            stop_vehicle();
+            break;
+
+        case SmartRTL_PathFollow:
+            // load point if required
+            if (_load_point) {
+                Vector3f next_point;
+                if (!g2.smart_rtl.pop_point(next_point)) {
+                    // if not more points, we have reached home
+                    gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
+                    smart_rtl_state = SmartRTL_StopAtHome;
+                    break;
+                }
+                _load_point = false;
+                // set target destination to new point
+                if (!set_desired_location_NED(next_point)) {
+                    gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
+                    smart_rtl_state = SmartRTL_Failure;
+                }
+            }
+            // check if we've reached the next point
+            _distance_to_destination = get_distance(rover.current_loc, _destination);
+            if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
+                _load_point = true;
+            }
+            // continue driving towards destination
+            calc_steering_to_waypoint(_origin, _destination);
+            calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
+            break;
+
+        case SmartRTL_StopAtHome:
+        case SmartRTL_Failure:
+            _reached_destination = true;
+            stop_vehicle();
+            break;
+    }
+}
+
+// save current position for use by the smart_rtl flight mode
+void ModeSmartRTL::save_position(bool save_pos)
+{
+    g2.smart_rtl.update(true, save_pos);
+}

APMrover2/system.cpp

@@ -133,6 +133,8 @@ void Rover::init_ardupilot()
     // give AHRS the range beacon sensor
     ahrs.set_beacon(&g2.beacon);
 
+    // initialize SmartRTL
+    g2.smart_rtl.init();
 
     init_capabilities();
 
@@ -332,6 +334,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
         return false;
     }
 
+    // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
+    g2.smart_rtl.reset_path(true);
+
     change_arm_state();
     return true;
 }
@@ -354,3 +359,10 @@ bool Rover::disarm_motors(void)
 
     return true;
 }
+
+// save current position for use by the smart_rtl mode
+void Rover::smart_rtl_update()
+{
+    const bool save_position = hal.util->get_soft_armed() && (control_mode != &mode_smartrtl);
+    mode_smartrtl.save_position(save_position);
+}