From 63e8ba02e55c411837ce2fec20a837a99b36863e Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Wed, 21 Apr 2021 14:50:45 -0700
Subject: [PATCH] Fix state machine not being advanced in all cases

---
 src/Vehicle/InitialConnectStateMachine.cc | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

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)