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

APM: Use g.airspeed_cruise for turn rate calculation if the airspeed sensor is disabled.

Browse Source 
This gives our best approximation for auto flight
This commit is contained in:
Andrew Tridgell 2012-07-19 10:48:29 +10:00
parent 3c4f971618
commit 2673179075
1 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduPlane/Attitude.pde
View File

@ -200,11 +200,27 @@ static void calc_nav_pitch()
static void calc_nav_roll() static void calc_nav_roll()
{ {
// negative error = left turn // Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
// positive error = right turn // desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
// Calculate the required roll of the plane // to turn at 15 degrees per second.
// ---------------------------------------- float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error) * .01);
nav_roll = g.pidNavRoll.get_pid(bearing_error); //returns desired bank angle in degrees*100
// Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
float speed;
if(airspeed.use()) {
speed = airspeed.get_airspeed();
} else {
speed = g.airspeed_cruise*0.01;
// Floor the speed so that the user can't enter a bad value
if(speed < 6) {
speed = 6;
}
}
// Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
nav_roll = ToDeg(atan(speed*turn_rate/9.81)*100);
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
} }