Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Fix fw position controller takeoff

Browse Source 
This was introduced by a rebase
This commit is contained in:
Jaeyoung-Lim
2021-11-09 15:10:13 +01:00
committed by Daniel Agar
parent 00905973c7
commit dd83ef1813
1 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+9 -7
View File
@@ -772,6 +772,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp); const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
_position_sp_type = position_sp_type;
switch (position_sp_type) { switch (position_sp_type) {
case position_setpoint_s::SETPOINT_TYPE_IDLE: case position_setpoint_s::SETPOINT_TYPE_IDLE:
_att_sp.thrust_body[0] = 0.0f; _att_sp.thrust_body[0] = 0.0f;
@@ -797,12 +799,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
} }
/* reset landing state */ /* reset landing state */
if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state(); reset_landing_state();
} }
/* reset takeoff/launch state */ /* reset takeoff/launch state */
if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state(); reset_takeoff_state();
} }
@@ -812,7 +814,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
} }
/* Copy thrust output for publication, handle special cases */ /* Copy thrust output for publication, handle special cases */
if (_position_sp_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 && _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
!_runway_takeoff.runwayTakeoffEnabled()) { !_runway_takeoff.runwayTakeoffEnabled()) {
@@ -821,12 +823,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. */ the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
_att_sp.thrust_body[0] = _param_fw_thr_idle.get(); _att_sp.thrust_body[0] = _param_fw_thr_idle.get();
} else if (_position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && } else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) { _runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
} else if (_position_sp_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; _att_sp.thrust_body[0] = 0.0f;
@@ -846,11 +848,11 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
bool use_tecs_pitch = true; bool use_tecs_pitch = true;
// auto runway takeoff // auto runway takeoff
use_tecs_pitch &= !(_position_sp_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())); (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
// flaring during landing // flaring during landing
use_tecs_pitch &= !(_position_sp_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) { if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch(); _att_sp.pitch_body = get_tecs_pitch();