From 7e697e4abd31a5066a740a70fa364356aef748b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Jul 2012 09:49:18 +1000 Subject: [PATCH] APM: switch back to old nav_roll calculation the old nav_roll will be used for the 2.50 release to prevent the need for re-tuning. For the release after that we will use the new calculation --- ArduPlane/ArduPlane.pde | 2 -- ArduPlane/Attitude.pde | 10 ++++++++++ ArduPlane/Log.pde | 2 +- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index b9441f8b19..c87ae58b5b 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -394,8 +394,6 @@ static int32_t target_bearing; //This is the direction from the last waypoint to the next waypoint // deg * 100 : 0 to 360 static int32_t crosstrack_bearing; -// A gain scaler to account for ground speed/headwind/tailwind -static float nav_gain_scaler = 1; // Direction held during phases of takeoff and landing // deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use static int32_t hold_course = -1; // deg * 100 dir of plane diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3de9baf173..c03b628bf6 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -200,6 +200,8 @@ static void calc_nav_pitch() static void calc_nav_roll() { +#define NAV_ROLL_BY_RATE 0 +#if NAV_ROLL_BY_RATE // Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a // desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try // to turn at 15 degrees per second. @@ -221,6 +223,14 @@ static void calc_nav_roll() // 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); +#else + // this is the old nav_roll calculation. We will use this for 2.50 + // then remove for a future release + float nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0); + nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4); + nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100 +#endif + nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); } diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 99d6ec8813..4fb429353e 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -315,7 +315,7 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteInt((uint16_t)nav_bearing); DataFlash.WriteInt(altitude_error); DataFlash.WriteInt((int16_t)airspeed.get_airspeed_cm()); - DataFlash.WriteInt((int16_t)(nav_gain_scaler*1000)); + DataFlash.WriteInt(0); // was nav_gain_scaler DataFlash.WriteByte(END_BYTE); }