From 73ee098a2571865757ecda0be6090d2a6477e17c Mon Sep 17 00:00:00 2001
From: mahima-yoga <mahima@auterion.com>
Date: Tue, 28 Oct 2025 09:27:37 +0100
Subject: [PATCH] fw_autotune: continue to next axis in case of convergence
 timeout

---
 .../fw_autotune_attitude_control.cpp          | 43 ++++++++++++++++---
 1 file changed, 37 insertions(+), 6 deletions(-)

diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
index bfcd857092..48633f3e73 100644
--- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
+++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
@@ -548,16 +548,47 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	}
 
 	// In case of convergence timeout
-	// the identification sequence is aborted immediately
+	// the identification sequence is aborted and the FSM moves on to the next axis
 	if (_state != state::wait_for_disarm && _state != state::idle && _state != state::fail && _state != state::complete) {
-		if (now - _state_start_time > 30_s
-		    || (_param_fw_at_man_aux.get() && !_aux_switch_en)
-		    || _start_flight_mode != _nav_state) {
-			orb_advert_t mavlink_log_pub = nullptr;
+
+		const bool timeout = (now - _state_start_time) > 30_s;
+		const bool mode_changed = (_start_flight_mode != _nav_state);
+		const bool aux_switched_off = (_param_fw_at_man_aux.get() && !_aux_switch_en);
+
+		orb_advert_t mavlink_log_pub = nullptr;
+
+		if (mode_changed || aux_switched_off) {
+			// Abort
 			mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing");
 			_state = state::fail;
-			_state_start_time = now;
+
+		} else if (timeout) {
+			// Skip to next axis
+			mavlink_log_critical(&mavlink_log_pub, "Autotune axis timeout, skipping to next axis");
+
+			switch (_state) {
+			case state::roll_amp_detection:
+			case state::roll:
+				_state = state::roll_pause;   // proceed to pitch
+				break;
+
+			case state::pitch_amp_detection:
+			case state::pitch:
+				_state = state::pitch_pause;  // proceed to yaw
+				break;
+
+			case state::yaw_amp_detection:
+			case state::yaw:
+				_state = state::yaw_pause;    // proceed to verification
+				break;
+
+			default:
+				_state = state::fail;         // safety fallback
+				break;
+			}
 		}
+
+		_state_start_time = now;
 	}
 }