From 5808dac4bc1d2135d581a6a7b6aaef80a180081f Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Wed, 24 Jul 2024 14:09:12 +0200 Subject: [PATCH] reset position-mode line following after position reset --- src/modules/ekf2/EKF/ekf.cpp | 1 + .../FixedwingPositionControl.cpp | 25 +++++++++++++------ .../FixedwingPositionControl.hpp | 10 +++----- 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index ac75a367d6..9f6a0ab40b 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -330,6 +330,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1) ) { ECL_INFO("fused external observation as position measurement"); + _state_reset_status.reset_count.posNE++; _time_last_hor_pos_fuse = _time_delayed_us; _last_known_pos.xy() = _state.pos.xy(); return true; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a1286db402..c80a9edb41 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2134,6 +2134,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max = 0.0f; } + if (_local_pos.xy_reset_counter != _xy_reset_counter) { + _time_last_xy_reset = _local_pos.timestamp; + } + /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2164,6 +2168,13 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_position.lon = _current_longitude; } + // 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 < 2e6) { + _hdg_hold_position.lat = _current_latitude; + _hdg_hold_position.lon = _current_longitude; + } + 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); @@ -2310,28 +2321,24 @@ FixedwingPositionControl::Run() // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { - if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) { + if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _vz_reset_counter) { // make TECS accept step in altitude and demanded altitude _tecs.handle_alt_step(_current_altitude, -_local_pos.vz); } // adjust navigation waypoints in position control mode if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled - && _local_pos.vxy_reset_counter != _pos_reset_counter) { + && _local_pos.vxy_reset_counter != _vxy_reset_counter) { // reset heading hold flag, which will re-initialise position control _hdg_hold_enabled = false; } } - // update the reset counters in any case - _alt_reset_counter = _local_pos.vz_reset_counter; - _pos_reset_counter = _local_pos.vxy_reset_counter; - // Convert Local setpoints to global setpoints if (!_global_local_proj_ref.isInitialized() || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp) - || (_local_pos.vxy_reset_counter != _pos_reset_counter)) { + || (_local_pos.vxy_reset_counter != _vxy_reset_counter)) { double reference_latitude = 0.; double reference_longitude = 0.; @@ -2616,6 +2623,10 @@ FixedwingPositionControl::Run() _spoilers_setpoint_pub.publish(spoilers_setpoint); } + _vz_reset_counter = _local_pos.vz_reset_counter; + _vxy_reset_counter = _local_pos.vxy_reset_counter; + _xy_reset_counter = _local_pos.xy_reset_counter; + perf_end(_loop_perf); } } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 3f2398f340..9a34f43524 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -406,12 +406,10 @@ private: matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; // ESTIMATOR RESET COUNTERS - - // captures the number of times the estimator has reset the horizontal position - uint8_t _pos_reset_counter{0}; - - // captures the number of times the estimator has reset the altitude state - uint8_t _alt_reset_counter{0}; + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint64_t _time_last_xy_reset{0}; // LATERAL-DIRECTIONAL GUIDANCE