mirror of https://github.com/ArduPilot/ardupilot
Rover: add loiter radius parameter
This commit is contained in:
parent
e3d5db8db5
commit
100c494127
|
@ -599,6 +599,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||||
// @RebootRequired: True
|
// @RebootRequired: True
|
||||||
AP_GROUPINFO("SIMPLE_TYPE", 29, ParametersG2, simple_type, 0),
|
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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -361,6 +361,7 @@ public:
|
||||||
|
|
||||||
// loiter type
|
// loiter type
|
||||||
AP_Int8 loit_type;
|
AP_Int8 loit_type;
|
||||||
|
AP_Float loit_radius;
|
||||||
|
|
||||||
// Sprayer
|
// Sprayer
|
||||||
AC_Sprayer sprayer;
|
AC_Sprayer sprayer;
|
||||||
|
|
|
@ -24,13 +24,13 @@ void ModeLoiter::update()
|
||||||
_distance_to_destination = get_distance(rover.current_loc, _destination);
|
_distance_to_destination = get_distance(rover.current_loc, _destination);
|
||||||
|
|
||||||
// if within waypoint radius slew desired speed towards zero and use existing desired heading
|
// 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);
|
_desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
|
||||||
_yaw_error_cd = 0.0f;
|
_yaw_error_cd = 0.0f;
|
||||||
} else {
|
} else {
|
||||||
// P controller with hard-coded gain to convert distance to desired speed
|
// 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
|
// 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
|
// calculate bearing to destination
|
||||||
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
|
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
|
||||||
|
|
Loading…
Reference in New Issue