Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: add loiter gain and max vehicle speed params

This commit is contained in:
Henry Wurzburg 2019-09-18 14:23:28 -05:00 committed by Randy Mackay
parent 06df44c184
commit 266457711a
4 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
APMrover2/Parameters.cpp
View File

@ -603,6 +603,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp
AP_SUBGROUPINFO(oa, "OA_", 45, ParametersG2, AP_OAPathPlanner),
// @Param: SPEED_MAX
// @DisplayName: Speed maximum
// @Description: Maximum speed vehicle can obtain at full throttle. If 0, it will be estimated based on CRUISE_SPEED and CRUISE_THROTTLE.
// @Units: m/s
// @Range: 0 30
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SPEED_MAX", 46, ParametersG2, speed_max, 0.0f),
// @Param: LOIT_SPEED_GAIN
// @DisplayName: Loiter speed gain
// @Description: Determines how agressively LOITER tries to correct for drift from loiter point. Higher is faster but default should be acceptable.
// @Range: 0 5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("LOIT_SPEED_GAIN", 47, ParametersG2, loiter_speed_gain, 0.5f),
AP_GROUPEND
};

6
APMrover2/Parameters.h
View File

@ -388,6 +388,12 @@ public:
// object avoidance path planning
AP_OAPathPlanner oa;
// maximum speed for vehicle
AP_Float speed_max;
// gain for speed of correction in loiter
AP_Float loiter_speed_gain;
};
extern const AP_Param::Info var_info[];

2
APMrover2/mode.cpp
View File

@ -335,6 +335,8 @@ float Mode::calc_speed_max(float cruise_speed, float cruise_throttle) const
// sanity checks
if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
speed_max = cruise_speed;
} else if (is_positive(g2.speed_max)) {
speed_max = g2.speed_max;
} else {
// project vehicle's maximum speed
speed_max = (1.0f / cruise_throttle) * cruise_speed;

6
APMrover2/mode_loiter.cpp
View File

@ -30,10 +30,8 @@ void ModeLoiter::update()
const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
_desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
} 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 - g2.loit_radius) * 0.5f, g2.wp_nav.get_default_speed());
// P controller with hard-coded gain to convert distance to desired speed
_desired_speed = MIN((_distance_to_destination - g2.loit_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
// calculate bearing to destination
_desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);