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

Navigator/FW Position Control: VTOL front towards specified transition heading if available

Browse Source 
If the current yaw setpoint is valid we should use it to make the transition in this direction.
For a VTOL_TAKEOFF the yaw_setpoint is used to specify the transition direction (the
vehicle is aligned towards it and then transition is started). The current yaw can be
a bit off (e.g. because MIS_YAW_ERR is large), and it is better to track the actual setpoint.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-09-13 19:20:12 +02:00
parent 82b28bc72f
commit f9b6edab07
2 changed files with 14 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+7 -3
View File
@@ -871,9 +871,13 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
&lat_transition,
&lon_transition);
_transition_waypoint(0) = lat_transition;