aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: add loiter radius parameter

Browse Source
This commit is contained in:
IamPete1 2018-09-25 22:09:21 +09:00 committed by Randy Mackay
parent e3d5db8db5
commit 100c494127
3 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
APMrover2/Parameters.cpp
View File

@ -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
};

1
APMrover2/Parameters.h
View File

@ -361,6 +361,7 @@ public:
// loiter type
AP_Int8 loit_type;
AP_Float loit_radius;
// Sprayer
AC_Sprayer sprayer;

4
APMrover2/mode_loiter.cpp
View File

@ -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);