Fix state machine not being advanced in all cases

src/Vehicle/InitialConnectStateMachine.cc

@@ -232,8 +232,9 @@ void InitialConnectStateMachine::_stateRequestProtocolVersion(StateMachine* stat
 
 void InitialConnectStateMachine::_protocolVersionCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, Vehicle::MavCmdResultFailureCode_t failureCode)
 {
-    if (result != MAV_RESULT_ACCEPTED) {
     InitialConnectStateMachine* connectMachine  = static_cast<InitialConnectStateMachine*>(resultHandlerData);
+
+    if (result != MAV_RESULT_ACCEPTED) {
         Vehicle* vehicle = connectMachine->_vehicle;
 
         switch (failureCode) {
@@ -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)