|
|
@@ -1113,16 +1113,20 @@ static bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
|
|
|
// Check for user calibration data. If it's present and set, it'll override the factory settings
|
|
|
readParams.unAddress = k_unSPIIMUUserScaleStartOffset;
|
|
|
readParams.ucLength = k_unSPIIMUUserScaleLength;
|
|
|
- if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply) && (pIMUScale[0] | pIMUScale[1] << 8) == 0xA1B2) {
|
|
|
- pIMUScale = reply->spiReadData.rgucReadData;
|
|
|
+ if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) {
|
|
|
+ Uint8 *pUserIMUScale = reply->spiReadData.rgucReadData;
|
|
|
|
|
|
- sAccelRawX = (pIMUScale[3] << 8) | pIMUScale[2];
|
|
|
- sAccelRawY = (pIMUScale[5] << 8) | pIMUScale[4];
|
|
|
- sAccelRawZ = (pIMUScale[7] << 8) | pIMUScale[6];
|
|
|
+ if ((pUserIMUScale[0] | (pUserIMUScale[1] << 8)) == 0xA1B2) {
|
|
|
+ pIMUScale = pUserIMUScale;
|
|
|
|
|
|
- sGyroRawX = (pIMUScale[15] << 8) | pIMUScale[14];
|
|
|
- sGyroRawY = (pIMUScale[17] << 8) | pIMUScale[16];
|
|
|
- sGyroRawZ = (pIMUScale[19] << 8) | pIMUScale[18];
|
|
|
+ sAccelRawX = (pIMUScale[3] << 8) | pIMUScale[2];
|
|
|
+ sAccelRawY = (pIMUScale[5] << 8) | pIMUScale[4];
|
|
|
+ sAccelRawZ = (pIMUScale[7] << 8) | pIMUScale[6];
|
|
|
+
|
|
|
+ sGyroRawX = (pIMUScale[15] << 8) | pIMUScale[14];
|
|
|
+ sGyroRawY = (pIMUScale[17] << 8) | pIMUScale[16];
|
|
|
+ sGyroRawZ = (pIMUScale[19] << 8) | pIMUScale[18];
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
// Accelerometer scale
|