mirror of https://github.com/ArduPilot/ardupilot
259 lines
10 KiB
C++
259 lines
10 KiB
C++
#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()) {
|
|
gcs().send_text(MAV_SEVERITY_NOTICE, "Dock: target not 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);
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
// @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);
|
|
#endif
|
|
}
|
|
|
|
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));
|
|
|
|
// restrict speed to avoid going beyond stopping distance
|
|
desired_speed = MIN(desired_speed, safe_sqrt(2 * fabsf(_distance_to_destination - stopping_dist) * g2.pos_control.get_accel_max()));
|
|
|
|
// 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
|