From f9b6edab073aa09ed04632e423fbf2639c72a087 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 13 Sep 2022 19:20:12 +0200 Subject: [PATCH] Navigator/FW Position Control: VTOL front towards specified transition heading if available 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 --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 10 +++++++--- src/modules/navigator/mission_block.cpp | 8 +++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 161fdca5e0..dc332cbeb3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 01c1c03a05..7dc6408b1d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -887,7 +887,13 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_ item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; item->params[0] = (float) new_mode; item->params[1] = 0.0f; - item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading + + // Keep yaw from previous mission item if valid, as that is containing the transition heading. + // If not valid use current yaw as yaw setpoint + if (!PX4_ISFINITE(item->yaw)) { + item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading + } + item->autocontinue = true; }