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

encapsulate attitude setpoints from position control

Browse Source
This commit is contained in:
Jaeyoung-Lim 2021-11-13 12:57:03 +01:00
parent 29b9e46664
commit 0d6416cb8a
1 changed files with 14 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -842,21 +842,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_att_sp.thrust_body[0] = get_tecs_thrust();
}
}
// 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 &= !(position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
// flaring during landing
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();
}
}
void
@ -1086,6 +1071,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
_att_sp.pitch_body = get_tecs_pitch();
}
void
@ -1197,6 +1184,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
_att_sp.pitch_body = get_tecs_pitch();
}
void
@ -1356,6 +1344,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
radians(_param_fw_p_lim_min.get()));
}
_att_sp.pitch_body = get_tecs_pitch();
} else {
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
@ -1368,6 +1358,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
}
}
// flaring during landing
use_tecs_pitch &= !(position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
if (use_tecs_pitch) {
}
}
void
@ -1643,6 +1640,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
false,
radians(_param_fw_p_lim_min.get()));
}
_att_sp.pitch_body = get_tecs_pitch();
}
void