fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw position controller: logic cleanup

Browse Source 
cleaned up logic when not to use pitch setpoint from TECS

Signed-off-by: tumbili <roman@px4.io>
This commit is contained in:
tumbili
2016-07-25 08:56:18 +02:00
committed by Andreas Antener
parent fd51bf44d5
commit 8c8120e2fc
1 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+18 -9
View File
@@ -2065,15 +2065,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && (
(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE ||
_runway_takeoff.runwayTakeoffEnabled())) ||
(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
_land_noreturn_vertical))
)) {
// decide when to use pitch setpoint from TECS because in some cases pitch
// setpoint is generated by other means
bool use_tecs_pitch = true;
// auto runway takeoff
use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
// flaring during landing
use_tecs_pitch &= !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
_land_noreturn_vertical);
// manual attitude control
use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER);
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}