Fix TECS init when switching into a altitude controlled mode only once in air #22499
Conversation
|
This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there: |
|
@KonradRudin in f57ae5f and following I propose to completely remove the manual TECS resetting from the fw_position_controller, and instead do the initialization only at one place anymore: internally in the |
In general i think it would be better if the initialization is done outside (and inside only if we know that the timestamp would be unsafe due to CPU spikes or whatever), because it is easier for the executor to know, if it switches to TECS control loop or not. Internally we can only check this based on the timestamp. However, currently the fixedwingpositioncontrol has wierd logic on when to initialize it and it is convoluted as well. So your fix makes it cleaner |
|
I almost crashed because of the underlying issue yesterday when I switched to RTL for a test: My analysis pointed exactly toward the things fixed in this pull request. It's reproducible in SITL I got lucky with having enough altitude to react, switching back to Manual mode quickly enough and the plane surviving the >9g pull up 😅 If desired I can test these changes in SITL later today. |
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
The control params (eg min/max pitch) are used before they are correctly set by TECS::update(). While this is an issue we should fix, it also doesn't hurt to set them to more reasobale values (eg 30° limit). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
-remove external init, and instead always (but only) init when dt is too large -init the controller params correctly Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
sfuhrer
force-pushed
the
pr-fix-tecs-init-main
branch
from
87f73c3
to
0a7db64
Compare
|
Squashed commits and rebased on main.
Yes I've tested it again, and it initialized properly now in all my tests (with and without airspeed, on ground and in air).
For now I would assume them to be sane. Otherwise we'd need to sanitize them on many other places. EKF2 hopefully invalidates the position estimate before it outputs garbage data. |
Sorry to hear, but happy that you had some altitude margin! |
|
@sfuhrer JFYI I did some testing yesterday and RTL worked nicely exactly like expected in the exact same use case. |
|
This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there: |
Solved Problem
Fixes #22487 resp. https://discuss.px4.io/t/cube-o-px4-v1-14-0-erroneous-pitch-roll-setpoint-in-autonomous-mode/35557/11
The issue is only noticeable when the vehicle is switched into an altitude controlled mode when already in air, and there is no airspeed sensor connected. Then the airspeed state is 0, which leads to a division by 0 while calculating the pitch setpoint in TECS. And TECS initializes wrongly and constrains the INF only to a large value of 5rad.
It is NOT observable if at any time before takeoff the flight mode was already in an altitude controlled mode for a bit, then TECS is correctly initialized by the time it is actually needed.
Solution
ac33e4e is the real fix. Prior to this commit, the
_control_params, eg._control_param.pitch_max, are already used inTECS::initialize()but not initialized with the actual settings on the vehicle. As the default for the pitch min/max is 5rad the effect is then clearly noticeable.e177e9a is patching the airspeed-less handling, which is further robustified with 4a8f346 and 0964831. Without these changes, the .tas state is at 0 which then leads to a division by 0, and due to the wrong pitch limits it doesn't get constrained properly.
The other commits are not directly related to the crash, but proposals for mini clean ups and further guards.
Changelog Entry
For release notes:
Alternatives
Test coverage
SITL testing. The issue is reproducible in SITL. As the SITL though inits in Position mode, there is a small hack needed to not have it run TECS::update() before switching into Stabilized mode: do not run it within 10s of boot -->
if (_local_pos_sub.update(&_local_pos) && _local_pos.timestamp > 10_s). Further one has to disable the airspeed sensor, I did that by commenting out the publishing of the simulated differential_pressure topic in SimulatorMavlink.With current main:
With this PR:
