Plane: removed old speed/altitude control algorithms

Use TECS only. This makes the code a lot simpler and easier to
properly document

ArduPlane/ArduPlane.pde

@@ -428,13 +428,6 @@ static int32_t target_airspeed_cm;
 // The difference between current and desired airspeed.  Used in the pitch controller.  Centimeters per second.
 static float airspeed_error_cm;
 
-// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
-// Used by the throttle controller
-static int32_t energy_error;
-
-// kinetic portion of energy error (m^2/s^2)
-static int32_t airspeed_energy_error;
-
 // An amount that the airspeed should be increased in auto modes based on the user positioning the
 // throttle stick in the top half of the range.  Centimeters per second.
 static int16_t airspeed_nudge_cm;
@@ -827,10 +820,7 @@ static void fast_loop()
  */
 static void update_speed_height(void)
 {
-    if ((g.alt_control_algorithm == ALT_CONTROL_TECS ||
+    if (auto_throttle_mode && !throttle_suppressed) {
-         g.alt_control_algorithm == ALT_CONTROL_DEFAULT) &&
-        auto_throttle_mode && !throttle_suppressed) 
-    {
 	    // Call TECS 50Hz update
         SpdHgt_Controller->update_50hz(relative_altitude());
     }
@@ -1068,7 +1058,7 @@ static void update_flight_mode(void)
 				nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
             }
 
-            if (alt_control_airspeed()) {
+            if (airspeed.use()) {
                 calc_nav_pitch();
                 if (nav_pitch_cd < takeoff_pitch_cd)
                     nav_pitch_cd = takeoff_pitch_cd;
@@ -1094,7 +1084,7 @@ static void update_flight_mode(void)
                 nav_pitch_cd = g.land_pitch_cd;
             } else {
                 calc_nav_pitch();
-                if (!alt_control_airspeed()) {
+                if (!airspeed.use()) {
                     // when not under airspeed control, don't allow
                     // down pitch in landing
                     nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd);
@@ -1305,13 +1295,8 @@ static void update_alt()
 
     geofence_check(true);
 
-    // Calculate new climb rate
-    //if(medium_loopCounter == 0 && slow_loopCounter == 0)
-    //	add_altitude_data(millis() / 100, g_gps->altitude / 10);
-
     // Update the speed & height controller states
-    if ((g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) &&
+    if (auto_throttle_mode && !throttle_suppressed) {
-        auto_throttle_mode && !throttle_suppressed) {
         SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100), 
                                                  target_airspeed_cm,
                                                  (control_mode==AUTO && takeoff_complete == false), 

ArduPlane/Attitude.pde

@@ -354,39 +354,7 @@ static void calc_throttle()
         return;
     }
 
-    if (g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) {
+    channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
-        channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
-    } else if (!alt_control_airspeed()) {
-        int16_t throttle_target = aparm.throttle_cruise + throttle_nudge;
-
-        // TODO: think up an elegant way to bump throttle when
-        // groundspeed_undershoot > 0 in the no airspeed sensor case; PID
-        // control?
-
-        // no airspeed sensor, we use nav pitch to determine the proper throttle output
-        // AUTO, RTL, etc
-        // ---------------------------------------------------------------------------
-        if (nav_pitch_cd >= 0) {
-            channel_throttle->servo_out = throttle_target + (aparm.throttle_max - throttle_target) * nav_pitch_cd / aparm.pitch_limit_max_cd;
-        } else {
-            channel_throttle->servo_out = throttle_target - (throttle_target - aparm.throttle_min) * nav_pitch_cd / aparm.pitch_limit_min_cd;
-        }
-
-        channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, aparm.throttle_min.get(), aparm.throttle_max.get());
-    } else {
-        // throttle control with airspeed compensation
-        // -------------------------------------------
-        energy_error = airspeed_energy_error + altitude_error_cm * 0.098f;
-
-        // positive energy errors make the throttle go higher
-        channel_throttle->servo_out = aparm.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
-        channel_throttle->servo_out += (channel_pitch->servo_out * g.kff_pitch_to_throttle);
-
-        channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
-                                                       aparm.throttle_min.get(), aparm.throttle_max.get());
-    }
-
-
 }
 
 /*****************************************
@@ -419,13 +387,7 @@ static void calc_nav_pitch()
 {
     // Calculate the Pitch of the plane
     // --------------------------------
-    if (g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) {
+    nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
-        nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
-    } else if (alt_control_airspeed()) {
-        nav_pitch_cd = -g.pidNavPitchAirspeed.get_pid(airspeed_error_cm);
-    } else {
-        nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
-    }
     nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
 }
 
@@ -864,8 +826,3 @@ static void demo_servos(uint8_t i)
     }
 }
 
-// return true if we should use airspeed for altitude/throttle control
-static bool alt_control_airspeed(void)
-{
-    return airspeed.use() && g.alt_control_algorithm == ALT_CONTROL_AIRSPEED;
-}

ArduPlane/Parameters.h

@@ -200,7 +200,7 @@ public:
         //
         k_param_kff_pitch_compensation = 200, // unused
         k_param_kff_rudder_mix,
-        k_param_kff_pitch_to_throttle,
+        k_param_kff_pitch_to_throttle, // unused
         k_param_kff_throttle_to_pitch,
         k_param_scaling_speed,
 
@@ -244,10 +244,10 @@ public:
         k_param_pidNavRoll = 240, // unused
         k_param_pidServoRoll, // unused
         k_param_pidServoPitch, // unused
-        k_param_pidNavPitchAirspeed,
+        k_param_pidNavPitchAirspeed, // unused
-        k_param_pidServoRudder,
+        k_param_pidServoRudder, // unused
-        k_param_pidTeThrottle,
+        k_param_pidTeThrottle, // unused
-        k_param_pidNavPitchAltitude,
+        k_param_pidNavPitchAltitude, // unused
         k_param_pidWheelSteer,
 
         // 254,255: reserved
@@ -415,9 +415,6 @@ public:
     AP_YawController   yawController;
 
     // PID controllers
-    PID         pidNavPitchAirspeed;
-    PID         pidTeThrottle;
-    PID         pidNavPitchAltitude;
     PID         pidWheelSteer;
 
     Parameters() :
@@ -449,9 +446,6 @@ public:
 
         // PID controller    initial P        initial I        initial D        initial imax
         //-----------------------------------------------------------------------------------
-        pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
-        pidTeThrottle       (THROTTLE_TE_P,   THROTTLE_TE_I,   THROTTLE_TE_D,   THROTTLE_TE_INT_MAX),
-        pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),
         pidWheelSteer         (0, 0, 0, 0)
 
         {}

ArduPlane/Parameters.pde

@@ -59,14 +59,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(kff_rudder_mix,         "KFF_RDDRMIX",    RUDDER_MIX),
 
-    // @Param: KFF_PTCH2THR
-    // @DisplayName: Pitch to Throttle Mix
-    // @Description: Pitch to throttle feed-forward gain.
-    // @Range: 0 5
-    // @Increment: 0.01
-    // @User: Advanced
-    GSCALAR(kff_pitch_to_throttle,  "KFF_PTCH2THR",   0),
-
     // @Param: KFF_THR2PTCH
     // @DisplayName: Throttle to Pitch Mix
     // @Description: Throttle to pitch feed-forward gain.
@@ -812,9 +804,6 @@ const AP_Param::Info var_info[] PROGMEM = {
     GGROUP(rc_12,                    "RC12_", RC_Channel_aux),
 #endif
 
-	GGROUP(pidNavPitchAirspeed,     "ARSP2PTCH_", PID),
-	GGROUP(pidTeThrottle,           "ENRGY2THR_", PID),
-	GGROUP(pidNavPitchAltitude,     "ALT2PTCH_",  PID),
 	GGROUP(pidWheelSteer,           "WHEELSTEER_",PID),
 
     // @Group: RLL2SRV_

ArduPlane/navigation.pde

@@ -111,7 +111,6 @@ static void calc_airspeed_errors()
         target_airspeed_cm = (aparm.airspeed_max * 100);
 
     airspeed_error_cm = target_airspeed_cm - aspeed_cm;
-    airspeed_energy_error = ((target_airspeed_cm * target_airspeed_cm) - (aspeed_cm*aspeed_cm))*0.00005;
 }
 
 static void calc_gndspeed_undershoot()

ArduPlane/radio.pde

@@ -101,7 +101,7 @@ static void read_radio()
 
     if (g.throttle_nudge && channel_throttle->servo_out > 50) {
         float nudge = (channel_throttle->servo_out - 50) * 0.02;
-        if (alt_control_airspeed()) {
+        if (airspeed.use()) {
             airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
         } else {
             throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
@@ -110,14 +110,6 @@ static void read_radio()
         airspeed_nudge_cm = 0;
         throttle_nudge = 0;
     }
-
-    /*
-     *  cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
-     *                       (int)g.rc_1.control_in,
-     *                       (int)g.rc_2.control_in,
-     *                       (int)g.rc_3.control_in,
-     *                       (int)g.rc_4.control_in);
-     */
 }
 
 static void control_failsafe(uint16_t pwm)