diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ac76323a756..e3b0e7a6a7c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2534,10 +2534,10 @@ int commander_thread_main(int argc, char *argv[]) // transition to previous state if sticks are touched if ((_last_sp_man.timestamp != sp_man.timestamp) && - ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || - (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || - (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || - (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { + ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { // revert to position control in any case main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); @@ -2550,11 +2550,12 @@ int commander_thread_main(int argc, char *argv[]) internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER) { // transition to previous state if sticks are touched + if ((_last_sp_man.timestamp != sp_man.timestamp) && - ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || - (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || - (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || - (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { + ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { // revert to position control in any case main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);