@@ -928,13 +928,14 @@ static bool SetIMUEnabled(SDL_DriverSwitch_Context *ctx, bool enabled)
928
928
929
929
static bool LoadStickCalibration (SDL_DriverSwitch_Context * ctx )
930
930
{
931
- Uint8 * pLeftStickCal ;
932
- Uint8 * pRightStickCal ;
931
+ Uint8 * pLeftStickCal = NULL ;
932
+ Uint8 * pRightStickCal = NULL ;
933
933
size_t stick , axis ;
934
934
SwitchSubcommandInputPacket_t * user_reply = NULL ;
935
935
SwitchSubcommandInputPacket_t * factory_reply = NULL ;
936
936
SwitchSPIOpData_t readUserParams ;
937
937
SwitchSPIOpData_t readFactoryParams ;
938
+ Uint8 userParamsReadSuccessCount = 0 ;
938
939
939
940
// Read User Calibration Info
940
941
readUserParams .unAddress = k_unSPIStickUserCalibrationStartOffset ;
@@ -947,33 +948,46 @@ static bool LoadStickCalibration(SDL_DriverSwitch_Context *ctx)
947
948
readFactoryParams .unAddress = k_unSPIStickFactoryCalibrationStartOffset ;
948
949
readFactoryParams .ucLength = k_unSPIStickFactoryCalibrationLength ;
949
950
950
- const int MAX_ATTEMPTS = 3 ;
951
- for (int attempt = 0 ; ; ++ attempt ) {
952
- if (!WriteSubcommand (ctx , k_eSwitchSubcommandIDs_SPIFlashRead , (uint8_t * )& readFactoryParams , sizeof (readFactoryParams ), & factory_reply )) {
953
- return false;
954
- }
955
-
956
- if (factory_reply -> stickFactoryCalibration .opData .unAddress == k_unSPIStickFactoryCalibrationStartOffset ) {
957
- // We successfully read the calibration data
958
- break ;
959
- }
960
-
961
- if (attempt == MAX_ATTEMPTS ) {
962
- return false;
963
- }
964
- }
965
-
966
951
// Automatically select the user calibration if magic bytes are set
967
952
if (user_reply && user_reply -> stickUserCalibration .rgucLeftMagic [0 ] == 0xB2 && user_reply -> stickUserCalibration .rgucLeftMagic [1 ] == 0xA1 ) {
953
+ userParamsReadSuccessCount += 1 ;
968
954
pLeftStickCal = user_reply -> stickUserCalibration .rgucLeftCalibration ;
969
- } else {
970
- pLeftStickCal = factory_reply -> stickFactoryCalibration .rgucLeftCalibration ;
971
955
}
972
956
973
957
if (user_reply && user_reply -> stickUserCalibration .rgucRightMagic [0 ] == 0xB2 && user_reply -> stickUserCalibration .rgucRightMagic [1 ] == 0xA1 ) {
958
+ userParamsReadSuccessCount += 1 ;
974
959
pRightStickCal = user_reply -> stickUserCalibration .rgucRightCalibration ;
975
- } else {
976
- pRightStickCal = factory_reply -> stickFactoryCalibration .rgucRightCalibration ;
960
+ }
961
+
962
+ // Only read the factory calibration info if we failed to receive the correct magic bytes
963
+ if (userParamsReadSuccessCount < 2 ) {
964
+ // Read Factory Calibration Info
965
+ readFactoryParams .unAddress = k_unSPIStickFactoryCalibrationStartOffset ;
966
+ readFactoryParams .ucLength = k_unSPIStickFactoryCalibrationLength ;
967
+
968
+ const int MAX_ATTEMPTS = 3 ;
969
+ for (int attempt = 0 ;; ++ attempt ) {
970
+ if (!WriteSubcommand (ctx , k_eSwitchSubcommandIDs_SPIFlashRead , (uint8_t * )& readFactoryParams , sizeof (readFactoryParams ), & factory_reply )) {
971
+ return false;
972
+ }
973
+
974
+ if (factory_reply -> stickFactoryCalibration .opData .unAddress == k_unSPIStickFactoryCalibrationStartOffset ) {
975
+ // We successfully read the calibration data
976
+ pLeftStickCal = factory_reply -> stickFactoryCalibration .rgucLeftCalibration ;
977
+ pRightStickCal = factory_reply -> stickFactoryCalibration .rgucRightCalibration ;
978
+ break ;
979
+ }
980
+
981
+ if (attempt == MAX_ATTEMPTS ) {
982
+ return false;
983
+ }
984
+ }
985
+ }
986
+
987
+ // If we still don't have calibration data, return false
988
+ if (pLeftStickCal == NULL || pRightStickCal == NULL )
989
+ {
990
+ return false;
977
991
}
978
992
979
993
/* Stick calibration values are 12-bits each and are packed by bit
0 commit comments