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

Rover: yaw_error_cd becomes local variable in Loiter only

This commit is contained in:
Randy Mackay 2019-05-04 15:29:18 +09:00
parent c4766ec143
commit 2b654983a9
4 changed files with 5 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/mode.cpp
View File

@ -369,7 +369,7 @@ float Mode::calc_speed_nudge(float target_speed, bool reversed)
// high level call to navigate to waypoint
// uses wp_nav to calculate turn rate and speed to drive along the path from origin to destination
// this function updates _distance_to_destination and _yaw_error_cd
// this function updates _distance_to_destination
void Mode::navigate_to_waypoint()
{
// update navigation controller
@ -382,8 +382,6 @@ void Mode::navigate_to_waypoint()
calc_throttle(desired_speed, true);
float desired_heading_cd = g2.wp_nav.wp_bearing_cd();
_yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
if (rover.sailboat_use_indirect_route(desired_heading_cd)) {
// sailboats use heading controller when tacking upwind
desired_heading_cd = rover.sailboat_calc_heading(desired_heading_cd);
@ -438,9 +436,6 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse
// rate_max is a maximum turn rate in deg/s. set to zero to use default turn rate limits
void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max_degs)
{
// calculate yaw error so it can be used for reporting and slowing the vehicle
_yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
// call heading controller
const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f),
radians(rate_max_degs),

1
APMrover2/mode.h
View File

@ -206,7 +206,6 @@ protected:
float _distance_to_destination; // distance from vehicle to final destination in meters
bool _reached_destination; // true once the vehicle has reached the destination
float _desired_yaw_cd; // desired yaw in centi-degrees
float _yaw_error_cd; // error between desired yaw and actual yaw in centi-degrees
float _desired_speed; // desired speed in m/s
};

1
APMrover2/mode_follow.cpp
View File

@ -13,7 +13,6 @@ bool ModeFollow::_enter()
// initialise heading to current heading
_desired_yaw_cd = ahrs.yaw_sensor;
_yaw_error_cd = 0.0f;
return true;
}

10
APMrover2/mode_loiter.cpp
View File

@ -13,7 +13,6 @@ bool ModeLoiter::_enter()
// initialise heading to current heading
_desired_yaw_cd = ahrs.yaw_sensor;
_yaw_error_cd = 0.0f;
return true;
}
@ -28,7 +27,6 @@ void ModeLoiter::update()
// sailboats do not stop
const float desired_speed_within_radius = g2.motors.has_sail() ? 0.1f : 0.0f;
_desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, 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
@ -36,17 +34,17 @@ void ModeLoiter::update()
// calculate bearing to destination
_desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
if (fabsf(yaw_error_cd) > 9000 && g2.loit_type == 0) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
float yaw_error_ratio = 1.0f - constrain_float(fabsf(yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
_desired_speed *= yaw_error_ratio;
}