Plane: add acceleration based height control option in TECS #31698

tridge · 2025-12-10T20:40:18Z

This adds an acceleration based height controller as an alternative to the current TECS height control system.
The maths and most of the code was developed by @priseborough - I am just bringing it as a PR and coordinating testing.
To try this system you need to set TECS_OPTIONS=4 to enable the new controller.
Issues:

stick mixing on pitch axis doesn't work while this new controller is active

Georacer

I have yet to run the simulation to look at the performance, I will do that soon.
The control logic seems sound.

But I really don't like how this new control scheme is merged as part of TECS. This is no longer a Total Energy controller. It's two separate controllers for altitude and airspeed errors. Embedding them in TECS makes an already complicated file even less maintainable.

Additionally, all of the traditional TECS functions are still run (e.g. pitch demand calculation), even though their result is never actually used. This seems wasteful, resource-wise.

I would much prefer if this new controller was a new subclass of longitudinal control, much like AP_L1_Control is a subclass of AP_Navigation. I know this is a much larger refactoring, but I think it's necessary, especially if we are to develop other longitudinal controllers in the future.
I see how switching between the two controllers will be much harder in this case, but I still think it's the right choice.

I'm happy to help along with this.

Georacer · 2025-12-11T10:19:06Z

libraries/AP_TECS/AP_TECS.cpp

+        Vector3f vel_NED, vel_wind_NED;
+        if (_ahrs.get_velocity_NED(vel_NED) && _ahrs.wind_estimate(vel_wind_NED)) {
+            // Calculate a wind relative flight path unit vector
+            Vector3f flight_path_NED = (vel_NED - vel_wind_NED);
+            flight_path_NED.normalize();
+            // Get rate of change of velocity vector along wind relative flight path
+            Vector3f vel_dot_NED = _ahrs.get_accel_ef();
+            vel_dot_NED.z += GRAVITY_MSS;
+            // * operator is overloaded as a dot product
+            const float temp = vel_dot_NED * flight_path_NED;
+            // take 5 point moving average
+            _vel_dot = _vdot_filter.apply(temp);
+        } else {
+            // If no wind relative velocity data is available, use less accurate method
+            // that uses acceleration along X axis and is subject to pitch coupling
+            const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
+            // Calculate speed rate of change
+            const float temp = rotMat.c.x * GRAVITY_MSS + AP::ins().get_accel().x;
+            // take 5 point moving average
+            _vel_dot = _vdot_filter.apply(temp);
+        }


Didn't we bring this in in another PR recently?

I don't see it in master, I've split it out as a separate commit
@priseborough did you intend this change to be part of this PR?

libraries/AP_TECS/AP_TECS.cpp

Georacer · 2025-12-11T11:09:46Z

libraries/AP_TECS/AP_TECS.cpp

-        _SEBdot_dem_clip = clipStatus::MIN;
+        // The vertical accln control method is incompatible with the height demand
+        // profile mechanism controlled by _SEBdot_dem_clip
+        if (!option_is_set(Option::HGT_ACCLN_CTRL) &&
+            !option_is_set(Option::INHIBIT_CLIPSTATUS)) {
+            _SEBdot_dem_clip = clipStatus::MIN;
+        }


If I read this correctly, you are preventing clipping in the climb/descent rate here: The only consumer of _SEBdot_de_clip is _update_height_demand().
This is the only modification in _update_pitch(), right?

Then my question is, why run _update_pitch() at all? Its result is not used during acceleration control mode.

@priseborough ?

You are correct. We don't need to run it in this mode. Will fix.

Georacer · 2025-12-11T11:12:17Z

libraries/APM_Control/AP_PitchController.cpp

+    // @Param: _NOR_GLIM
+    // @DisplayName: Normal accln limit
+    // @Description: Maximum allowed normal g when airspeed = SCALING_SPEED. When flying faster or slower than SCALING_SPEED the limit is adjusted for dynamic pressure. Use this to prevent wing stall. Use this to stay within angle of attack limits.
+    // @Units: g
+    // @Range: 1.5 5.0
+    // @User: Standard
+    AP_GROUPINFO("_NOR_GLIM", 12, AP_PitchController, acro_normal_g_lim, 2.0f),


Should we try to reuse/merge the TECS acceleration limit? Or is this a controller performance vs "pilot feel" situation?

not sure, @priseborough ?

The TECS limit is a vertical acceleration limit, this is a normal g limit and needs to be higher because it includes g buildup in constant height turns, whereas the TECS limit is more about limiting TECS authority and smoothing out response to steps in demands, not a vehicle limit.

Georacer · 2025-12-11T11:16:16Z

libraries/APM_Control/AP_PitchController.cpp

+*/
+float AP_PitchController::get_servo_out_accln(float up_accln, float scaler, float pitch_angle_min, float pitch_angle_max, float pitch_trim_deg)
+{
+    Vector3f vdot_dem_clipped = Vector3f(0.0f, 0.0f, -constrain_float(up_accln, -7.0f, 7.0f));


Why the arbitrary 7 here and not reuse NOR_GLIM?

@priseborough ?

This is a vertical accln limit like the TECS one. It probably should be harmonised with the TECS limit. It is there to prevent small positive and below g pushovers which cause problems with horizontal tracking

libraries/APM_Control/AP_PitchController.cpp

timtuxworth · 2025-12-12T04:28:30Z

libraries/AP_TECS/AP_TECS.cpp

    // @DisplayName: Extra TECS options
    // @Description: This allows the enabling of special features in the speed/height controller.
-    // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup
+    // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup,2:DoAcclnControl,3:InhibitClipStatus


Pretty sure we need spaces here. This will appear in the GCS UI and this is what is displayed to the user so:

// @bitmask: 0:Glider Only,1:Allow Descent Speedup,2:Enable Height Acceleration Control,3:Inhibit Clip Status

I guess this fixes the two existing options which should have had spaces I'm thinking.

this gives more accurate velocity information and less pitch coupling

this gives an option for more precise height control when flying close to constraints (complex terrain, power-lines, landing etc)

for testing the new functionality

test mission with and without TECS_OPTIONS=4

ArduPlane/Attitude.cpp

peterbarker · 2025-12-17T07:10:04Z

ArduPlane/Attitude.cpp

+    } else {
+        return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator,
                                         ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)));
+    }


Suggested change

}

peterbarker · 2025-12-17T07:17:48Z

libraries/AP_TECS/AP_TECS.cpp

    // Update and average speed rate of change

    // calculate a low pass filtered _vel_dot
    if (_flags.reset) {


update_vel_dot_lpf()?

The if/else logic is getting worse over time

peterbarker · 2025-12-17T07:23:11Z

libraries/AP_TECS/AP_TECS_accel.cpp

+    // @Field: CR: current climb rate
+    // @Field: VAL: vertical acceleration limit
+    AP::logger().WriteStreaming("TEC6","TimeUS,HAD,UEL,DEL,HD,H,HRD,CR,VAL",
+                                "Qffffffff",


Units and multipliers

peterbarker · 2025-12-17T07:24:02Z

libraries/AP_TECS/AP_TECS_accel.cpp

+    // get throttle demand setpoint, then adjust using a PID control scheme if airspeed available
+    _update_throttle_without_airspeed(throttle_nudge, pitch_trim_deg);
+
+    if (_ahrs.using_airspeed_sensor() || _use_synthetic_airspeed) {


Didn't we have case somewhere where we needed to check the "once" flag or something here?

Yes, this is now covered by the new use_airspeed() private method.

peterbarker · 2025-12-17T07:24:21Z

libraries/AP_TECS/AP_TECS_accel.cpp

+        // @Field: TRD: true airspeed rate demand (low-pass filtered)
+        // @Field: Tdem: final throttle demand
+        AP::logger().WriteStreaming("TEC5","TimeUS,FF,P,D,I,TD,TM,TRD,Tdem",
+                                    "Qffffffff",


units and multipliers

peterbarker · 2025-12-17T07:25:08Z

libraries/APM_Control/AP_PitchController.cpp

+    Vector3f velRateDemBF; // demanded velocity rate of change - body frame
+    Vector3f acclnDemBF; // demanded specific force - body frame


intiialise as part of declaration

peterbarker · 2025-12-17T07:25:51Z

libraries/APM_Control/AP_PitchController.cpp

+        // @Field: clip: rate limit clip indicator
+#if HAL_LOGGING_ENABLED
+        AP::logger().WriteStreaming("ACC1","TimeUS,VRDBF,ADBF,AEBF,gain,PRDFF,AEI,PRmin,PRmax,DR,clip",
+                                    "Qfffffffffb",


units and multipliers

peterbarker · 2025-12-17T07:26:29Z

Tools/autotest/arduplane.py


+    def TECSAccelControl(self):
+        '''Test TECS height acceleration control via scripting'''
+        self.context_push()


Don't need this

tridge requested a review from priseborough December 10, 2025 20:40

tridge added Plane WIP NeedsTesting labels Dec 10, 2025

Hwurzburg added the WikiNeeded needs wiki update label Dec 10, 2025

tridge force-pushed the pr-plane-acc-height-control branch 3 times, most recently from 93b7ab3 to 47f7a69 Compare December 10, 2025 23:33

Georacer suggested changes Dec 11, 2025

View reviewed changes

tridge force-pushed the pr-plane-acc-height-control branch from 47f7a69 to d5c862b Compare December 12, 2025 02:31

timtuxworth reviewed Dec 12, 2025

View reviewed changes

tridge force-pushed the pr-plane-acc-height-control branch 2 times, most recently from 7ca38eb to d9c6145 Compare December 12, 2025 06:14

tridge added 8 commits December 14, 2025 09:41

AP_TECS: use wind relative flight path if available

e6d2907

this gives more accurate velocity information and less pitch coupling

AP_TECS: added support for acceleration based height control

b1d6fa3

this gives an option for more precise height control when flying close to constraints (complex terrain, power-lines, landing etc)

APM_Control: Add method to control height acceleration

5bd0cfd

Plane: Add alternate method of height control

a23a64a

AP_Scripting: added script to control TECS accel control

50824ea

for testing the new functionality

autotest: added more units

f8ebe03

autotest: added an autotest for new TECS accel controller

8b18688

test mission with and without TECS_OPTIONS=4

autotest: expand plane logger message whitelist

4fa8201

tridge force-pushed the pr-plane-acc-height-control branch from 55afc82 to 4fa8201 Compare December 13, 2025 22:47

tridge added the DevCallEU label Dec 15, 2025

peterbarker reviewed Dec 17, 2025

View reviewed changes

peterbarker removed the DevCallEU label Dec 17, 2025

AP_TECS: Don't run _update_pitch() if results are not used

0eff393

		Vector3f velRateDemBF; // demanded velocity rate of change - body frame
		Vector3f acclnDemBF; // demanded specific force - body frame

Plane: add acceleration based height control option in TECS #31698

Are you sure you want to change the base?

Plane: add acceleration based height control option in TECS #31698

Uh oh!

Conversation

tridge commented Dec 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Georacer left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants

tridge commented Dec 10, 2025 •

edited

Loading

Georacer left a comment •

edited

Loading