fix: detect silent filter-wheel move failures

firmware/controller/src/commands/stage_commands.cpp

@@ -1,15 +1,30 @@
 #include "stage_commands.h"
 
+// Mark a move callback as failed: clear in_progress so the host stops
+// blocking, but flag CMD_EXECUTION_ERROR so the host doesn't treat the
+// non-move as a successful completion. Without this, send_position_update
+// would broadcast COMPLETED_WITHOUT_ERRORS and the host's tracked position
+// would silently desync from the hardware.
+static inline void mark_move_failed()
+{
+    mcu_cmd_execution_in_progress = false;
+    mcu_cmd_execution_status = CMD_EXECUTION_ERROR;
+}
+
 void callback_move_x()
 {
     long relative_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
     long current_position = tmc4361A_currentPosition(&tmc4361[x]);
     X_direction = sgn(relative_position);
     X_commanded_target_position = ( relative_position > 0 ? min(current_position + relative_position, X_POS_LIMIT) : max(current_position + relative_position, X_NEG_LIMIT) );
+    mcu_cmd_execution_in_progress = true;
     if ( tmc4361A_moveTo(&tmc4361[x], X_commanded_target_position) == 0)
     {
         X_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
@@ -19,10 +34,14 @@ void callback_move_y()
     long current_position = tmc4361A_currentPosition(&tmc4361[y]);
     Y_direction = sgn(relative_position);
     Y_commanded_target_position = ( relative_position > 0 ? min(current_position + relative_position, Y_POS_LIMIT) : max(current_position + relative_position, Y_NEG_LIMIT) );
+    mcu_cmd_execution_in_progress = true;
     if ( tmc4361A_moveTo(&tmc4361[y], Y_commanded_target_position) == 0)
     {
         Y_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
@@ -33,26 +52,40 @@ void callback_move_z()
     Z_direction = sgn(relative_position);
     Z_commanded_target_position = ( relative_position > 0 ? min(current_position + relative_position, Z_POS_LIMIT) : max(current_position + relative_position, Z_NEG_LIMIT) );
     focusPosition = Z_commanded_target_position;
+    mcu_cmd_execution_in_progress = true;
     if ( tmc4361A_moveTo(&tmc4361[z], Z_commanded_target_position) == 0)
     {
         Z_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
 // Helper function for filter wheel movement (shared by W and W2)
 static void move_filterwheel(uint8_t axis, bool enabled, int* direction, long* target_position, bool* movement_in_progress)
 {
-    if (!enabled) return;
+    if (!enabled)
+    {
+        // Filter wheel not initialized: surface as an execution error so the
+        // host re-homes instead of trusting that the move happened.
+        mark_move_failed();
+        return;
+    }
 
     long relative_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
     long current_position = tmc4361A_currentPosition(&tmc4361[axis]);
     *direction = sgn(relative_position);
     *target_position = current_position + relative_position;
+    mcu_cmd_execution_in_progress = true;
     if (tmc4361A_moveTo(&tmc4361[axis], *target_position) == 0)
     {
         *movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
@@ -71,10 +104,14 @@ void callback_move_to_x()
     long absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
     X_direction = sgn(absolute_position - tmc4361A_currentPosition(&tmc4361[x]));
     X_commanded_target_position = absolute_position;
+    mcu_cmd_execution_in_progress = true;
     if (tmc4361A_moveTo(&tmc4361[x], X_commanded_target_position) == 0)
     {
         X_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
@@ -83,10 +120,14 @@ void callback_move_to_y()
     long absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
     Y_direction = sgn(absolute_position - tmc4361A_currentPosition(&tmc4361[y]));
     Y_commanded_target_position = absolute_position;
+    mcu_cmd_execution_in_progress = true;
     if (tmc4361A_moveTo(&tmc4361[y], Y_commanded_target_position) == 0)
     {
         Y_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
@@ -95,25 +136,36 @@ void callback_move_to_z()
     long absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
     Z_direction = sgn(absolute_position - tmc4361A_currentPosition(&tmc4361[z]));
     Z_commanded_target_position = absolute_position;
+    mcu_cmd_execution_in_progress = true;
     if (tmc4361A_moveTo(&tmc4361[z], Z_commanded_target_position) == 0)
     {
         focusPosition = absolute_position;
         Z_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 
 void callback_move_to_w()
 {
-    if (enable_filterwheel == true) {
-        long absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
-        W_direction = sgn(absolute_position - tmc4361A_currentPosition(&tmc4361[w]));
-        W_commanded_target_position = absolute_position;
-        if (tmc4361A_moveTo(&tmc4361[w], W_commanded_target_position) == 0)
-        {
+    if (enable_filterwheel != true)
+    {
+        mark_move_failed();
+        return;
+    }
+    long absolute_position = int32_t(uint32_t(buffer_rx[2]) << 24 | uint32_t(buffer_rx[3]) << 16 | uint32_t(buffer_rx[4]) << 8 | uint32_t(buffer_rx[5]));
+    W_direction = sgn(absolute_position - tmc4361A_currentPosition(&tmc4361[w]));
+    W_commanded_target_position = absolute_position;
+    mcu_cmd_execution_in_progress = true;
+    if (tmc4361A_moveTo(&tmc4361[w], W_commanded_target_position) == 0)
+    {
         W_commanded_movement_in_progress = true;
-        mcu_cmd_execution_in_progress = true;
-        }
+    }
+    else
+    {
+        mark_move_failed();
     }
 }
 

firmware/controller/src/constants.h

@@ -9,8 +9,10 @@
 // Version is sent in response byte 22 as nibble-encoded: high nibble = major, low nibble = minor
 // Version 1.0 = first version with multi-port illumination support
 // Version 1.1 = serial watchdog for illumination auto-shutoff
+// Version 1.2 = move callbacks report CMD_EXECUTION_ERROR on tmc4361A_moveTo
+//               failure; W axis microstep position broadcast in bytes 19-20
 #define FIRMWARE_VERSION_MAJOR 1
-#define FIRMWARE_VERSION_MINOR 1
+#define FIRMWARE_VERSION_MINOR 2
 
 #include "def/def_v1.h"
 

firmware/controller/src/globals.cpp

@@ -33,6 +33,11 @@ uint16_t home_safety_margin[TOTAL_AXES] = {4, 4, 4, 4, 4};
 volatile int buffer_rx_ptr = 0;
 byte cmd_id = 0;
 bool mcu_cmd_execution_in_progress = false;
+// Status reported in the next position-update packet whenever
+// mcu_cmd_execution_in_progress is false. Reset to COMPLETED_WITHOUT_ERRORS
+// at the start of every received command and overwritten with
+// CMD_EXECUTION_ERROR by a callback that detects a failure.
+byte mcu_cmd_execution_status = COMPLETED_WITHOUT_ERRORS;
 bool checksum_error = false;
 
 // limit switch

firmware/controller/src/globals.h

@@ -37,6 +37,7 @@ extern uint16_t home_safety_margin[TOTAL_AXES];
 extern volatile int buffer_rx_ptr;
 extern byte cmd_id;
 extern bool mcu_cmd_execution_in_progress;
+extern byte mcu_cmd_execution_status;
 extern bool checksum_error;
 
 // limit switch

firmware/controller/src/serial_communication.cpp

@@ -26,6 +26,12 @@ void process_serial_message()
       // Reset watchdog timer on every valid serial message
       last_serial_message_time = millis();
 
+      // Reset execution status for this new command; callbacks set it to
+      // CMD_EXECUTION_ERROR if they detect a failure (e.g. tmc4361A_moveTo
+      // returning non-zero) so the host can distinguish a real success
+      // from a silently dropped move.
+      mcu_cmd_execution_status = COMPLETED_WITHOUT_ERRORS;
+
       CommandCallback p_callback = cmd_map[buffer_rx[1]];
       if (!p_callback) {
         callback_default();
@@ -46,8 +52,10 @@ void send_position_update()
     buffer_tx[0] = cmd_id;
     if (checksum_error)
       buffer_tx[1] = CMD_CHECKSUM_ERROR; // cmd_execution_status
+    else if (mcu_cmd_execution_in_progress)
+      buffer_tx[1] = IN_PROGRESS;
     else
-      buffer_tx[1] = mcu_cmd_execution_in_progress ? IN_PROGRESS : COMPLETED_WITHOUT_ERRORS; // cmd_execution_status
+      buffer_tx[1] = mcu_cmd_execution_status; // COMPLETED_WITHOUT_ERRORS or CMD_EXECUTION_ERROR
 
     uint32_t X_pos_int32t = uint32_t( X_use_encoder ? X_pos : int32_t(tmc4361A_currentPosition(&tmc4361[x])) );
     buffer_tx[2] = byte(X_pos_int32t >> 24);
@@ -74,9 +82,34 @@ void send_position_update()
     buffer_tx[18] &= ~ (1 << BIT_POS_JOYSTICK_BUTTON); // clear the joystick button bit
     buffer_tx[18] = buffer_tx[18] | joystick_button_pressed << BIT_POS_JOYSTICK_BUTTON;
 
-    // Clear reserved bytes to avoid stale data affecting the checksum
-    buffer_tx[19] = 0;
-    buffer_tx[20] = 0;
+    // Bytes 19-20: W axis current microstep position as signed int16,
+    // big-endian. Used by the host to detect silent filter-wheel move
+    // failures (W has no encoder feedback). Gated on `enable_filterwheel`
+    // because tmc4361[w] is uninitialized until INITFILTERWHEEL arrives
+    // (init.cpp:123 only inits indices 0..STAGE_AXES-1); reading from an
+    // uninitialized struct would dereference a NULL config pointer.
+    //
+    // Saturates rather than wraps: with ~1600 usteps/slot, an 8-slot wheel
+    // stays well within ±32k between homes during normal use, but a long
+    // monotonic sequence (or a misconfigured larger wheel) could overflow.
+    // The host sees the saturated value and the verification will mismatch,
+    // triggering a re-home — safer than a silent wrap.
+    // Byte 21 stays reserved (future use for W2 position).
+    if (enable_filterwheel)
+    {
+        long w_pos = tmc4361A_currentPosition(&tmc4361[w]);
+        int16_t W_pos_int16;
+        if (w_pos > INT16_MAX) W_pos_int16 = INT16_MAX;
+        else if (w_pos < INT16_MIN) W_pos_int16 = INT16_MIN;
+        else W_pos_int16 = (int16_t) w_pos;
+        buffer_tx[19] = byte((W_pos_int16 >> 8) & 0xFF);
+        buffer_tx[20] = byte(W_pos_int16 & 0xFF);
+    }
+    else
+    {
+        buffer_tx[19] = 0;
+        buffer_tx[20] = 0;
+    }
     buffer_tx[21] = 0;
 
     // Firmware version in byte 22: high nibble = major, low nibble = minor

software/control/_def.py

@@ -361,6 +361,14 @@ def port_index_to_source_code(port_index: int) -> int:
 # Response byte positions for MCU protocol (24-byte response)
 RESPONSE_BYTE_FIRMWARE_VERSION = 22  # Nibble-encoded: high=major, low=minor
 
+# Bytes 19-20: W axis current microstep position (signed int16, big-endian).
+# Populated by firmware >= v1.2; zero on older firmware. Used by the filter
+# wheel controller to verify that a MOVE_W command actually moved the motor.
+RESPONSE_BYTE_W_POS_HI = 19
+RESPONSE_BYTE_W_POS_LO = 20
+# Firmware version at which W position broadcast became available.
+MIN_FW_VERSION_W_POS_BROADCAST = (1, 2)
+
 # Serial watchdog (illumination auto-shutoff safety)
 # Must match firmware constants in constants.h
 DEFAULT_WATCHDOG_TIMEOUT_MS = 5000  # 5 seconds (matches firmware)
@@ -665,6 +673,13 @@ def is_piezo_only(self) -> bool:
 HAS_ENCODER_Z = False
 HAS_ENCODER_W = False
 
+# Maximum mismatch (in microsteps) tolerated when verifying a filter-wheel
+# move against the broadcast W position. Firmware completion guarantees
+# exact match for stepper-only setups (operations.cpp:515); the small budget
+# only absorbs PID/encoder settling on closed-loop W setups. ±10 ≈ 0.6% of
+# one slot (1600 microsteps).
+W_POS_TOLERANCE_USTEPS = 10
+
 # enable PID control
 ENABLE_PID_X = False
 ENABLE_PID_Y = False