From d8cfb8061970ce7fa63f419090611de926216d1f Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 10 Jul 2022 09:32:55 +0530 Subject: [PATCH] Rover: add DOCK mode in rover This mode will maneuver the rover towards a docking target automatically --- Rover/Parameters.cpp | 9 ++ Rover/Parameters.h | 5 + Rover/Rover.h | 6 ++ Rover/config.h | 6 ++ Rover/mode.cpp | 5 + Rover/mode.h | 56 +++++++++- Rover/mode_dock.cpp | 252 +++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 338 insertions(+), 1 deletion(-) create mode 100644 Rover/mode_dock.cpp diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index eaac76db44..19d755afd8 100644 --- a/Rover/Parameters.cpp +++ b/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 diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 435e1a7e14..cf9513fe6b 100644 --- a/Rover/Parameters.h +++ b/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; diff --git a/Rover/Rover.h b/Rover/Rover.h index 45c43bd851..87ec777627 100644 --- a/Rover/Rover.h +++ b/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 { diff --git a/Rover/config.h b/Rover/config.h index a631a5ad32..48b4c9aa63 100644 --- a/Rover/config.h +++ b/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 diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 21765ebadf..76898d9bf6 100644 --- a/Rover/mode.cpp +++ b/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; } diff --git a/Rover/mode.h b/Rover/mode.h index 496ec2c948..e12fd710cf 100644 --- a/Rover/mode.h +++ b/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 diff --git a/Rover/mode_dock.cpp b/Rover/mode_dock.cpp new file mode 100644 index 0000000000..e11124788e --- /dev/null +++ b/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