fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 00:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: revert altitude controller to old logic

Browse Source 
- if the height rate input into TECS is finite, use that one to
update a velocity reference generator
- if the velocity reference generator reports position locked or
height rate input is not finite, then run altidue reference generator
- run altitude controller only if altitue is controlled

if height_rate_setpoint is set, only use that one to update altitdue trajectory

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-29 13:21:11 +02:00
parent dde656f4b1
commit 460956fd33
4 changed files with 83 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+3 -1
View File
@@ -483,7 +483,9 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
}
tecs_status.altitude_sp = alt_sp;
tecs_status.altitude_sp_ref = debug_output.altitude_sp_ref;
tecs_status.altitude_reference = debug_output.altitude_reference;
tecs_status.height_rate_reference = debug_output.height_rate_reference;
tecs_status.height_rate_direct = debug_output.height_rate_direct;
tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
tecs_status.height_rate = -_local_pos.vz;
tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;