AP_TECS: Implement improved control loops

libraries/AP_TECS/AP_TECS.h

@@ -21,7 +21,6 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Vehicle/AP_FixedWing.h>
-#include <Filter/DerivativeFilter.h>
 #include <Filter/AverageFilter.h>
 
 class AP_Landing;
@@ -45,6 +44,7 @@ public:
     void update_50hz(void);
 
     // Update the control loop calculations
+    // Do not call slower than 10Hz or faster than 500Hz
     void update_pitch_throttle(int32_t hgt_dem_cm,
                                int32_t EAS_dem_cm,
                                enum AP_FixedWing::FlightStage flight_stage,
@@ -88,7 +88,8 @@ public:
     
     // added to let SoaringContoller reset pitch integrator to zero
     void reset_pitch_I(void) {
-        _integSEB_state = 0.0f;
+        _integSEBdot = 0.0f;
+        _integKE = 0.0f;
     }
 
     // reset throttle integrator
@@ -195,13 +196,17 @@ private:
     AP_Int8  _land_pitch_max;
     AP_Float _maxSinkRate_approach;
     AP_Int32 _options;
+    AP_Int8  _land_pitch_trim;
+    AP_Float _flare_holdoff_hgt;
+    AP_Float _hgt_dem_tconst;
 
     enum {
-        OPTION_GLIDER_ONLY=(1<<0),
+        OPTION_GLIDER_ONLY=(1<<0)
     };
 
     AP_Float _pitch_ff_v0;
     AP_Float _pitch_ff_k;
+    AP_Float _accel_gf;
 
     // temporary _pitch_max_limit. Cleared on each loop. Clear when >= 90
     int8_t _pitch_max_limit = 90;
@@ -218,6 +223,10 @@ private:
     // estimated climb rate (m/s)
     float _climb_rate;
 
+    // climb and sink rate limits
+    float _climb_rate_limit;
+    float _sink_rate_limit;
+
     /*
       a filter to estimate climb rate if we don't have it from the EKF
      */
@@ -238,8 +247,11 @@ private:
     // Integrator state 6 - throttle integrator
     float _integTHR_state;
 
-    // Integrator state 6 - pitch integrator
-    float _integSEB_state;
+    // energy balance error integral
+    float _integSEBdot;
+
+    // pitch demand kinetic energy error integral
+    float _integKE;
 
     // throttle demand rate limiter state
     float _last_throttle_dem;
@@ -249,6 +261,7 @@ private:
 
     // Rate of change of speed along X axis
     float _vel_dot;
+    float _vel_dot_lpf;
 
     // Equivalent airspeed
     float _EAS;
@@ -264,13 +277,24 @@ private:
     float _EAS_dem;
 
     // height demands
-    float _hgt_dem;
-    float _hgt_dem_in_old;
-    float _hgt_dem_adj;
-    float _hgt_dem_adj_last;
-    float _hgt_rate_dem;
-    float _hgt_dem_prev;
-    float _land_hgt_dem;
+    float _hgt_dem_in_raw;      // height demand input from autopilot before any modification (m)
+    float _hgt_dem_in;          // height demand input from autopilot after unachievable climb or descent limiting (m)
+    float _hgt_dem_in_prev;     // previous value of _hgt_dem_in (m)
+    float _hgt_dem_lpf;         // height demand after application of low pass filtering (m)
+    float _flare_hgt_dem_adj;   // height rate demand duirng flare adjusted for height tracking offset at flare entry (m)
+    float _flare_hgt_dem_ideal; // height we want to fly at during flare (m)
+    float _hgt_dem;             // height demand sent to control loops (m)
+    float _hgt_dem_prev;        // _hgt_dem from previous frame (m)
+
+    // height rate demands
+    float _hgt_dem_rate_ltd;    // height demand after application of the rate limiter (m)
+    float _hgt_rate_dem;        // height rate demand sent to control loops
+
+    // offset applied to height demand post takeoff to compensate for height demand filter lag
+    float _post_TO_hgt_offset;
+
+    // last lag compensation offset applied to height demand
+    float _lag_comp_hgt_offset;
 
     // Speed demand after application of rate limiting
     // This is the demand tracked by the TECS control loops
@@ -279,6 +303,7 @@ private:
     // Speed rate demand after application of rate limiting
     // This is the demand tracked by the TECS control loops
     float _TAS_rate_dem;
+    float _TAS_rate_dem_lpf;
 
     // Total energy rate filter state
     float _STEdotErrLast;
@@ -307,6 +332,9 @@ private:
 
         // true if a propulsion failure is detected.
         bool propulsion_failed:1;
+
+        // true when a reset of airspeed and height states to current is performed on this frame
+        bool reset:1;
     };
     union {
         struct flags _flags;
@@ -335,12 +363,12 @@ private:
     float _PITCHminf;
 
     // 1 if throttle is clipping at max value, -1 if clipping at min value, 0 otherwise
-    enum class ThrClipStatus  : int8_t {
+    enum class clipStatus  : int8_t {
         MIN  = -1,
         NONE =  0,
         MAX  =  1,
     };
-    ThrClipStatus _thr_clip_status;
+    clipStatus _thr_clip_status;
 
     // Specific energy quantities
     float _SPE_dem;
@@ -352,9 +380,24 @@ private:
     float _SPEdot;
     float _SKEdot;
 
+    // variables used for precision landing pitch control
+    float _hgt_at_start_of_flare;
+    float _hgt_rate_at_flare_entry;
+    float _hgt_afe;
+    float _pitch_min_at_flare_entry;
+
+    // used to scale max climb and sink limits to match vehicle ability
+    float _max_climb_scaler;
+    float _max_sink_scaler;
+
     // Specific energy error quantities
     float _STE_error;
 
+    // 1 when specific energy balance rate demand is clipping in the up direction
+    // -1 when specific energy balance rate demand is clipping in the down direction
+    // 0 when not clipping
+    clipStatus _SEBdot_dem_clip;
+
     // Time since last update of main TECS loop (seconds)
     float _DT;
 
@@ -374,21 +417,26 @@ private:
     // need to reset on next loop
     bool _need_reset;
 
-    // internal variables to be logged
-    struct {
-        float SKE_weighting;
-        float SPE_error;
-        float SKE_error;
-        float SEB_delta;
-    } logging;
+    float _SKE_weighting;
 
     AP_Int8 _use_synthetic_airspeed;
     
     // use synthetic airspeed for next loop
     bool _use_synthetic_airspeed_once;
-    
+
+    // using airspeed in throttle calculation this frame
+    bool _using_airspeed_for_throttle;
+
+    // low pass filters used for crossover filter that combines demanded and measured pitch
+    // when calculating a pitch to throttle mapping.
+    LowPassFilterFloat _pitch_demand_lpf;
+    LowPassFilterFloat _pitch_measured_lpf;
+
+    // aerodynamic load factor
+    float _load_factor;
+
     // Update the airspeed internal state using a second order complementary filter
-    void _update_speed(float load_factor);
+    void _update_speed(float DT);
 
     // Update the demanded airspeed
     void _update_speed_demand(void);