Temporary fix for mpu9250 compass (#560)

Author: digitalentity

src/main/drivers/compass_ak8963.c

@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <math.h>
 
@@ -261,12 +262,13 @@ bool ak8963Read(int16_t *magData)
     bool ack = false;
     uint8_t buf[7];
 
-    // set magData to zero for case of failed read
-    magData[X] = 0;
-    magData[Y] = 0;
-    magData[Z] = 0;
+    static bool lastReadResult = false;
 
 #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
+    static int16_t cachedMagData[3];
+
+    // set magData to latest cached value
+    memcpy(magData, cachedMagData, sizeof(cachedMagData));
 
     // we currently need a different approach for the MPU9250 connected via SPI.
     // we cannot use the ak8963SensorRead() method for SPI, it is to slow and blocks for far too long.
@@ -280,12 +282,12 @@ bool ak8963Read(int16_t *magData)
         case CHECK_STATUS:
             ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
             state++;
-            return false;
+            return lastReadResult;
 
         case WAITING_FOR_STATUS: {
             uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
             if (timeRemaining) {
-                return false;
+                return lastReadResult;
             }
 
             ack = ak8963SensorCompleteRead(&buf[0]);
@@ -299,22 +301,23 @@ bool ak8963Read(int16_t *magData)
                     retry = false;
                     goto restart;
                 }
-                return false;
-            }
 
+                lastReadResult = false;
+                return lastReadResult;
+            }
 
             // read the 6 bytes of data and the status2 register
             ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7);
 
             state++;
 
-            return false;
+            return lastReadResult;
         }
 
         case WAITING_FOR_DATA: {
             uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
             if (timeRemaining) {
-                return false;
+                return lastReadResult;
             }
 
             ack = ak8963SensorCompleteRead(&buf[0]);
@@ -326,25 +329,32 @@ bool ak8963Read(int16_t *magData)
     uint8_t status = buf[0];
 
     if (!ack || (status & STATUS1_DATA_READY) == 0) {
-        return false;
+        lastReadResult = false;
+        return lastReadResult;
     }
 
     ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]);
 #endif
+
     uint8_t status2 = buf[6];
     if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
-        return false;
+        lastReadResult = false;
+        return lastReadResult;
     }
 
     magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
     magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
     magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
 
 #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
+    // cache mag data for reuse
+    memcpy(cachedMagData, magData, sizeof(cachedMagData));
     state = CHECK_STATUS;
-    return true;
+    lastReadResult = true;
 #else
-    return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again
+    lastReadResult = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again
 #endif
+
+    return lastReadResult;
 }
 #endif

src/main/main.c

@@ -615,7 +615,7 @@ int main(void)
 #endif
 #ifdef MAG
     setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
-#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
+#if (defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE)) && defined(USE_MAG_AK8963)
     // fixme temporary solution for AK6983 via slave I2C on MPU9250
     rescheduleTask(TASK_COMPASS, 1000000 / 40);
 #endif

src/main/sensors/compass.c

@@ -87,6 +87,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
         magADC[Z] = 0;
         return;
     }
+
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         magADC[axis] = magADCRaw[axis];  // int32_t copy to work with
     }