Plane: use load factor to limit nav_roll_cd

this calculates the aerodymanic load factor from smoothed airspeed and
uses it to limit roll to keep it below the level where the load factor
would take us past the aerodymanic limit of the airframe

ArduPlane/ArduPlane.pde

@@ -598,6 +598,13 @@ static int32_t nav_roll_cd;
 // The instantaneous desired pitch angle.  Hundredths of a degree
 static int32_t nav_pitch_cd;
 
+// the aerodymamic load factor. This is calculated from the demanded
+// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
+static float aerodynamic_load_factor = 1.0f;
+
+// a smoothed airspeed estimate, used for limiting roll angle
+static float smoothed_airspeed;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Mission library
 ////////////////////////////////////////////////////////////////////////////////
@@ -1324,6 +1331,7 @@ static void update_flight_mode(void)
         // set nav_roll and nav_pitch using sticks
         nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
         nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+        update_load_factor();
         float pitch_input = channel_pitch->norm_input();
         if (pitch_input > 0) {
             nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
@@ -1357,6 +1365,8 @@ static void update_flight_mode(void)
     case FLY_BY_WIRE_B:
         // Thanks to Yury MonZon for the altitude limit code!
         nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
+        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+        update_load_factor();
         update_fbwb_speed_height();
         break;
         
@@ -1374,6 +1384,8 @@ static void update_flight_mode(void)
         
         if (!cruise_state.locked_heading) {
             nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
+            nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+            update_load_factor();
         } else {
             calc_nav_roll();
         }
@@ -1392,6 +1404,7 @@ static void update_flight_mode(void)
         // holding altitude at the altitude we set when we
         // switched into the mode
         nav_roll_cd  = roll_limit_cd / 3;
+        update_load_factor();
         calc_nav_pitch();
         calc_throttle();
         break;
@@ -1527,7 +1540,8 @@ static void update_flight_stage(void)
                                                  flight_stage,
                                                  auto_state.takeoff_pitch_cd,
                                                  throttle_nudge,
-                                                 relative_altitude());
+                                                 relative_altitude(),
+                                                 aerodynamic_load_factor);
         if (should_log(MASK_LOG_TECS)) {
             Log_Write_TECS_Tuning();
         }

ArduPlane/Attitude.pde

@@ -481,6 +481,9 @@ static void calc_nav_yaw_ground(void)
 }
 
 
+/*
+  calculate a new nav_pitch_cd from the speed height controller
+ */
 static void calc_nav_pitch()
 {
     // Calculate the Pitch of the plane
@@ -490,9 +493,13 @@ static void calc_nav_pitch()
 }
 
 
+/*
+  calculate a new nav_roll_cd from the navigation controller
+ */
 static void calc_nav_roll()
 {
     nav_roll_cd = nav_controller->nav_roll_cd();
+    update_load_factor();
     nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
 }
 
@@ -1002,3 +1009,38 @@ static void adjust_nav_pitch_throttle(void)
         nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
     }
 }
+
+
+/*
+  calculate a new aerodynamic_load_factor and limit nav_roll_cd to
+  ensure that the load factor does not take us below the sustainable
+  airspeed
+ */
+static void update_load_factor(void)
+{
+    float demanded_roll = fabsf(nav_roll_cd*0.01f);
+    if (demanded_roll > 85) {
+        // limit to 85 degrees to prevent numerical errors
+        demanded_roll = 85;
+    }
+    aerodynamic_load_factor = 1.0f / safe_sqrt(cos(radians(demanded_roll)));
+
+    float max_load_factor = smoothed_airspeed / aparm.airspeed_min;
+    if (max_load_factor <= 1) {
+        // our airspeed is below the minimum airspeed. Limit roll to
+        // 25 degrees
+        nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
+    } else if (max_load_factor < aerodynamic_load_factor) {
+        // the demanded nav_roll would take us past the aerodymamic
+        // load limit. Limit our roll to a bank angle that will keep
+        // the load within what the airframe can handle. We always
+        // allow at least 25 degrees of roll however, to ensure the
+        // aircraft can be maneuvered with a bad airspeed estimate. At
+        // 25 degrees the load factor is 1.1 (10%)
+        int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;
+        if (roll_limit < 2500) {
+            roll_limit = 2500;
+        }
+        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
+    }    
+}

ArduPlane/sensors.pde

@@ -45,6 +45,12 @@ static void read_airspeed(void)
             barometer.set_external_temperature(temperature);
         }
     }
+
+    // update smoothed airspeed estimate
+    float aspeed;
+    if (ahrs.airspeed_estimate(&aspeed)) {
+        smoothed_airspeed = smoothed_airspeed * 0.8f + aspeed * 0.2f;
+    }
 }
 
 static void zero_airspeed(void)