From de26ffa6e0ad5de3c0bb805635e2263f48c5f079 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Fri, 6 May 2022 11:41:18 +0200 Subject: [PATCH] fw pos ctrl: turn back to takeoff point with npfg --- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 5bbbec318d..100ebe6fa3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1508,7 +1508,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - _npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + // NOTE: current waypoint is passed twice to trigger the "point following" logic -- TODO: create + // point following navigation interface instead of this hack. + _npfg.navigateWaypoints(curr_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); target_airspeed = _npfg.getAirspeedRef() / _eas2tas;