Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane : AP_L1_Control : Fix wrong way turn behaviour on loiter entry

Browse Source
This commit is contained in:
priseborough 2013-04-14 21:46:03 +10:00 committed by Andrew Tridgell
parent 994d8e354a
commit a423d102e0
1 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -223,22 +223,30 @@ void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius
//Calculate PD control correction to circle waypoint //Calculate PD control correction to circle waypoint
float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv); float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
//Calculate centripetal acceleration to circle waypoint //Calculate tangential velocity
float velTangent = _maxf((xtrackVelCap * float(loiter_direction)) , 0.0f); float velTangent = xtrackVelCap * float(loiter_direction);
//Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
if ( velTangent < 0.0f ) {
latAccDemCircPD = _maxf(latAccDemCircPD , 0.0f);
}
// Calculate centripetal acceleration demand
float latAccDemCircCtr = velTangent * velTangent / _maxf((0.5f * radius) , (radius + xtrackErrCirc)); float latAccDemCircCtr = velTangent * velTangent / _maxf((0.5f * radius) , (radius + xtrackErrCirc));
//Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr); float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
// Perform switchover between 'capture' and 'circle' modes at the point where the commands cross over to achieve a seamless transfer // Perform switchover between 'capture' and 'circle' modes at the point where the commands cross over to achieve a seamless transfer
if ((latAccDemCap < latAccDemCirc && loiter_direction > 0) | (latAccDemCap > latAccDemCirc && loiter_direction < 0)) { // Only fly 'capture' mode if outside the circle
if ((latAccDemCap < latAccDemCirc && loiter_direction > 0 && xtrackErrCirc > 0.0f) | (latAccDemCap > latAccDemCirc && loiter_direction < 0 && xtrackErrCirc > 0.0f)) {
_latAccDem = latAccDemCap; _latAccDem = latAccDemCap;
_WPcircle = true; _WPcircle = false;
_bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
} else { } else {
_latAccDem = latAccDemCirc; _latAccDem = latAccDemCirc;
_WPcircle = false; _WPcircle = true;
_bearing_error = 0.0f; // bearing error (radians), +ve to left of track _bearing_error = 0.0f; // bearing error (radians), +ve to left of track
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
} }