diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4f26b492e0..03a4f78785 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -2293,7 +2293,6 @@ FixedwingPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) { _hold_alt += _global_pos.delta_alt; - _alt_reset_counter = _global_pos.alt_reset_counter; // make TECS accept step in altitude and demanded altitude _tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt); } @@ -2304,12 +2303,13 @@ FixedwingPositionControl::task_main() // reset heading hold flag, which will re-initialise position control _hdg_hold_enabled = false; - - // update reset counter - _pos_reset_counter = _global_pos.lat_lon_reset_counter; } } + // update the reset counters in any case + _alt_reset_counter = _global_pos.alt_reset_counter; + _pos_reset_counter = _global_pos.lat_lon_reset_counter; + // XXX add timestamp check _global_pos_valid = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ee9fd721af..1ea176ad70 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -724,19 +724,16 @@ MulticopterPositionControl::poll_subscriptions() if (_control_mode.flag_control_manual_enabled) { if (_z_reset_counter != _local_pos.z_reset_counter) { _pos_sp(2) += _local_pos.delta_z; - _z_reset_counter = _local_pos.z_reset_counter; } if (_xy_reset_counter != _local_pos.xy_reset_counter) { _pos_sp(0) += _local_pos.delta_xy[0]; _pos_sp(1) += _local_pos.delta_xy[1]; - _xy_reset_counter = _local_pos.xy_reset_counter; } if (_vz_reset_counter != _local_pos.vz_reset_counter) { _vel_sp(2) += _local_pos.delta_vz; _vel_sp_prev(2) += _local_pos.delta_vz; - _vz_reset_counter = _local_pos.vz_reset_counter; } if (_vxy_reset_counter != _local_pos.vxy_reset_counter) { @@ -744,9 +741,14 @@ MulticopterPositionControl::poll_subscriptions() _vel_sp(1) += _local_pos.delta_vxy[1]; _vel_sp_prev(0) += _local_pos.delta_vxy[0]; _vel_sp_prev(1) += _local_pos.delta_vxy[1]; - _vxy_reset_counter = _local_pos.vxy_reset_counter; } } + + // update the reset counters in any case + _z_reset_counter = _local_pos.z_reset_counter; + _xy_reset_counter = _local_pos.xy_reset_counter; + _vz_reset_counter = _local_pos.vz_reset_counter; + _vxy_reset_counter = _local_pos.vxy_reset_counter; } }