MAVLINKHUD

Total Energy Control System (TECS)

1. Core Physics: Energy Rate vs. Distribution

The Total Energy Control System (TECS) replaces classical independent altitude/airspeed PID loops with a physics-based MIMO (Multi-Input Multi-Output) controller. It manages the aircraft's energy state rather than just its spatial position.

The Energy State

An aircraft has two primary energy reservoirs:

  1. Gravitational Potential Energy ($E_P$): Function of Altitude ($mgh$).
  2. Translational Kinetic Energy ($E_K$): Function of Airspeed ($ \frac{1}{2}mV^2 $).

Control Objectives

TECS decouples control into two orthogonal loops:

  • Total Energy Rate ($\dot{E}_{total}$): Controlled by THROTTLE.

    • Logic: If the plane is both too low (low $E_P$) and too slow (low $E_K$), it has a total energy deficit. Only adding thrust can solve this. Elevator input effectively just trades one error for another.
    • Equation: $\dot{E}_{total} \propto \text{Thrust} - \text{Drag}$

  • Energy Distribution Rate ($\dot{E}_{dist}$): Controlled by PITCH.

    • Logic: If the plane is high but slow, or low but fast, the Total Energy might be correct, but distributed wrongly. The elevator exchanges Potential for Kinetic energy (or vice versa).
    • Equation: $\dot{E}_{dist} \propto \dot{h} - \frac{V\dot{V}}{g}$

2. ArduPilot Architecture (AP_TECS)

The AP_TECS library sits between the Navigation Controller (AP_L1_Control/Mission Planner) and the Attitude Controller (AP_AttitudeControl - Roll/Pitch PIDs).

  • Inputs: Demanded Speed (spdem), Demanded Height (hdem), State Estimates (EKF).
  • Processing: Calculates Specific Energy Error and Energy Distribution Error. Applies TECS_TIME_CONST filters and SPDWEIGHT weighting.
  • Outputs: Target Pitch (pitch_dem) $\rightarrow$ Inner Loop; Target Throttle (throttle_dem) $\rightarrow$ ESC.

CRITICAL ARCHITECTURAL CONSTRAINT: TECS assumes the inner pitch loop is perfect. If ATT.Pitch lags ATT.DesPitch, TECS receives delayed feedback on its energy distribution corrections, leading to divergent oscillation. Tune Inner Loops First.

3. Pre-Tuning Checklist

DO NOT attempt TECS tuning until these physical baselines are verified.

  1. Center of Gravity: Must be mechanically correct. Tail-heavy aircraft require rapid stabilizer corrections that TECS misinterprets as energy distribution volatility.
  2. Airspeed Sensor:
    • ARSPD_RATIO must be calibrated (flight circle/figure-8).
    • Kinetic energy is proportional to $V^2$; sensor errors are amplified exponentially in the energy calculation.
  3. Inner Loops (Attitude):
    • Run AUTOTUNE.
    • Log Analysis: Verify ATT.Pitch tracks ATT.DesPitch with minimal latency (<200ms) and no oscillation.

4. Tuning Protocol

Tuning is a process of System Identification, not random gain adjustment. We fly to measure physical limits, then map those limits to parameters.

Phase 1: Flight Envelope Identification (Manual/FBWA)

Fly in FBWA mode to manually probe limits.

Maneuver Action Observation Parameter Mapping
Cruise Trim Fly level at cruise speed. Record Throttle % and Pitch Angle. TRIM_THROTTLE
Max Climb Full Throttle (THR_MAX), pull back to max safe climb angle. Record stabilized Climb Rate (m/s). TECS_CLMB_MAX (Set to ~80-90% of measured)
Min Sink Idle Throttle (THR_MIN), glide level. Record Sink Rate (m/s). TECS_SINK_MIN
Max Sink Idle Throttle, pitch down to max safe descent. Record Sink Rate (m/s). TECS_SINK_MAX

Phase 2: Bench Configuration

Apply observed values to parameters.

  • TECS_CLMB_MAX: Set to 80-90% of observed max climb. Warning: Setting this to 100% causes integrator windup as the target is mathematically impossible.
  • AIRSPEED_MIN: Stall speed + 20% safety margin.
  • AIRSPEED_MAX: Slightly below structural $V_{ne}$.

Phase 3: Verification (Autonomous)

Switch to LOITER or CRUISE.

  1. Loiter Stability: Verify Altitude $\pm 1m$, Speed $\pm 1m/s$.
  2. Climb Step: Command +50m alt change. Throttle should smoothly surge to THR_MAX.
  3. Descent Step: Command -50m alt change. Throttle should cut to THR_MIN; Pitch should drop to maintain airspeed.

5. Advanced Parameter Tuning

Once the envelope is defined, tune the "personality" of the controller. (See Parameter Definitions in source).

5.1 TECS_SPDWEIGHT (Priority)

Determines how the controller resolves conflict (e.g., Underspeed AND Low Altitude).

  • Range: 0.0 - 2.0 (Default: 1.0)
  • 1.0 (Balanced): Splits error correction between Speed and Height.
  • 2.0 (Speed Priority): Mandatory for Gliders. Sacrifices altitude to maintain airspeed (prevent stall).
  • 0.0 (Height Priority): Sacrifices speed to maintain altitude (Precision Landing).

5.2 TECS_TIME_CONST (Aggressiveness)

Low-pass filter time constant on energy errors.

  • Default: 5.0
  • Tuning:
    • Porpoising (Sinusoidal oscillation): Increase to 7.0 or 8.0 to dampen response.
    • Sluggishness: Decrease to 3.0 or 4.0 for tighter tracking (requires fast inner loops).

5.3 THR_SLEWRATE (Throttle Smoothness)

Max % throttle change per second.

  • Electric: 100% (Instant).
  • Gas/Nitro: 20-30% to prevent engine choke on rapid transients.
  • Surging: Reduce to dampen "throttle hunting" in gusty conditions.

5.4 TECS_PTCH_FF_K (Feed-Forward)

Predicts pitch change needed for a speed change.

  • Value: -0.04 to -0.08.
  • Use Case: High-efficiency gliders. improves responsiveness to speed demands without waiting for error integration.

6. Forensics & Log Analysis

Diagnosis of common energy pathologies using Dataflash logs.

6.1 Porpoising (Height Oscillation)

  • Symptom: Aircraft undulates sinusoidally.
  • Log: TECS.h oscillates around TECS.hdem.
  • Root Cause 1: Inner Loop Lag. Check ATT.Pitch vs ATT.DesPitch. $\rightarrow$ Tune Pitch PID.
  • Root Cause 2: TECS too fast. $\rightarrow$ Increase TECS_TIME_CONST (+1.0 steps).
  • Root Cause 3: Damping. $\rightarrow$ Increase TECS_PTCH_DAMP (+0.1 steps).

6.2 Throttle Hunting (Surging)

  • Symptom: Motors rev up/down in level flight.
  • Log: TECS.th shows sawtooth/square wave.
  • Fix 1: Decrease THR_SLEWRATE.
  • Fix 2: Increase TECS_THR_DAMP.
  • Fix 3: Verify TRIM_THROTTLE matches actual cruise throttle.

6.3 Altitude Loss in Turns

  • Symptom: Drops altitude when banking.
  • Physics: Banking diverts lift vector; vertical component decreases.
  • Fix: Increase TECS_RLL2THR. Adds feed-forward throttle based on bank angle to overcome induced drag.

7. Special Configurations

QuadPlanes (VTOL)

  • Transition: TECS does not fully own altitude control until transition is complete.
  • Q_ASSIST_SPEED: Set this below AIRSPEED_MIN to prevent VTOL motors firing during normal fixed-wing energy maneuvers (which confuses TECS).

Gliders

  • THR_MAX = 0 (or climb only).
  • TECS_SPDWEIGHT = 2.0 (Must prioritize speed/stall prevention).
  • SOAR_ENABLE: Use ArduPilot Soaring features to treat thermals as lift sources rather than altitude errors.