Rover: implement Follow mode

APMrover2/Parameters.cpp

@@ -33,7 +33,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: INITIAL_MODE
     // @DisplayName: Initial driving mode
     // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART.
-    // @Values: 0:MANUAL,1:ACRO,3:STEERING,4:HOLD,5:LOITER,10:AUTO,11:RTL,15:GUIDED
+    // @Values: 0:MANUAL,1:ACRO,3:STEERING,4:HOLD,5:LOITER,6:FOLLOW,10:AUTO,11:RTL,15:GUIDED
     // @User: Advanced
     GSCALAR(initial_mode,        "INITIAL_MODE",     Mode::Number::MANUAL),
 
@@ -112,7 +112,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: CH7_OPTION
     // @DisplayName: Channel 7 option
     // @Description: What to do use channel 7 for
-    // @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed,3:ArmDisarm,4:Manual,5:Acro,6:Steering,7:Hold,8:Auto,9:RTL,10:SmartRTL,11:Guided,12:Loiter
+    // @Values: 0:Nothing,1:SaveWaypoint,2:LearnCruiseSpeed,3:ArmDisarm,4:Manual,5:Acro,6:Steering,7:Hold,8:Auto,9:RTL,10:SmartRTL,11:Guided,12:Loiter,13:Follow
     // @User: Standard
     GSCALAR(ch7_option,             "CH7_OPTION",          CH7_OPTION),
 
@@ -224,7 +224,7 @@ const AP_Param::Info Rover::var_info[] = {
 
     // @Param: MODE1
     // @DisplayName: Mode1
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,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",         Mode::Number::MANUAL),
@@ -232,35 +232,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,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode2,           "MODE2",         Mode::Number::MANUAL),
 
     // @Param: MODE3
     // @DisplayName: Mode3
     // @Description: Driving mode for switch position 3 (1361 to 1490)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode3,           "MODE3",         Mode::Number::MANUAL),
 
     // @Param: MODE4
     // @DisplayName: Mode4
     // @Description: Driving mode for switch position 4 (1491 to 1620)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode4,           "MODE4",         Mode::Number::MANUAL),
 
     // @Param: MODE5
     // @DisplayName: Mode5
     // @Description: Driving mode for switch position 5 (1621 to 1749)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode5,           "MODE5",         Mode::Number::MANUAL),
 
     // @Param: MODE6
     // @DisplayName: Mode6
     // @Description: Driving mode for switch position 6 (1750 to 2049)
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:FOLLOW,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     GSCALAR(mode6,           "MODE6",         Mode::Number::MANUAL),
 
@@ -565,6 +565,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("CRASH_ANGLE", 22, ParametersG2, crash_angle, 0),
 
+    // @Group: FOLL
+    // @Path: ../libraries/AP_Follow/AP_Follow.cpp
+    AP_SUBGROUPINFO(follow, "FOLL", 23, ParametersG2, AP_Follow),
 
     AP_GROUPEND
 };
@@ -581,7 +584,8 @@ ParametersG2::ParametersG2(void)
     smart_rtl(),
     fence(rover.ahrs),
     proximity(rover.serial_manager),
-    avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon)
+    avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
+    follow()
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -347,6 +347,9 @@ public:
 
     // pitch/roll angle for crash check
     AP_Int8 crash_angle;
+
+    // follow mode library
+    AP_Follow follow;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -79,6 +79,7 @@
 #include <AC_Fence/AC_Fence.h>
 #include <AP_Proximity/AP_Proximity.h>
 #include <AC_Avoidance/AC_Avoid.h>
+#include <AP_Follow/AP_Follow.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <SITL/SITL.h>
 #endif
@@ -117,6 +118,7 @@ public:
     friend class ModeManual;
     friend class ModeRTL;
     friend class ModeSmartRTL;
+    friend class ModeFollow;
 
     Rover(void);
 
@@ -364,6 +366,7 @@ private:
     ModeSteering mode_steering;
     ModeRTL mode_rtl;
     ModeSmartRTL mode_smartrtl;
+    ModeFollow mode_follow;
 
     // cruise throttle and speed learning
     struct {

APMrover2/control_modes.cpp

@@ -33,6 +33,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::GUIDED:
        ret = &mode_guided;
         break;
+    case Mode::Number::FOLLOW:
+        ret = &mode_follow;
+        break;
     case Mode::Number::INITIALISING:
         ret = &mode_initializing;
         break;
@@ -263,5 +266,11 @@ void Rover::read_aux_switch()
     case CH7_LOITER:
         do_aux_function_change_mode(rover.mode_loiter, aux_ch7);
         break;
+
+    // Set mode to Follow
+    case CH7_FOLLOW:
+        do_aux_function_change_mode(rover.mode_follow, aux_ch7);
+        break;
+
     }
 }

APMrover2/defines.h

@@ -27,7 +27,8 @@ enum ch7_option {
     CH7_RTL             = 9,
     CH7_SMART_RTL       = 10,
     CH7_GUIDED          = 11,
-    CH7_LOITER          = 12
+    CH7_LOITER          = 12,
+    CH7_FOLLOW          = 13
 };
 
 // HIL enumerations

APMrover2/make.inc

@@ -50,3 +50,4 @@ LIBRARIES += AP_SmartRTL
 LIBRARIES += AC_Avoidance
 LIBRARIES += AC_AttitudeControl
 LIBRARIES += AP_Devo_Telem
+LIBRARIES += AP_Follow

APMrover2/mode.h

@@ -24,6 +24,7 @@ public:
         STEERING     = 3,
         HOLD         = 4,
         LOITER       = 5,
+        FOLLOW       = 6,
         AUTO         = 10,
         RTL          = 11,
         SMART_RTL    = 12,
@@ -465,3 +466,19 @@ public:
     bool has_manual_input() const override { return true; }
     bool attitude_stabilized() const override { return false; }
 };
+
+class ModeFollow : public ModeGuided
+{
+public:
+
+    uint32_t mode_number() const override { return FOLLOW; }
+    const char *name4() const override { return "FOLL"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+protected:
+
+    bool _enter() override;
+    uint32_t last_log_ms;   // system time of last time desired velocity was logging
+};

APMrover2/mode_follow.cpp

@@ -0,0 +1,57 @@
+#include "mode.h"
+#include "Rover.h"
+
+// initialize follow mode
+bool ModeFollow::_enter()
+{
+    return ModeGuided::enter();
+}
+
+void ModeFollow::update()
+{
+    // variables to be sent to velocity controller
+    Vector3f desired_velocity_neu_cms;
+    float yaw_cd = 0.0f;
+
+    Vector3f dist_vec;  //vector to lead vehicle
+    Vector3f dist_vec_offs; // vector to lead vehicle + offset
+    Vector3f vel_of_target; // velocity of lead vehicle
+
+    if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
+
+        // convert dist_vec_offs to cm in NEU
+        const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
+
+        // calculate desired velocity vector in cm/s in NEU
+        const float kp = g2.follow.get_pos_p().kP();
+        desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
+        desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
+
+        // scaled desired velocity to stay within horizontal speed limit
+        float desired_speed = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
+        if (!is_zero(desired_speed) && (desired_speed > _desired_speed)) {
+            const float scalar_xy = _desired_speed / desired_speed;
+            desired_velocity_neu_cms.x *= scalar_xy;
+            desired_velocity_neu_cms.y *= scalar_xy;
+            desired_speed = _desired_speed;
+        }
+
+        // unit vector towards target position (i.e. vector to lead vehicle + offset)
+        Vector3f dir_to_target_neu = dist_vec_offs_neu;
+        const float dir_to_target_neu_len = dir_to_target_neu.length();
+        if (!is_zero(dir_to_target_neu_len)) {
+            dir_to_target_neu /= dir_to_target_neu_len;
+        }
+
+        // calculate vehicle heading
+        const Vector3f dist_vec_xy(dist_vec_offs.x, dist_vec_offs.y, 0.0f);
+        if (dist_vec_xy.length() > 1.0f) {
+            yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
+        }
+    }
+
+    // re-use guided mode's heading and speed controller
+    ModeGuided::set_desired_heading_and_speed(yaw_cd, safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y)));
+
+    ModeGuided::update();
+}

APMrover2/wscript

@@ -24,6 +24,7 @@ def build(bld):
             'AC_Avoidance',
             'AC_AttitudeControl',
             'AP_Devo_Telem',
+            'AP_Follow',
         ],
     )