TECS pitch demand implementation details

[Tiago Marques]

Hi,

I'm an aerospace engineering PhD student and I'm thinking of modifying APM to test flight some of the navigation/control algorithms I'm researching.

When reading through the TECS code, there were some implementation details that I did not understand:

- _detect_underspeed: it there any reason to use a throttle condition on the airspeed detection algorithm, aside from avoiding a potential full throttle descent in case of airspeed sensor failure (which should probably be treated elsewhere)?

- _detect_underspeed: why do we leave the underspeed condition only when reaching desired altitude and not when reaching the desired airspeed? (to fly/not crash even if the pilot has set a bad trim airspeed/min airspeed?)
- _update_pitch: why do we need to saturate integrator input (_integ7_input) at all, when we saturate integrator_state (_integ7_state) later, before it's used for any calculation?

- _update_pitch: even if we want to be saturating integrator input, the units seem all wrong (shouldn't it be scaled with gainInv/DT?)
- _update_pitch: in the _integ7_state saturation I think there is a typo on the magic number (5 degrees - as per the comment above that line - equals roughly 0.873 rad, not 0.783 rad)

- _update_speed: does filtering airspeed with accelerometer data work well (since small errors in attitude may yield a significant difference in x-acceleration due to gravity being a comparatively large term)

Can someone shed some light on these?
Is this the right place to ask?

Thank you,

Tiago

[Paul Riseborough]

Tiago,

I was responsible for the original TECS implementation, although there have been a number of additions by other developers to extend it to perform a basic auto-land capability.

To assist me with responding to your questions and avoid misunderstandings, can you please provide links to the specific lines of code in Github along with your question: eg https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L691

Thanks,

-Paul

[Tom Pittenger]

Hi Tiago!

Thanks for your interest, we can use some improvement in this area along with some other areas you may be interested.

Like what Paul said, pointing to specific code lines would give some additional context on your questions. Meanwhile, mind if I point out a few issues that you may be interested in doing research on which are possibly related?

We already have stall prevention that takes into account restricting bank angle during turns considering wing loading but we're missing the other two parts: stall detection and stall recovery.

Stall detection
https://github.com/diydrones/ardupilot/issues/2933

Stall recovery
https://github.com/diydrones/ardupilot/issues/2669

Pitot tube failure detection and switch to/from internal synthetic airspeed calculation
https://github.com/diydrones/ardupilot/issues/1628

-TomP

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Tiago Marques]

Hi,

Thank you for your replies, and sorry for the delayed response (different timezones!).

As for the concerned lines:

- _detect_underspeed: it there any reason to use a throttle condition on the underspeed detection algorithm, aside from avoiding a potential full throttle descent in case of airspeed sensor failure (which should probably be treated elsewhere)? https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L458

- _detect_underspeed: why do we leave the underspeed condition only when reaching desired altitude and not when reaching the desired airspeed? (to fly/not crash even if the pilot has set a bad trim airspeed/min airspeed?) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L460
- _update_pitch: why do we need to saturate integrator input (_integ7_input) at all, when we saturate integrator_state (_integ7_state) later, before it's used for any calculation? _integ7_input saturation from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L683 to https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L690, _integ7_state saturation at https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L719, with no _integ7_state use in between

- _update_pitch: even if we want to be saturating integrator input, the units seem all wrong (shouldn't it be scaled with gainInv/DT?) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L685 and https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L689
- _update_pitch: in the _integ7_state saturation I think there is a typo on the magic number (5 degrees - as per the comment above that line - equals roughly 0.873 rad, not 0.783 rad) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L719, related comment starts at https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L702

- _update_speed: does filtering airspeed with accelerometer data work well (since small errors in attitude may yield a significant difference in x-acceleration due to gravity being a comparatively large term) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L339

Thanks for your help,

Tiago Marques

[Paul Riseborough]

Tiago,

Answers below.

-Paul

On Saturday, November 28, 2015 at 1:04:09 AM UTC+11, Tiago Marques wrote:

Hi,

Thank you for your replies, and sorry for the delayed response (different timezones!).

As for the concerned lines:

- _detect_underspeed: it there any reason to use a throttle condition on the underspeed detection algorithm, aside from avoiding a potential full throttle descent in case of airspeed sensor failure (which should probably be treated elsewhere)? https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L458

It prevents false positives due to gusts. 

- _detect_underspeed: why do we leave the underspeed condition only when reaching desired altitude and not when reaching the desired airspeed? (to fly/not crash even if the pilot has set a bad trim airspeed/min airspeed?) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L460

Because otherwise the condition can rapidly enter and exit with a large amplitude pitching motion and possible continual loss of height. The method adopted sets throttle to maximum and adjusts pitch angle to fly the aircraft at the minimum safe speed. This gives it the best chance of regaining the height lost during the initial pitch down.

- _update_pitch: why do we need to saturate integrator input (_integ7_input) at all, when we saturate integrator_state (_integ7_state) later, before it's used for any calculation? _integ7_input saturation from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L683 to https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L690, _integ7_state saturation at https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L719, with no _integ7_state use in between

Applying a limit to the input of an integrator if the control output from the previous time step has exceeded limits is a good way to prevent the integrator winding up and works across wider range of flight conditions than can be achieved by setting an arbitrary limit on the integrator state. however the method can leave the integrator state temporarily too high (or too low) when there is a rapid change in controller set-point, so there is an additional level of protection that adjusts the state to keep the output saturation less than 0.0783 rad.

- _update_pitch: even if we want to be saturating integrator input, the units seem all wrong (shouldn't it be scaled with gainInv/DT?) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L685 and https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L689

Units are correct and correct excessive integrator states with a 1 second time constant because the DT is accounted for in the integration of the state.

- _update_pitch: in the _integ7_state saturation I think there is a typo on the magic number (5 degrees - as per the comment above that line - equals roughly 0.873 rad, not 0.783 rad) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L719, related comment starts at https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L702

Talking about typos, you dropped a decimal place in your question (5 degrees is 0.0873 radians), but yes, the comment and magic are inconsistent. The 5 degrees should be changed to 4.5 degrees in the comment. There is no good reason to change the magic number now given the number of flight hours it has accrued. 

- _update_speed: does filtering airspeed with accelerometer data work well (since small errors in attitude may yield a significant difference in x-acceleration due to gravity being a comparatively large term) https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L339

It works better than not filtering and is an established technique for combining air data and inertial data to obtain an airspeed and airspeed derivative estimate. The TECS algorithm was developed before Plane had an inertial navigation solution available (EKF), and still needs to work without EKF.

[Tiago Marques]

Paul,

Thanks a lot for your answers!

Some of them still puzzle me, so I've decided to spend a little more time on it than I had planned as it seems like a good learning opportunity. I'll post back with any relevant results I get.

Just one early thought: I can stall my platform in automatic modes without underspeed ever been detected, because throttle demand does not get to 95% "fast enough" (e.g. flying around the minimum safety speed, or being above target altitude, or having some throttle damping/rate saturation). Yes, a perfectly tuned controller may not allow the underspeed condition to occur at all, but I think that the all point of underspeed (stall) detection is to avoid those special cases where it does not.

Thanks again,

Tiago Marques

[Tiago Marques]

Hi,

I've has some time to come back to this and I've focused on the integrator input saturation implementation.

What is puzzling me is:

1. From what I can tell we never actually enter the input saturation code (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L683) unless we change the maximum pitch or minimum pitch parameters in between TECS cycles, as we're checking if the constrained pitch demand is out of limits, which it never is unless we actually change the limits parameters, as the constrained pitch demand is calculated by constraining it to max/min pitch (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L741). Before commit https://github.com/diydrones/ardupilot/commit/060f5530979585140f89dde345a60d32b1786c33 pitch_dem_unc was used instead for the integrator input saturation check.

2. The units used to saturate the input (e.g. https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L685) are apparently radians. This would imply that the integrator state units are (rad)*s (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L691). However, we then add the integrator state (in https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L722) to a variable (temp) that has SEB_error units, m^2/s^2 (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L710), and saturate it later with pitch*gainInv (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L719), which again has the same units, given that gainInv has units m^2/s^2 (V*t*g, https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L709). Both the saturation and the unconstrained pitch demand calculation in https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L722  indicate that _integrator_state actually has units m^2/s^2, which is not consistent with its input having units (rad).

Am I missing something?

Thanks,

Tiago

[Paul Riseborough]

Tiago,

Responses below.

-Paul

On Friday, December 18, 2015 at 6:21:53 AM UTC+11, Tiago Marques wrote:

Hi,

I've has some time to come back to this and I've focused on the integrator input saturation implementation.

What is puzzling me is:

1. From what I can tell we never actually enter the input saturation code (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L683) unless we change the maximum pitch or minimum pitch parameters in between TECS cycles, as we're checking if the constrained pitch demand is out of limits, which it never is unless we actually change the limits parameters, as the constrained pitch demand is calculated by constraining it to max/min pitch (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L741).Before commit https://github.com/diydrones/ardupilot/commit/060f5530979585140f89dde345a60d32b1786c33 pitch_dem_unc was used instead for the integrator input saturation check.

 

It looks like you have spotted a bug introduced by that commit, so thank you. I'll work through it with the commit author.

2. The units used to saturate the input (e.g. https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L685) are apparently radians. This would imply that the integrator state units are (rad)*s (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L691).

However, we then add the integrator state (in https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L722) to a variable (temp) that has SEB_error units, m^2/s^2 (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L710), and saturate it later with pitch*gainInv (https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L719), which again has the same units, given that gainInv has units m^2/s^2 (V*t*g, https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L709). Both the saturation and the unconstrained pitch demand calculation in https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L722  indicate that _integrator_state actually has units m^2/s^2, which is not consistent with its input having units (rad).

There is an implicit gain of 1.0 in the applied to _PITCHmaxf - _pitch_dem in https://github.com/diydrones/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L685 with units of s^2/m^2 and a gain of gain of 1.0 with units of 1/s applied to the MIN function. However given we are now implementing this on 32 bit micros (the original design was for a resource constrained 8bit micro, those implicit gains should be made explicit for greater clarity.