From f04aa2494b39f851b883b1f53b132d7f2086c92f Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 31 Jul 2024 15:14:16 +0200 Subject: [PATCH] FW pos control: do not requre global pos in manual position control --- .../fw_pos_control/FixedwingPositionControl.cpp | 15 ++++++--------- .../fw_pos_control/FixedwingPositionControl.hpp | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 9571c29865..daef50572d 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2158,28 +2158,25 @@ FixedwingPositionControl::control_manual_position(const float control_interval, if (_yaw_lock_engaged) { - /* just switched back from non heading-hold to heading hold */ + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + if (!_hdg_hold_enabled) { + // just switched back from non heading-hold to heading hold _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } // if there's a reset-by-fusion, the ekf needs some time to converge, // therefore we go into track holiding for 2 seconds if (_local_pos.timestamp - _time_last_xy_reset < 2_s) { - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); - _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); + navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 77eb88eb65..84300ae1b4 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -298,7 +298,7 @@ private: bool _hdg_hold_enabled{false}; // heading hold enabled bool _yaw_lock_engaged{false}; // yaw is locked for heading hold - position_setpoint_s _hdg_hold_position{}; // position where heading hold started + Vector2f _hdg_hold_position{}; // position where heading hold started // [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands float _manual_control_setpoint_for_height_rate{0.0f};