From 90789be68f9d6c3c74597652b7a2681f4e3d2c28 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 b362d8682c..493a19d9a5 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1512,7 +1512,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;