Merge pull request #2846 from iNavFlight/de_bulk_mpu_read

Bulk gyro/acc reads, temperature sensor framework

Makefile

@@ -653,6 +653,7 @@ COMMON_SRC = \
             scheduler/scheduler.c \
             sensors/acceleration.c \
             sensors/battery.c \
+            sensors/temperature.c \
             sensors/boardalignment.c \
             sensors/compass.c \
             sensors/diagnostics.c \

src/main/drivers/accgyro/accgyro_mpu.c

@@ -29,8 +29,7 @@
 #include "common/maths.h"
 #include "common/utils.h"
 
-#include "drivers/bus_i2c.h"
-#include "drivers/bus_spi.h"
+#include "drivers/bus.h"
 #include "drivers/exti.h"
 #include "drivers/io.h"
 #include "drivers/exti.h"
@@ -43,6 +42,9 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_mpu.h"
 
+// Check busDevice scratchpad memory size
+STATIC_ASSERT(sizeof(mpuContextData_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
+
 /*
  * Gyro interrupt service routine
  */
@@ -103,39 +105,63 @@ bool mpuGyroRead(gyroDev_t *gyro)
     return true;
 }
 
-bool mpuAccRead(accDev_t *acc)
-{
-    uint8_t data[6];
-
-    const bool ack = busReadBuf(acc->busDev, MPU_RA_ACCEL_XOUT_H, data, 6);
-    if (!ack) {
-        return false;
-    }
-
-    acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
-    acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
-    acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
-
-    return true;
-}
-
 bool mpuCheckDataReady(gyroDev_t* gyro)
 {
     bool ret;
     if (gyro->dataReady) {
         ret = true;
-        gyro->dataReady= false;
+        gyro->dataReady = false;
     } else {
         ret = false;
     }
     return ret;
 }
 
-/*
-void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
+static bool mpuUpdateSensorContext(busDevice_t * busDev, mpuContextData_t * ctx)
+{
+    ctx->lastReadStatus = busReadBuf(busDev, MPU_RA_ACCEL_XOUT_H, ctx->accRaw, 6 + 2 + 6);
+    return ctx->lastReadStatus;
+}
+
+bool mpuGyroReadScratchpad(gyroDev_t *gyro)
 {
-    ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
-        gyro->updateFn = updateFn;
+    busDevice_t * busDev = gyro->busDev;
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(busDev);
+
+    if (mpuUpdateSensorContext(busDev, ctx)) {
+        gyro->gyroADCRaw[X] = (int16_t)((ctx->gyroRaw[0] << 8) | ctx->gyroRaw[1]);
+        gyro->gyroADCRaw[Y] = (int16_t)((ctx->gyroRaw[2] << 8) | ctx->gyroRaw[3]);
+        gyro->gyroADCRaw[Z] = (int16_t)((ctx->gyroRaw[4] << 8) | ctx->gyroRaw[5]);
+
+        return true;
+    }
+
+    return false;
+}
+
+bool mpuAccReadScratchpad(accDev_t *acc)
+{
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+
+    if (ctx->lastReadStatus) {
+        acc->ADCRaw[X] = (int16_t)((ctx->accRaw[0] << 8) | ctx->accRaw[1]);
+        acc->ADCRaw[Y] = (int16_t)((ctx->accRaw[2] << 8) | ctx->accRaw[3]);
+        acc->ADCRaw[Z] = (int16_t)((ctx->accRaw[4] << 8) | ctx->accRaw[5]);
+        return true;
     }
+
+    return false;
+}
+
+bool mpuTemperatureReadScratchpad(gyroDev_t *gyro, int16_t * data)
+{
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+
+    if (ctx->lastReadStatus) {
+        // Convert to degC*10: degC = raw / 340 + 36.53
+        *data = (int16_t)((ctx->tempRaw[0] << 8) | ctx->tempRaw[1]) / 34 + 365;
+        return true;
+    }
+
+    return false;
 }
-*/

src/main/drivers/accgyro/accgyro_mpu.h

@@ -134,6 +134,15 @@ typedef struct mpuConfiguration_s {
     uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
 } mpuConfiguration_t;
 
+typedef struct __attribute__ ((__packed__)) mpuContextData_s {
+    uint16_t    chipMagicNumber;
+    uint8_t     lastReadStatus;
+    uint8_t     __padding;
+    uint8_t     accRaw[6];  // MPU_RA_ACCEL_XOUT_H
+    uint8_t     tempRaw[2]; // MPU_RA_TEMP_OUT_H
+    uint8_t     gyroRaw[6]; // MPU_RA_GYRO_XOUT_H
+} mpuContextData_t;
+
 enum gyro_fsr_e {
     INV_FSR_250DPS = 0,
     INV_FSR_500DPS,
@@ -163,11 +172,12 @@ enum accel_fsr_e {
 };
 
 struct gyroDev_s;
+struct accDev_s;
 void mpuIntExtiInit(struct gyroDev_s *gyro);
 
-struct accDev_s;
-bool mpuAccRead(struct accDev_s *acc);
 bool mpuGyroRead(struct gyroDev_s *gyro);
-void mpuDetect(struct gyroDev_s *gyro);
+bool mpuGyroReadScratchpad(struct gyroDev_s *gyro);
+bool mpuAccReadScratchpad(struct accDev_s *acc);
+bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data);
+
 bool mpuCheckDataReady(struct gyroDev_s *gyro);
-void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);

src/main/drivers/accgyro/accgyro_mpu6000.c

@@ -142,12 +142,13 @@ bool mpu6000AccDetect(accDev_t *acc)
         return false;
     }
 
-    if (busDeviceReadScratchpad(acc->busDev) != 0xFFFF6000) {
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber != 0x6860) {
         return false;
     }
 
     acc->initFn = mpu6000AccInit;
-    acc->readFn = mpuAccRead;
+    acc->readFn = mpuAccReadScratchpad;
 
     return true;
 }
@@ -207,11 +208,14 @@ bool mpu6000GyroDetect(gyroDev_t *gyro)
         return false;
     }
 
-    busDeviceWriteScratchpad(gyro->busDev, 0xFFFF6000);    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+    ctx->chipMagicNumber = 0x6860;
 
     gyro->initFn = mpu6000AccAndGyroInit;
-    gyro->readFn = mpuGyroRead;
+    gyro->readFn = mpuGyroReadScratchpad;
     gyro->intStatusFn = mpuCheckDataReady;
+    gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 
     return true;

src/main/drivers/accgyro/accgyro_mpu6050.c

@@ -106,9 +106,8 @@ static void mpu6050AccAndGyroInit(gyroDev_t *gyro)
 
 static void mpu6050AccInit(accDev_t *acc)
 {
-    uint32_t magic = busDeviceReadScratchpad(acc->busDev);
-
-    if (magic == 0xFFFF6050) {
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber == 0x6850) {
         acc->acc_1G = 512 * 8;
     }
     else {
@@ -123,10 +122,10 @@ bool mpu6050AccDetect(accDev_t *acc)
         return false;
     }
 
-    uint32_t magic = busDeviceReadScratchpad(acc->busDev);
-    if (magic == 0x00006050 || magic == 0xFFFF6050) {
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber == 0x6850 || ctx->chipMagicNumber == 0x6050) {
         acc->initFn = mpu6050AccInit;
-        acc->readFn = mpuAccRead;
+        acc->readFn = mpuAccReadScratchpad;
         return true;
     }
 
@@ -204,11 +203,14 @@ bool mpu6050GyroDetect(gyroDev_t *gyro)
         return false;
     }
 
-    busDeviceWriteScratchpad(gyro->busDev, res == MPU6050_FULL_RESOLUTION ? 0xFFFF6050 : 0x00006050);
+    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+    ctx->chipMagicNumber = res == MPU6050_FULL_RESOLUTION ? 0x6850 : 0x6050;
 
     gyro->initFn = mpu6050AccAndGyroInit;
-    gyro->readFn = mpuGyroRead;
+    gyro->readFn = mpuGyroReadScratchpad;
     gyro->intStatusFn = mpuCheckDataReady;
+    gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 
     return true;

src/main/drivers/accgyro/accgyro_mpu6500.c

@@ -55,12 +55,13 @@ bool mpu6500AccDetect(accDev_t *acc)
         return false;
     }
 
-    if (busDeviceReadScratchpad(acc->busDev) != 0xFFFF6500) {
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber != 0x6860) {
         return false;
     }
 
     acc->initFn = mpu6500AccInit;
-    acc->readFn = mpuAccRead;
+    acc->readFn = mpuAccReadScratchpad;
 
     return true;
 }
@@ -157,11 +158,14 @@ bool mpu6500GyroDetect(gyroDev_t *gyro)
         return false;
     }
 
-    busDeviceWriteScratchpad(gyro->busDev, 0xFFFF6500);    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+    ctx->chipMagicNumber = 0x6500;
 
     gyro->initFn = mpu6500AccAndGyroInit;
-    gyro->readFn = mpuGyroRead;
+    gyro->readFn = mpuGyroReadScratchpad;
     gyro->intStatusFn = mpuCheckDataReady;
+    gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 
     return true;

src/main/drivers/accgyro/accgyro_mpu9250.c

@@ -55,12 +55,13 @@ bool mpu9250AccDetect(accDev_t *acc)
         return false;
     }
 
-    if (busDeviceReadScratchpad(acc->busDev) != 0xFFFF9250) {
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber != 0x9250) {
         return false;
     }
 
     acc->initFn = mpu9250AccInit;
-    acc->readFn = mpuAccRead;
+    acc->readFn = mpuAccReadScratchpad;
 
     return true;
 }
@@ -154,11 +155,14 @@ bool mpu9250GyroDetect(gyroDev_t *gyro)
         return false;
     }
 
-    busDeviceWriteScratchpad(gyro->busDev, 0xFFFF9250);    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    // Magic number for ACC detection to indicate that we have detected MPU6000 gyro
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+    ctx->chipMagicNumber = 0x9250;
 
     gyro->initFn = mpu9250AccAndGyroInit;
-    gyro->readFn = mpuGyroRead;
+    gyro->readFn = mpuGyroReadScratchpad;
     gyro->intStatusFn = mpuCheckDataReady;
+    gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 
     return true;

src/main/drivers/bus.c

@@ -196,12 +196,17 @@ void busSetSpeed(const busDevice_t * dev, busSpeed_e speed)
 
 uint32_t busDeviceReadScratchpad(const busDevice_t * dev)
 {
-    return dev->scratchpad;
+    return dev->scratchpad[0];
 }
 
 void busDeviceWriteScratchpad(busDevice_t * dev, uint32_t value)
 {
-    dev->scratchpad = value;
+    dev->scratchpad[0] = value;
+}
+
+void * busDeviceGetScratchpadMemory(const busDevice_t * dev)
+{
+    return (void *)dev->scratchpad;
 }
 
 bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length)

src/main/drivers/bus.h

@@ -45,6 +45,8 @@
 #define BUS_I2C4            I2CDEV_4
 #define BUS_I2C_EMULATED    I2CINVALID
 
+#define BUS_SCRATCHPAD_MEMORY_SIZE      (20)
+
 typedef enum {
     BUS_SPEED_INITIALIZATION = 0,
     BUS_SPEED_SLOW           = 1,
@@ -159,8 +161,9 @@ typedef struct busDevice_s {
 #endif
     } busdev;
     IO_t irqPin;                    // Device IRQ pin. Bus system will only assign IO_t object to this var. Initialization is up to device driver
-    uint32_t scratchpad;            // Memory where device driver can store persistent data. Zeroed out when initializing the device for the first time
-                                    // Useful when once device is shared between several sensors (like MPU/ICM acc-gyro sensors)
+    uint32_t scratchpad[BUS_SCRATCHPAD_MEMORY_SIZE / sizeof(uint32_t)];     // Memory where device driver can store persistent data. Zeroed out when initializing the device 
+                                                                            // for the first time. Useful when once device is shared between several sensors 
+                                                                            // (like MPU/ICM acc-gyro sensors)
 } busDevice_t;
 
 #ifdef __APPLE__
@@ -254,6 +257,7 @@ void busDeviceDeInit(busDevice_t * dev);
 
 uint32_t busDeviceReadScratchpad(const busDevice_t * dev);
 void busDeviceWriteScratchpad(busDevice_t * dev, uint32_t value);
+void * busDeviceGetScratchpadMemory(const busDevice_t * dev);
 
 void busSetSpeed(const busDevice_t * dev, busSpeed_e speed);
 

src/main/drivers/compass/compass_mpu9250.c

@@ -291,7 +291,8 @@ bool mpu9250CompassDetect(magDev_t * mag)
     }
 
     // Check if Gyro driver initialized the chip
-    if (busDeviceReadScratchpad(mag->busDev) != 0xFFFF9250) {
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(mag->busDev);
+    if (ctx->chipMagicNumber != 0x9250) {
         return false;
     }
 

src/main/fc/fc_tasks.c

@@ -68,6 +68,7 @@
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
+#include "sensors/temperature.h"
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/compass.h"
@@ -111,6 +112,12 @@ void taskUpdateBattery(timeUs_t currentTimeUs)
     batMonitoringLastServiced = currentTimeUs;
 }
 
+void taskUpdateTemperature(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    temperatureUpdate();
+}
+
 #ifdef USE_GPS
 void taskProcessGPS(timeUs_t currentTimeUs)
 {
@@ -307,6 +314,7 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_LIGHTS, true);
 #endif
     setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
+    setTaskEnabled(TASK_TEMPERATURE, true);
     setTaskEnabled(TASK_RX, true);
 #ifdef USE_GPS
     setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
@@ -452,6 +460,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 
+    [TASK_TEMPERATURE] = {
+        .taskName = "TEMPERATURE",
+        .taskFunc = taskUpdateTemperature,
+        .desiredPeriod = TASK_PERIOD_HZ(1),       // 1 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+
     [TASK_RX] = {
         .taskName = "RX",
         .checkFunc = taskUpdateRxCheck,

src/main/scheduler/scheduler.h

@@ -62,6 +62,7 @@ typedef enum {
     TASK_RX,
     TASK_SERIAL,
     TASK_BATTERY,
+    TASK_TEMPERATURE,
 #ifdef BEEPER
     TASK_BEEPER,
 #endif