Rover: add DOCK mode in rover

This mode will maneuver the rover towards a docking target automatically

Rover/Parameters.cpp

@@ -670,6 +670,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("MANUAL_OPTIONS", 53, ParametersG2, manual_options, 0),
 
+#if MODE_DOCK_ENABLED == ENABLED
+    // @Group: DOCK
+    // @Path: mode_dock.cpp
+    AP_SUBGROUPPTR(mode_dock_ptr, "DOCK", 54, ParametersG2, ModeDock),
+#endif
+
     AP_GROUPEND
 };
 
@@ -713,6 +719,9 @@ ParametersG2::ParametersG2(void)
     wheel_rate_control(wheel_encoder),
     attitude_control(),
     smart_rtl(),
+#if MODE_DOCK_ENABLED == ENABLED
+    mode_dock_ptr(&rover.mode_dock),
+#endif
 #if HAL_PROXIMITY_ENABLED
     proximity(),
 #endif

Rover/Parameters.h

@@ -341,6 +341,11 @@ public:
     AP_Proximity proximity;
 #endif
 
+#if MODE_DOCK_ENABLED == ENABLED
+    // we need a pointer to the mode for the G2 table
+    class ModeDock *mode_dock_ptr;
+#endif
+
     // avoidance library
     AC_Avoid avoid;
 

Rover/Rover.h

@@ -90,6 +90,9 @@ public:
     friend class ModeSmartRTL;
     friend class ModeFollow;
     friend class ModeSimple;
+#if MODE_DOCK_ENABLED == ENABLED
+    friend class ModeDock;
+#endif
 
     friend class RC_Channel_Rover;
     friend class RC_Channels_Rover;
@@ -230,6 +233,9 @@ private:
     ModeSmartRTL mode_smartrtl;
     ModeFollow mode_follow;
     ModeSimple mode_simple;
+#if MODE_DOCK_ENABLED == ENABLED
+    ModeDock mode_dock;
+#endif
 
     // cruise throttle and speed learning
     typedef struct {

Rover/config.h

@@ -81,6 +81,12 @@
 
 #define DEFAULT_LOG_BITMASK    0xffff
 
+//////////////////////////////////////////////////////////////////////////////
+// Dock mode - allows vehicle to dock to a docking target
+#ifndef MODE_DOCK_ENABLED
+# define MODE_DOCK_ENABLED PRECISION_LANDING
+#endif
+
 
 //////////////////////////////////////////////////////////////////////////////
 // Developer Items

Rover/mode.cpp

@@ -550,6 +550,11 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::INITIALISING:
         ret = &mode_initializing;
         break;
+#if MODE_DOCK_ENABLED == ENABLED
+    case Mode::Number::DOCK:
+        ret = (Mode *)g2.mode_dock_ptr;
+        break;
+#endif
     default:
         break;
     }

Rover/mode.h

@@ -19,11 +19,14 @@ public:
         LOITER       = 5,
         FOLLOW       = 6,
         SIMPLE       = 7,
+#if MODE_DOCK_ENABLED == ENABLED
+        DOCK         = 8,
+#endif
         AUTO         = 10,
         RTL          = 11,
         SMART_RTL    = 12,
         GUIDED       = 15,
-        INITIALISING = 16
+        INITIALISING = 16,
     };
 
     // Constructor
@@ -744,3 +747,54 @@ private:
     float _desired_heading_cd;  // latest desired heading (in centi-degrees) from pilot
 };
 
+#if MODE_DOCK_ENABLED == ENABLED
+class ModeDock : public Mode
+{
+public:
+
+    // need a constructor for parameters
+    ModeDock(void);
+
+    // Does not allow copies
+    CLASS_NO_COPY(ModeDock);
+
+    uint32_t mode_number() const override { return DOCK; }
+    const char *name4() const override { return "DOCK"; }
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    bool is_autopilot_mode() const override { return true; }
+
+    // return distance (in meters) to destination
+    float get_distance_to_destination() const override { return _distance_to_destination; }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+
+    AP_Float speed; // dock mode speed
+    AP_Float desired_dir; // desired direction of approach
+    AP_Int8 hdg_corr_enable; // enable heading correction
+    AP_Float hdg_corr_weight; // heading correction weight
+    AP_Float stopping_dist; // how far away from the docking target should we start stopping
+
+    bool _enter() override;
+
+    // return reduced speed of vehicle based on error in position and current distance from the dock
+    float apply_slowdown(float desired_speed);
+
+    // calculate position of dock relative to the vehicle
+    bool calc_dock_pos_rel_vehicle_NE(Vector2f &dock_pos_rel_vehicle) const;
+
+    // we force the vehicle to use real dock target vector when this much close to the docking station
+    const float _force_real_target_limit_cm = 300.0f;
+    // acceptable lateral error in vehicle's position with respect to dock. This is used while slowing down the vehicle
+    const float _acceptable_pos_error_cm = 20.0f;
+
+    Vector2f _dock_pos_rel_origin_cm;   // position vector towards docking target relative to ekf origin
+    Vector2f _desired_heading_NE;       // unit vector in desired direction of docking
+    bool _docking_complete = false;     // flag to mark docking complete when we are close enough to the dock
+    bool _loitering = false; // true if we are loitering after mission completion
+};
+#endif

Rover/mode_dock.cpp

@@ -0,0 +1,252 @@
+#include "Rover.h"
+
+#if MODE_DOCK_ENABLED == ENABLED
+
+const AP_Param::GroupInfo ModeDock::var_info[] = {
+    // @Param: _SPEED
+    // @DisplayName: Dock mode speed
+    // @Description: Vehicle speed limit in dock mode
+    // @Units: m/s
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("_SPEED", 1, ModeDock, speed, 0.0f),
+
+    // @Param: _DIR
+    // @DisplayName: Dock mode direction of approach
+    // @Description: Compass direction in which vehicle should approach towards dock. -1 value represents unset parameter
+    // @Units: deg
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("_DIR", 2, ModeDock, desired_dir, -1.00f),
+
+    // @Param: _HDG_CORR_EN
+    // @DisplayName: Dock mode heading correction enable/disable
+    // @Description: When enabled, the autopilot modifies the path to approach the target head-on along desired line of approch in dock mode
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("_HDG_CORR_EN", 3, ModeDock, hdg_corr_enable, 0),
+
+    // @Param: _HDG_CORR_WT
+    // @DisplayName: Dock mode heading correction weight
+    // @Description: This value describes how aggressively vehicle tries to correct its heading to be on desired line of approach
+    // @Range: 0.00 0.90
+    // @Increment: 0.05
+    // @User: Advanced
+    AP_GROUPINFO("_HDG_CORR_WT", 4, ModeDock, hdg_corr_weight, 0.75f),
+
+    // @Param: _STOP_DIST
+    // @DisplayName: Distance from docking target when we should stop
+    // @Description: The vehicle starts stopping when it is this distance away from docking target
+    // @Units: m
+    // @Range: 0 2
+    // @Increment: 0.01
+    // @User: Standard
+    AP_GROUPINFO("_STOP_DIST", 5, ModeDock, stopping_dist, 0.30f),
+
+    AP_GROUPEND
+};
+
+ModeDock::ModeDock(void) : Mode()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+#define AR_DOCK_ACCEL_MAX              20.0    // acceleration used when user has specified no acceleration limit
+
+// initialize dock mode
+bool ModeDock::_enter()
+{
+    // refuse to enter the mode if dock is not in sight
+    if (!rover.precland.enabled() || !rover.precland.target_acquired()) {
+        return false;
+    }
+
+    if (hdg_corr_enable && is_negative(desired_dir)) {
+        // DOCK_DIR is required for heading correction
+        gcs().send_text(MAV_SEVERITY_NOTICE, "Dock: Set DOCK_DIR or disable heading correction");
+        return false;
+    }
+
+    // set speed limit to dock_speed param if available
+    // otherwise the vehicle uses wp_nav default speed limit
+    const float speed_max = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
+    float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
+    if (!is_positive(atc_accel_max)) {
+        // accel_max of zero means no limit so use maximum acceleration
+        atc_accel_max = AR_DOCK_ACCEL_MAX;
+    }
+    const float accel_max = is_positive(g2.wp_nav.get_default_accel()) ? MIN(g2.wp_nav.get_default_accel(), atc_accel_max) : atc_accel_max;
+    const float jerk_max = is_positive(g2.wp_nav.get_default_jerk()) ? g2.wp_nav.get_default_jerk() : accel_max;
+
+    // initialise position controller
+    g2.pos_control.set_limits(speed_max, accel_max, g2.attitude_control.get_turn_lat_accel_max(), jerk_max);
+    g2.pos_control.init();
+
+    // set the position controller reversed in case the camera is mounted on vehicle's back
+    g2.pos_control.set_reversed(rover.precland.get_orient() == 4);
+
+    // construct unit vector in the desired direction from where we want the vehicle to approach the dock
+    // this helps to dock the vehicle head-on even when we enter the dock mode at an angle towards the dock
+    _desired_heading_NE = Vector2f{cosf(radians(desired_dir)), sinf(radians(desired_dir))};
+
+    _docking_complete = false;
+
+    return true;
+}
+
+void ModeDock::update()
+{
+    // if docking is complete, rovers stop and boats loiter
+    if (_docking_complete) {
+        // rovers stop, boats loiter 
+        // note that loiter update must be called after successfull initialisation on mode loiter
+        if (_loitering) {
+            // mode loiter must be initialised before calling update method
+            rover.mode_loiter.update();
+        } else {
+            stop_vehicle();
+        }
+        return;
+    }
+
+    const bool real_dock_in_sight = rover.precland.get_target_position_cm(_dock_pos_rel_origin_cm);
+    Vector2f dock_pos_rel_vehicle_cm;
+    if (!calc_dock_pos_rel_vehicle_NE(dock_pos_rel_vehicle_cm)) {
+        g2.motors.set_throttle(0.0f);
+        g2.motors.set_steering(0.0f);
+        return;
+    }
+
+    _distance_to_destination = dock_pos_rel_vehicle_cm.length() * 0.01f;
+
+    // we force the vehicle to use real dock as target when we are too close to the dock
+    // note that heading correction does not work when we force real target
+    const bool force_real_target = _distance_to_destination < _force_real_target_limit_cm * 0.01f;
+
+    // if we are close enough to the dock or the target is not in sight when we strictly require it
+    // we mark the docking to be completed so that the vehicle stops
+    if (_distance_to_destination <= stopping_dist || (force_real_target && !real_dock_in_sight)) {
+        _docking_complete = true;
+
+        // send a one time notification to GCS
+        gcs().send_text(MAV_SEVERITY_INFO, "Dock: Docking complete");
+
+        // initialise mode loiter if it is a boat
+        if (rover.is_boat()) {
+            // if we fail to enter, we set _loitering to false
+            _loitering = rover.mode_loiter.enter();
+        }
+        return;
+    }
+
+    Vector2f target_cm = _dock_pos_rel_origin_cm;
+
+    // ***** HEADING CORRECTION *****
+    // to make to vehicle dock from a given direction we simulate a virtual moving target on the line of approach
+    // this target is always at DOCK_HDG_COR_WT times the distance from dock to vehicle (along the line of approach)
+    // For e.g., if the dock is 100 m away form dock, DOCK_HDG_COR_WT is 0.75
+    // then the position target is 75 m from the dock, i.e., 25 m from the vehicle
+    // as the vehicle tries to reach this target, this target appears to move towards the dock and at last it is sandwiched b/w dock and vehicle
+    // since this target is moving along desired direction of approach, the vehicle also comes on that line while following it
+    if  (!force_real_target && hdg_corr_enable) {
+        const float correction_vec_mag = hdg_corr_weight * dock_pos_rel_vehicle_cm.projected(_desired_heading_NE).length();
+        target_cm = _dock_pos_rel_origin_cm - _desired_heading_NE * correction_vec_mag;
+    }
+
+    const Vector2p target_pos { target_cm.topostype() * 0.01 };
+    g2.pos_control.input_pos_target(target_pos, rover.G_Dt);
+    g2.pos_control.update(rover.G_Dt);
+
+    // get desired speed and turn rate from pos_control
+    float desired_speed = g2.pos_control.get_desired_speed();
+    float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
+
+    // slow down the vehicle as we approach the dock
+    desired_speed = apply_slowdown(desired_speed);
+
+    // run steering and throttle controllers
+    calc_steering_from_turn_rate(desired_turn_rate);
+    calc_throttle(desired_speed, true);
+
+// @LoggerMessage: DOCK
+// @Description: Dock mode target information
+// @Field: TimeUS: Time since system startup
+// @Field: DockX: Docking Station position, X-Axis
+// @Field: DockY: Docking Station position, Y-Axis
+// @Field: DockDist: Distance to docking station
+// @Field: TPosX: Current Position Target, X-Axis
+// @Field: TPosY: Current Position Target, Y-Axis
+// @Field: DSpd: Desired speed
+// @Field: DTrnRt: Desired Turn Rate
+
+        AP::logger().WriteStreaming(
+            "DOCK",
+            "TimeUS,DockX,DockY,DockDist,TPosX,TPosY,DSpd,DTrnRt",
+            "smmmmmnE",
+            "FBB0BB00",
+            "Qfffffff",
+            AP_HAL::micros64(),
+            _dock_pos_rel_origin_cm.x,
+            _dock_pos_rel_origin_cm.y,
+            _distance_to_destination,
+            target_cm.x,
+            target_cm.y,
+            desired_speed,
+            desired_turn_rate);
+}
+
+float ModeDock::apply_slowdown(float desired_speed)
+{
+    const float dock_speed_slowdown_lmt = 0.5f;
+
+    // no slowdown for speed below dock_speed_slowdown_lmt
+    if (fabsf(desired_speed) < dock_speed_slowdown_lmt) {
+        return desired_speed;
+    }
+
+    Vector3f target_vec_rel_vehicle_NED;
+    if(!calc_dock_pos_rel_vehicle_NE(target_vec_rel_vehicle_NED.xy())) {
+        return desired_speed;
+    }
+
+    const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned();
+    Vector3f target_vec_body = body_to_ned.mul_transpose(target_vec_rel_vehicle_NED);
+    const float target_error_cm = fabsf(target_vec_body.y);
+    float error_ratio = target_error_cm / _acceptable_pos_error_cm;
+    error_ratio = constrain_float(error_ratio, 0.0f, 1.0f);
+
+    const float dock_slow_dist_max_m = 15.0f;
+    const float dock_slow_dist_min_m = 5.0f;
+    // approach slowdown is not applied when the vehicle is more than dock_slow_dist_max meters away
+    // within dock_slow_dist_max and dock_slow_dist_min the weight of the slowdown increases linearly
+    // once the vehicle reaches dock_slow_dist_min the slowdown weight becomes 1
+    float slowdown_weight = 1 - (target_vec_body.x * 0.01f - dock_slow_dist_min_m) / (dock_slow_dist_max_m - dock_slow_dist_min_m);
+    slowdown_weight = constrain_float(slowdown_weight, 0.0f, 1.0f);
+    
+    desired_speed = MAX(dock_speed_slowdown_lmt, fabsf(desired_speed) * (1 - error_ratio * slowdown_weight));
+
+    // we worked on absolute value of speed before
+    // make it negative again if reversed
+    if (g2.pos_control.get_reversed()) {
+        desired_speed *= -1;
+    }
+
+    return desired_speed;
+}
+
+// calculate position of dock relative to the vehicle
+// we need this method here because there can be a window during heading correction when we might lose the target
+// during that window precland won't be able to give us this vector
+// we can calculate it based on most recent value from precland because the dock is assumed stationary wrt ekf origin
+bool ModeDock::calc_dock_pos_rel_vehicle_NE(Vector2f &dock_pos_rel_vehicle) const {
+    Vector2f current_pos_m;
+    if (!AP::ahrs().get_relative_position_NE_origin(current_pos_m)) {
+        return false;
+    }
+ 
+    dock_pos_rel_vehicle = _dock_pos_rel_origin_cm - current_pos_m * 100.0f;
+    return true;
+}
+#endif // MODE_DOCK_ENABLED