Rover: Loiter mode for boats

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,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,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,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,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,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,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,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,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,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,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,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode6,           "MODE6",         MANUAL),
 

APMrover2/Rover.h

@@ -113,6 +113,7 @@ public:
     friend class ModeAuto;
     friend class ModeGuided;
     friend class ModeHold;
+    friend class ModeLoiter;
     friend class ModeSteering;
     friend class ModeManual;
     friend class ModeRTL;
@@ -372,6 +373,7 @@ private:
     ModeAcro mode_acro;
     ModeGuided mode_guided;
     ModeAuto mode_auto;
+    ModeLoiter mode_loiter;
     ModeSteering mode_steering;
     ModeRTL mode_rtl;
     ModeSmartRTL mode_smartrtl;

APMrover2/control_modes.cpp

@@ -18,6 +18,9 @@ Mode *Rover::mode_from_mode_num(const enum mode num)
     case HOLD:
         ret = &mode_hold;
         break;
+    case LOITER:
+        ret = &mode_loiter;
+        break;
     case AUTO:
         ret = &mode_auto;
         break;

APMrover2/defines.h

@@ -40,6 +40,7 @@ enum mode {
     ACRO         = 1,
     STEERING     = 3,
     HOLD         = 4,
+    LOITER       = 5,
     AUTO         = 10,
     RTL          = 11,
     SMART_RTL    = 12,

APMrover2/mode.h

@@ -303,6 +303,23 @@ public:
     bool requires_velocity() const override { return false; }
 };
 
+class ModeLoiter : public Mode
+{
+public:
+
+    uint32_t mode_number() const override { return LOITER; }
+    const char *name4() const override { return "LOIT"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    // return distance (in meters) to destination
+    float get_distance_to_destination() const override { return _distance_to_destination; }
+
+protected:
+
+    bool _enter() override;
+};
 
 class ModeManual : public Mode
 {

APMrover2/mode_loiter.cpp

@@ -0,0 +1,65 @@
+#include "mode.h"
+#include "Rover.h"
+
+bool ModeLoiter::_enter()
+{
+    // set _destination to reasonable stopping point
+    calc_stopping_location(_destination);
+
+    // initialise desired speed to current speed
+    if (!attitude_control.get_forward_speed(_desired_speed)) {
+        _desired_speed = 0.0f;
+    }
+
+    // initialise heading to current heading
+    _desired_yaw_cd = ahrs.yaw_sensor;
+    _yaw_error_cd = 0.0f;
+
+    // set reversed based on speed
+    rover.set_reverse(is_negative(_desired_speed));
+
+    return true;
+}
+
+void ModeLoiter::update()
+{
+    // get distance (in meters) to destination
+    _distance_to_destination = get_distance(rover.current_loc, _destination);
+
+    // if within waypoint radius slew desired speed towards zero and use existing desired heading
+    if (_distance_to_destination <= g.waypoint_radius) {
+        if (is_negative(_desired_speed)) {
+            _desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
+        } else {
+            _desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
+        }
+        _yaw_error_cd = 0.0f;
+    } else {
+        // P controller with hard-coded gain to convert distance to desired speed
+        // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
+        _desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
+
+        // calculate bearing to destination
+        _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
+        _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
+        // if destination is behind vehicle, reverse towards it
+        if (fabsf(_yaw_error_cd) > 9000) {
+            _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
+            _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
+            _desired_speed = -_desired_speed;
+        }
+
+        // reduce desired speed if yaw_error is large
+        // note: copied from calc_reduced_speed_for_turn_or_distance
+        float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
+        _desired_speed *= yaw_error_ratio;
+    }
+
+    // run steering and throttle controllers
+    calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
+    calc_throttle(_desired_speed, false, true);
+
+    // mark us as in_reverse when using a negative throttle
+    // To-Do: only in reverse if vehicle is actually travelling backwards?
+    rover.set_reverse(_desired_speed < 0);
+}