From 87c78cff5e1ab7652b618d77a163c1b6ab29f31e Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 16 Jul 2012 20:25:55 -0700 Subject: [PATCH] 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. --- ArduPlane/Attitude.pde | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 67da793bc5..174bd96aaf 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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;