Browse Source

APM: Removed incorrect nav_gain_scaler from nav_roll calculation.

nav_gain_scaler was originally added due to a perceived improvement in
loiter performance, but is incorrect for navigation. Turn rate is a
function of airspeed and bank angle, ground speed has no effect on
that.
mission-4.1.18
Jonathan Challinger 13 years ago committed by Andrew Tridgell
parent
commit
d9d055ef78
  1. 8
      ArduPlane/Attitude.pde

8
ArduPlane/Attitude.pde

@ -202,17 +202,11 @@ static void calc_nav_pitch() @@ -202,17 +202,11 @@ static void calc_nav_pitch()
static void calc_nav_roll()
{
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
// This does not make provisions for wind speed in excess of airframe speed
nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// negative error = left turn
// positive error = right turn
// Calculate the required roll of the plane
// ----------------------------------------
nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = g.pidNavRoll.get_pid(bearing_error); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega;

Loading…
Cancel
Save