diff --git a/src/Vehicle/InitialConnectStateMachine.cc b/src/Vehicle/InitialConnectStateMachine.cc
index cf9bbef..eebf407 100644
--- a/src/Vehicle/InitialConnectStateMachine.cc
+++ b/src/Vehicle/InitialConnectStateMachine.cc
@@ -232,9 +232,10 @@ void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stat
 
 void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode)
 {
+    InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
+
     if (result != MAV_RESULT_ACCEPTED) {
-        InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
-        Vehicle*                    vehicle         = connectMachine->_vehicle;
+        Vehicle* vehicle = connectMachine->_vehicle;
 
         switch (failureCode) {
         case Vehicle::MavCmdResultCommandResultOnly:
@@ -252,8 +253,8 @@ void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHa
         vehicle->_mavlinkProtocolRequestComplete = true;
         vehicle->_setMaxProtoVersionFromBothSources();
         vehicle->_waitForMavlinkMessageClear();
-        connectMachine->advance();
     }
+    connectMachine->advance();
 }
 
 void InitialConnectStateMachine::_waitForProtocolVersionResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message)