diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 66915de8ab..25517db691 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -599,6 +599,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @RebootRequired: True AP_GROUPINFO("SIMPLE_TYPE", 29, ParametersG2, simple_type, 0), + // @Param: LOIT_RADIUS + // @DisplayName: Loiter radius + // @Description: Vehicle will drift when within this distance of the target position + // @Units: m + // @Range: 0 20 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("LOIT_RADIUS", 30, ParametersG2, loit_radius, 2), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 6d8fd12b33..15c4ac85b4 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -361,6 +361,7 @@ public: // loiter type AP_Int8 loit_type; + AP_Float loit_radius; // Sprayer AC_Sprayer sprayer; diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index 050f4fbd4f..7bfbeda20a 100644 --- a/APMrover2/mode_loiter.cpp +++ b/APMrover2/mode_loiter.cpp @@ -24,13 +24,13 @@ void ModeLoiter::update() _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 (_distance_to_destination <= g2.loit_radius) { _desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt); _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); + _desired_speed = MIN((_distance_to_destination - g2.loit_radius) * 0.5f, g.speed_cruise); // calculate bearing to destination _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);