diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 197e3561cb..16afb349ff 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -615,30 +615,52 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_time_const_throt(_param_fw_t_thro_const.get()); - /* current waypoint (the one currently heading for) */ - Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + Vector2f curr_wp{0.0f, 0.0f}; + Vector2f prev_wp{0.0f, 0.0f}; - /* Initialize attitude controller integrator reset flags to 0 */ - _att_sp.roll_reset_integral = false; - _att_sp.pitch_reset_integral = false; - _att_sp.yaw_reset_integral = false; + if (_vehicle_status.in_transition_to_fw) { - /* previous waypoint */ - Vector2f prev_wp{0.0f, 0.0f}; + 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, + &lon_transition); + + _transition_waypoint(0) = (float)lat_transition; + _transition_waypoint(1) = (float)lon_transition; + } - if (pos_sp_prev.valid) { - prev_wp(0) = (float)pos_sp_prev.lat; - prev_wp(1) = (float)pos_sp_prev.lon; + + curr_wp = prev_wp = _transition_waypoint; } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp(0) = (float)pos_sp_curr.lat; - prev_wp(1) = (float)pos_sp_curr.lon; + /* current waypoint (the one currently heading for) */ + curr_wp = Vector2f((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + + if (pos_sp_prev.valid) { + prev_wp(0) = (float)pos_sp_prev.lat; + prev_wp(1) = (float)pos_sp_prev.lon; + + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = (float)pos_sp_curr.lat; + prev_wp(1) = (float)pos_sp_curr.lon; + } + + + /* reset transition waypoint, will be set upon entering front transition */ + _transition_waypoint.setNaN(); } + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; + float mission_airspeed = _param_fw_airspd_trim.get(); if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 9a796aafa0..e21fb3e3ea 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -253,6 +253,8 @@ private: bool _vtol_tailsitter{false}; + Vector2f _transition_waypoint{NAN, NAN}; + // estimator reset counters uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state