Plane: added new TECS altitude controller

Includes improvements to takeoff/launch detect logic to support hand
launching for X-8 flight

ArduPlane/ArduPlane.pde

@@ -59,6 +59,9 @@
 #include <AP_L1_Control.h>
 #include <AP_RCMapper.h>        // RC input mapping library
 
+#include <AP_SpdHgtControl.h>
+#include <AP_TECS.h>
+
 // Pre-AP_HAL compatibility
 #include "compat.h"
 
@@ -232,6 +235,7 @@ AP_InertialSensor_Oilpan ins( &apm1_adc );
 AP_AHRS_DCM ahrs(&ins, g_gps);
 
 static AP_L1_Control L1_controller(&ahrs);
+static AP_TECS TECS_controller(&ahrs, &barometer);
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
@@ -250,6 +254,9 @@ static GCS_MAVLINK gcs3;
 // selected navigation controller
 static AP_Navigation *nav_controller = &L1_controller;
 
+// selected navigation controller
+static AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Analog Inputs
 ////////////////////////////////////////////////////////////////////////////////
@@ -342,6 +349,14 @@ static uint32_t last_heartbeat_ms;
 // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
 static uint32_t ch3_failsafe_timer = 0;
 
+
+////////////////////////////////////////////////////////////////////////////////
+// AUTO takeoff
+////////////////////////////////////////////////////////////////////////////////
+// States used for delay from launch detect to engine start
+static bool launchCountStarted;
+static uint32_t last_tkoff_arm_time;
+
 ////////////////////////////////////////////////////////////////////////////////
 // LED output
 ////////////////////////////////////////////////////////////////////////////////
@@ -456,7 +471,7 @@ static bool takeoff_complete    = true;
 //Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
 static bool land_complete;
 // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
-static int32_t takeoff_altitude;
+static int32_t takeoff_altitude_cm;
 
 // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
 static int16_t takeoff_pitch_cd;
@@ -684,7 +699,7 @@ void setup() {
 
     batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
     batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
-    
+   
     init_ardupilot();
 
     // initialise the main loop scheduler
@@ -761,6 +776,11 @@ static void fast_loop()
     inertialNavigation();
 #endif
 
+    if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
+        // Call TECS 50Hz update
+        SpdHgt_Controller->update_50hz();
+    }
+
     // custom code/exceptions for flight modes
     // calculates roll, pitch and yaw demands from navigation demands
     // --------------------------------------------------------------
@@ -1215,6 +1235,15 @@ static void update_alt()
     // 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) {
+        SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt, target_airspeed_cm, 
+                                                 nav_command_ID == MAV_CMD_NAV_TAKEOFF, takeoff_pitch_cd);
+        if (g.log_bitmask & MASK_LOG_TECS) {
+            Log_Write_TECS_Tuning();
+        }
+    }
 }
 
 AP_HAL_MAIN();

ArduPlane/Attitude.pde

@@ -273,7 +273,9 @@ static void calc_throttle()
         return;
     }
 
-    if (!alt_control_airspeed()) {
+    if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
+        channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
+    } else if (!alt_control_airspeed()) {
         int16_t throttle_target = g.throttle_cruise + throttle_nudge;
 
         // TODO: think up an elegant way to bump throttle when
@@ -303,6 +305,7 @@ static void calc_throttle()
                                                        g.throttle_min.get(), g.throttle_max.get());
     }
 
+
 }
 
 /*****************************************
@@ -336,7 +339,9 @@ static void calc_nav_pitch()
 {
     // Calculate the Pitch of the plane
     // --------------------------------
-    if (alt_control_airspeed()) {
+    if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
+        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);
@@ -381,11 +386,13 @@ static void throttle_slew_limit(int16_t last_throttle)
     }
 }
 
+
 /*
   check for automatic takeoff conditions being met
  */
 static bool auto_takeoff_check(void)
 {
+#if 1
     if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) {
         // no auto takeoff without GPS lock
         return false;
@@ -411,6 +418,52 @@ static bool auto_takeoff_check(void)
 
     // we're good for takeoff
     return true;
+
+#else
+    // this is a more advanced check that relies on TECS
+    uint32_t now = hal.scheduler->micros();
+
+    if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) 
+    {
+        // no auto takeoff without GPS lock
+        return false;
+    }
+    if (SpdHgt_Controller->get_VXdot() >= g.takeoff_throttle_min_accel || g.takeoff_throttle_min_accel == 0.0 || launchCountStarted)
+    {
+        if (!launchCountStarted) 
+        {
+		launchCountStarted = true;
+        last_tkoff_arm_time = now;
+        gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"), SpdHgt_Controller->get_VXdot(), 0.1f*float(min(g.takeoff_throttle_delay,15)));
+        }
+ 		if ((now - last_tkoff_arm_time) <= 2500000)
+		{
+            
+			if ((g_gps->ground_speed > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0) && ((now -last_tkoff_arm_time) >= min(uint32_t(g.takeoff_throttle_delay*100000),1500000)))
+            {
+                gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed*100.0f);
+			    launchCountStarted = false;
+		        last_tkoff_arm_time = 0;
+			    return true;
+		    }
+		    else
+			{
+ 			    launchCountStarted = true;
+ 			    return false;
+            }
+        }
+        else
+        {
+            gcs_send_text_fmt(PSTR("Timeout AUTO"));
+		    launchCountStarted = false;
+		    last_tkoff_arm_time = 0;
+	    	return false;
+        }
+    }         
+    launchCountStarted = false;
+	last_tkoff_arm_time = 0;
+    return false;
+#endif
 }
 
 

ArduPlane/Log.pde

@@ -49,6 +49,7 @@ print_log_menu(void)
         PLOG(CMD);
         PLOG(CURRENT);
         PLOG(COMPASS);
+        PLOG(TECS);
  #undef PLOG
     }
 
@@ -317,6 +318,12 @@ static void Log_Write_Control_Tuning()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write a TECS tuning packet
+static void Log_Write_TECS_Tuning(void)
+{
+    SpdHgt_Controller->log_data(DataFlash, LOG_TECS_MSG);
+}
+
 struct PACKED log_Nav_Tuning {
     LOG_PACKET_HEADER;
     uint16_t yaw;
@@ -447,6 +454,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "CURR", "hhhHf",      "Thr,Volt,Curr,Vcc,CurrTot" },
     { LOG_COMPASS_MSG, sizeof(log_Compass),             
       "MAG", "hhhhhhhhh",   "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
+    TECS_LOG_FORMAT(LOG_TECS_MSG),
 };
 
 // Read the DataFlash.log memory : Packet Parser
@@ -478,6 +486,7 @@ static void Log_Write_Startup(uint8_t type) {}
 static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
 static void Log_Write_Current() {}
 static void Log_Write_Nav_Tuning() {}
+static void Log_Write_TECS_Tuning() {}
 static void Log_Write_Performance() {}
 static void Log_Write_Attitude() {}
 static void Log_Write_Control_Tuning() {}

ArduPlane/Parameters.h

@@ -90,6 +90,7 @@ public:
         k_param_mixing_gain,
         k_param_scheduler,
         k_param_relay,
+        k_param_takeoff_throttle_delay,
 
         // 110: Telemetry control
         //
@@ -231,6 +232,7 @@ public:
         k_param_yawController,
         k_param_L1_controller,
         k_param_rcmap,
+        k_param_TECS_controller,
 
         //
         // 240: PID Controllers
@@ -269,6 +271,9 @@ public:
     // navigation controller type. See AP_Navigation::ControllerType
     AP_Int8  nav_controller;
 
+    // attitude controller type.
+    AP_Int8  att_controller;
+
     // Estimation
     //
     AP_Float altitude_mix;
@@ -376,6 +381,7 @@ public:
     AP_Int8 stick_mixing;
     AP_Float takeoff_throttle_min_speed;
     AP_Float takeoff_throttle_min_accel;
+    AP_Int8 takeoff_throttle_delay;
     AP_Int8 level_roll_limit;
 
     // RC channels

ArduPlane/Parameters.pde

@@ -87,7 +87,7 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: TKOFF_THR_MINSPD
     // @DisplayName: Takeoff throttle min speed
-    // @Description: Minimum GPS ground speed in m/s before un-suppressing throttle in auto-takeoff. This is meant to be used for catapult launches where you want the motor to engage only after the plane leaves the catapult. Note that the GPS velocity will lag the real velocity by about 0.5seconds.
+    // @Description: Minimum GPS ground speed in m/s used by the speed check that un-suppresses throttle in auto-takeoff. This can be be used for catapult launches where you want the motor to engage only after the plane leaves the catapult, but it is preferable to use the TKOFF_THR_MINACC and TKOFF_THR_DELAY parameters for cvatapult launches due to the errors associated with GPS measurements. For hand launches with a pusher prop it is strongly advised that this parameter be set to a value no less than 4 m/s to provide additional protection against premature motor start. Note that the GPS velocity will lag the real velocity by about 0.5 seconds. The ground speed check is delayed by the TKOFF_THR_DELAY parameter.
     // @Units: m/s
     // @Range: 0 30
     // @Increment: 0.1
@@ -96,13 +96,22 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: TKOFF_THR_MINACC
     // @DisplayName: Takeoff throttle min acceleration
-    // @Description: Minimum forward acceleration in m/s/s before un-suppressing throttle in auto-takeoff. This is meant to be used for hand launches with a tractor style (front engine) plane. If this is set then the auto takeoff will only trigger if the pitch of the plane is between -30 and +45 degrees, and the roll is less than 30 degrees. This makes it less likely it will trigger due to carrying the plane with the nose down.
+    // @Description: Minimum forward acceleration in m/s/s before arming the ground speed check in auto-takeoff. This is meant to be used for hand launches. Setting this value to 0 disables the acceleration test which means the ground speed check will always be armed which could allow GPS velocity jumps to start the engine. For hand launches this should be set to 15.
     // @Units: m/s/s
     // @Range: 0 30
     // @Increment: 0.1
     // @User: User
     GSCALAR(takeoff_throttle_min_accel,     "TKOFF_THR_MINACC",  0),
 
+    // @Param: TKOFF_THR_DELAY
+    // @DisplayName: Takeoff throttle delay
+    // @Description: This parameter sets the time delay (in 1/10ths of a second) that the ground speed check is delayed after the forward acceleration check controlled by TKOFF_THR_MINACC has passed. For hand launches with pusher propellers it is essential that this is set to a value of no less than 2 (0.2 seconds) to ensure that the aircraft is safely clear of the throwers arm before the motor can start. 
+    // @Units: 0.1 seconds
+    // @Range: 0 15
+    // @Increment: 1
+    // @User: User
+    GSCALAR(takeoff_throttle_delay,     "TKOFF_THR_DELAY",  0),
+
     // @Param: LEVEL_ROLL_LIMIT
     // @DisplayName: Level flight roll limit
     // @Description: This controls the maximum bank angle in degrees during flight modes where level flight is desired, such as in the final stages of landing, and during auto takeoff. This should be a small angle (such as 5 degrees) to prevent a wing hitting the runway during takeoff or landing. Setting this to zero will completely disable heading hold on auto takeoff and final landing approach.
@@ -813,6 +822,10 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
     GOBJECT(L1_controller,         "NAVL1_",   AP_L1_Control),
 
+    // @Group: TECS_
+    // @Path: ../libraries/AP_TECS/AP_TECS.cpp
+    GOBJECT(TECS_controller,         "TECS_",   AP_TECS),
+
 #if MOUNT == ENABLED
     // @Group: MNT_
     // @Path: ../libraries/AP_Mount/AP_Mount.cpp

ArduPlane/commands_logic.pde

@@ -254,7 +254,7 @@ static void do_takeoff()
     set_next_WP(&next_nav_command);
     // pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
     takeoff_pitch_cd                = (int)next_nav_command.p1 * 100;
-    takeoff_altitude        = next_nav_command.alt;
+    takeoff_altitude_cm     = next_nav_command.alt;
     next_WP.lat             = home.lat + 1000;          // so we don't have bad calcs
     next_WP.lng             = home.lng + 1000;          // so we don't have bad calcs
     takeoff_complete        = false;                            // set flag to use gps ground course during TO.  IMU will be doing yaw drift correction
@@ -322,7 +322,8 @@ static bool verify_takeoff()
         nav_controller->update_level_flight();        
     }
 
-    if (adjusted_altitude_cm() > takeoff_altitude)  {
+    // see if we have reached takeoff altitude
+    if (adjusted_altitude_cm() > takeoff_altitude) {
         hold_course_cd = -1;
         takeoff_complete = true;
         next_WP = prev_WP = current_loc;

ArduPlane/config.h

@@ -646,7 +646,8 @@
     MASK_LOG_MODE | \
     MASK_LOG_CMD | \
     MASK_LOG_COMPASS | \
-    MASK_LOG_CURRENT
+    MASK_LOG_CURRENT | \
+    MASK_LOG_TECS
 
 
 

ArduPlane/defines.h

@@ -157,7 +157,8 @@ enum log_messages {
     LOG_ATTITUDE_MSG,
     LOG_MODE_MSG,
     LOG_COMPASS_MSG,
-    MAX_NUM_LOGS
+    LOG_TECS_MSG,
+    MAX_NUM_LOGS // always at the end
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
@@ -171,6 +172,7 @@ enum log_messages {
 #define MASK_LOG_CMD                    (1<<8)
 #define MASK_LOG_CURRENT                (1<<9)
 #define MASK_LOG_COMPASS                (1<<10)
+#define MASK_LOG_TECS                   (1<<11)
 
 // Waypoint Modes
 // ----------------
@@ -250,8 +252,9 @@ enum log_messages {
 
 // altitude control algorithms
 enum {
-    ALT_CONTROL_DEFAULT=0,
-    ALT_CONTROL_NON_AIRSPEED=1
+    ALT_CONTROL_DEFAULT      = 0,
+    ALT_CONTROL_NON_AIRSPEED = 1,
+    ALT_CONTROL_TECS         = 2
 };
 
 // attitude controller choice