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

Handle setpoint types properly for fixedwing position control

Browse Source 
Handle setpoint types properly for fixedwing position control
This commit is contained in:
Jaeyoung-Lim 2021-11-05 18:16:22 +01:00 committed by Daniel Agar
parent f43adaeeb5
commit 6a2ebfc0cc
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -787,7 +787,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
}
/* Copy thrust output for publication, handle special cases */
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && // launchdetector only available in auto
if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && // launchdetector only available in auto
_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
!_runway_takeoff.runwayTakeoffEnabled()) {
@ -796,12 +796,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
} else if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
} else if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust_body[0] = 0.0f;
@ -821,11 +821,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
bool use_tecs_pitch = true;
// auto runway takeoff
use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
use_tecs_pitch &= !(_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
// flaring during landing
use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
use_tecs_pitch &= !(_position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();