From 1d3bfc86d46c4238a03efc51ed407feec828d6ad Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 14 Dec 2016 14:32:38 +0000
Subject: [PATCH] Updated sensor driver read signatures

---
 src/main/drivers/accgyro.h            |  5 +++--
 src/main/drivers/accgyro_adxl345.c    | 14 +++++++-------
 src/main/drivers/accgyro_bma280.c     |  8 ++++----
 src/main/drivers/accgyro_fake.c       | 13 +++++++------
 src/main/drivers/accgyro_lsm303dlhc.c |  8 ++++----
 src/main/drivers/accgyro_mma845x.c    |  8 ++++----
 src/main/drivers/accgyro_mpu.c        |  8 ++++----
 src/main/drivers/accgyro_mpu.h        |  3 ++-
 src/main/drivers/accgyro_mpu3050.c    |  4 +++-
 src/main/drivers/compass_ak8963.c     |  3 ++-
 src/main/drivers/compass_ak8975.c     |  3 ++-
 src/main/drivers/compass_fake.c       |  3 ++-
 src/main/drivers/compass_hmc5883l.c   |  3 ++-
 src/main/drivers/sensor.h             | 10 ++++++----
 src/main/fc/mw.c                      |  2 +-
 src/main/sensors/acceleration.c       |  8 +++-----
 16 files changed, 56 insertions(+), 47 deletions(-)

diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h
index c9d23dac3e..746279a452 100644
--- a/src/main/drivers/accgyro.h
+++ b/src/main/drivers/accgyro.h
@@ -37,7 +37,7 @@
 typedef struct gyroDev_s {
     sensorGyroInitFuncPtr init;                             // initialize function
     sensorGyroReadFuncPtr read;                             // read 3 axis data function
-    sensorReadFuncPtr temperature;                          // read temperature if available
+    sensorGyroReadDataFuncPtr temperature;                          // read temperature if available
     sensorGyroInterruptStatusFuncPtr intStatus;
     extiCallbackRec_t exti;
     float scale;                                            // scalefactor
@@ -49,8 +49,9 @@ typedef struct gyroDev_s {
 
 typedef struct accDev_s {
     sensorAccInitFuncPtr init;                              // initialize function
-    sensorReadFuncPtr read;                                 // read 3 axis data function
+    sensorAccReadFuncPtr read;                              // read 3 axis data function
     uint16_t acc_1G;
+    int16_t ADCRaw[XYZ_AXIS_COUNT];
     char revisionCode;                                      // a revision code for the sensor, if known
     sensor_align_e accAlign;
 } accDev_t;
diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c
index 122062318b..9e2b623b40 100644
--- a/src/main/drivers/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro_adxl345.c
@@ -76,7 +76,7 @@ static void adxl345Init(accDev_t *acc)
 
 uint8_t acc_samples = 0;
 
-static bool adxl345Read(int16_t *accelData)
+static bool adxl345Read(accDev_t *acc)
 {
     uint8_t buf[8];
 
@@ -99,9 +99,9 @@ static bool adxl345Read(int16_t *accelData)
             z += (int16_t)(buf[4] + (buf[5] << 8));
             samples_remaining = buf[7] & 0x7F;
         } while ((i < 32) && (samples_remaining > 0));
-        accelData[0] = x / i;
-        accelData[1] = y / i;
-        accelData[2] = z / i;
+        acc->ADCRaw[0] = x / i;
+        acc->ADCRaw[1] = y / i;
+        acc->ADCRaw[2] = z / i;
         acc_samples = i;
     } else {
 
@@ -109,9 +109,9 @@ static bool adxl345Read(int16_t *accelData)
             return false;
         }
 
-        accelData[0] = buf[0] + (buf[1] << 8);
-        accelData[1] = buf[2] + (buf[3] << 8);
-        accelData[2] = buf[4] + (buf[5] << 8);
+        acc->ADCRaw[0] = buf[0] + (buf[1] << 8);
+        acc->ADCRaw[1] = buf[2] + (buf[3] << 8);
+        acc->ADCRaw[2] = buf[4] + (buf[5] << 8);
     }
 
     return true;
diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c
index 3c285a9428..f65b22286a 100644
--- a/src/main/drivers/accgyro_bma280.c
+++ b/src/main/drivers/accgyro_bma280.c
@@ -40,7 +40,7 @@ static void bma280Init(accDev_t *acc)
     acc->acc_1G = 512 * 8;
 }
 
-static bool bma280Read(int16_t *accelData)
+static bool bma280Read(accDev_t *acc)
 {
     uint8_t buf[6];
 
@@ -49,9 +49,9 @@ static bool bma280Read(int16_t *accelData)
     }
 
     // Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
-    accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
-    accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
-    accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
+    acc->ADCRaw[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
+    acc->ADCRaw[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
+    acc->ADCRaw[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
 
     return true;
 }
diff --git a/src/main/drivers/accgyro_fake.c b/src/main/drivers/accgyro_fake.c
index c592f01846..9e661f55a6 100644
--- a/src/main/drivers/accgyro_fake.c
+++ b/src/main/drivers/accgyro_fake.c
@@ -51,9 +51,10 @@ static bool fakeGyroRead(gyroDev_t *gyro)
     return true;
 }
 
-static bool fakeGyroReadTemperature(int16_t *tempData)
+static bool fakeGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
 {
-    UNUSED(tempData);
+    UNUSED(gyro);
+    UNUSED(temperatureData);
     return true;
 }
 
@@ -91,11 +92,11 @@ void fakeAccSet(int16_t x, int16_t y, int16_t z)
     fakeAccData[Z] = z;
 }
 
-static bool fakeAccRead(int16_t *accData)
+static bool fakeAccRead(accDev_t *acc)
 {
-    accData[X] = fakeAccData[X];
-    accData[Y] = fakeAccData[Y];
-    accData[Z] = fakeAccData[Z];
+    acc->ADCRaw[X] = fakeAccData[X];
+    acc->ADCRaw[Y] = fakeAccData[Y];
+    acc->ADCRaw[Z] = fakeAccData[Z];
     return true;
 }
 
diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c
index 71baf89f15..ade701a38f 100644
--- a/src/main/drivers/accgyro_lsm303dlhc.c
+++ b/src/main/drivers/accgyro_lsm303dlhc.c
@@ -133,7 +133,7 @@ void lsm303dlhcAccInit(accDev_t *acc)
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static bool lsm303dlhcAccRead(int16_t *gyroADC)
+static bool lsm303dlhcAccRead(accDev_t *acc)
 {
     uint8_t buf[6];
 
@@ -144,9 +144,9 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
     }
 
     // the values range from -8192 to +8191
-    gyroADC[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
-    gyroADC[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
-    gyroADC[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
+    acc->ADCRaw[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
+    acc->ADCRaw[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2;
+    acc->ADCRaw[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2;
 
 #if 0
     debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c
index 7dcb4ed194..96fed71c3b 100644
--- a/src/main/drivers/accgyro_mma845x.c
+++ b/src/main/drivers/accgyro_mma845x.c
@@ -111,7 +111,7 @@ static void mma8452Init(accDev_t *acc)
     acc->acc_1G = 256;
 }
 
-static bool mma8452Read(int16_t *accelData)
+static bool mma8452Read(accDev_t *acc)
 {
     uint8_t buf[6];
 
@@ -119,9 +119,9 @@ static bool mma8452Read(int16_t *accelData)
         return false;
     }
 
-    accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
-    accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
-    accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
+    acc->ADCRaw[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
+    acc->ADCRaw[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
+    acc->ADCRaw[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index 77ac0b8c2c..d1ee898eec 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/src/main/drivers/accgyro_mpu.c
@@ -290,7 +290,7 @@ static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
     return ack;
 }
 
-bool mpuAccRead(int16_t *accData)
+bool mpuAccRead(accDev_t *acc)
 {
     uint8_t data[6];
 
@@ -299,9 +299,9 @@ bool mpuAccRead(int16_t *accData)
         return false;
     }
 
-    accData[0] = (int16_t)((data[0] << 8) | data[1]);
-    accData[1] = (int16_t)((data[2] << 8) | data[3]);
-    accData[2] = (int16_t)((data[4] << 8) | data[5]);
+    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;
 }
diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h
index cb97af5cec..bca217fb21 100644
--- a/src/main/drivers/accgyro_mpu.h
+++ b/src/main/drivers/accgyro_mpu.h
@@ -186,7 +186,8 @@ extern mpuDetectionResult_t mpuDetectionResult;
 void mpuConfigureDataReadyInterruptHandling(void);
 struct gyroDev_s;
 void mpuGyroInit(struct gyroDev_s *gyro);
-bool mpuAccRead(int16_t *accData);
+struct accDev_s;
+bool mpuAccRead(struct accDev_s *acc);
 bool mpuGyroRead(struct gyroDev_s *gyro);
 mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse);
 bool mpuCheckDataReady(struct gyroDev_s *gyro);
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index fe4692c205..baf605e575 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -21,6 +21,7 @@
 #include "platform.h"
 
 #include "common/maths.h"
+#include "common/utils.h"
 
 #include "system.h"
 #include "exti.h"
@@ -62,8 +63,9 @@ static void mpu3050Init(gyroDev_t *gyro)
     mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
 }
 
-static bool mpu3050ReadTemperature(int16_t *tempData)
+static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
 {
+    UNUSED(gyro);
     uint8_t buf[2];
     if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
         return false;
diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c
index 584b8a00a8..53f72e202c 100644
--- a/src/main/drivers/compass_ak8963.c
+++ b/src/main/drivers/compass_ak8963.c
@@ -188,7 +188,7 @@ static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
 }
 #endif
 
-static void ak8963Init()
+static bool ak8963Init()
 {
     uint8_t calibration[3];
     uint8_t status;
@@ -219,6 +219,7 @@ static void ak8963Init()
 #else
     ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
 #endif
+    return true;
 }
 
 static bool ak8963Read(int16_t *magData)
diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c
index 0d8fe1192e..d2856496f8 100644
--- a/src/main/drivers/compass_ak8975.c
+++ b/src/main/drivers/compass_ak8975.c
@@ -61,7 +61,7 @@
 #define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
 #define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
 
-static void ak8975Init()
+static bool ak8975Init()
 {
     uint8_t buffer[3];
     uint8_t status;
@@ -84,6 +84,7 @@ static void ak8975Init()
 
     // Trigger first measurement
     i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
+    return true;
 }
 
 #define BIT_STATUS1_REG_DATA_READY              (1 << 0)
diff --git a/src/main/drivers/compass_fake.c b/src/main/drivers/compass_fake.c
index 20b58b1a16..cb8098c77d 100644
--- a/src/main/drivers/compass_fake.c
+++ b/src/main/drivers/compass_fake.c
@@ -32,12 +32,13 @@
 
 static int16_t fakeMagData[XYZ_AXIS_COUNT];
 
-static void fakeMagInit(void)
+static bool fakeMagInit(void)
 {
     // initially point north
     fakeMagData[X] = 4096;
     fakeMagData[Y] = 0;
     fakeMagData[Z] = 0;
+    return true;
 }
 
 void fakeMagSet(int16_t x, int16_t y, int16_t z)
diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c
index e96af8526b..ff0cd12d22 100644
--- a/src/main/drivers/compass_hmc5883l.c
+++ b/src/main/drivers/compass_hmc5883l.c
@@ -184,7 +184,7 @@ static bool hmc5883lRead(int16_t *magData)
     return true;
 }
 
-static void hmc5883lInit(void)
+static bool hmc5883lInit(void)
 {
     int16_t magADC[3];
     int i;
@@ -254,6 +254,7 @@ static void hmc5883lInit(void)
     }
 
     hmc5883lConfigureDataReadyInterruptHandling();
+    return true;
 }
 
 bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h
index 480aca7dc2..b9763695c3 100644
--- a/src/main/drivers/sensor.h
+++ b/src/main/drivers/sensor.h
@@ -29,12 +29,14 @@ typedef enum {
     CW270_DEG_FLIP = 8
 } sensor_align_e;
 
-struct accDev_s;
-typedef void (*sensorInitFuncPtr)(void);                    // sensor init prototype
+typedef bool (*sensorInitFuncPtr)(void);                    // sensor init prototype
 typedef bool (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
-typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc);    // sensor init prototype
+typedef bool (*sensorInterruptFuncPtr)(void);
+struct accDev_s;
+typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc);
+typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
 struct gyroDev_s;
 typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
 typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);
 typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
-typedef bool (*sensorInterruptFuncPtr)(void);
diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c
index 00e8cddda1..59e4dadec5 100644
--- a/src/main/fc/mw.c
+++ b/src/main/fc/mw.c
@@ -693,7 +693,7 @@ void subTaskMainSubprocesses(void)
 
     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
     if (gyro.dev.temperature) {
-        gyro.dev.temperature(&telemTemperature1);
+        gyro.dev.temperature(&gyro.dev, &telemTemperature1);
     }
 
 #ifdef MAG
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 8da672fc36..0b2017d3db 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -379,15 +379,13 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 {
-    int16_t accADCRaw[XYZ_AXIS_COUNT];
-
-    if (!acc.dev.read(accADCRaw)) {
+    if (!acc.dev.read(&acc.dev)) {
         return;
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        DEBUG_SET(DEBUG_ACCELEROMETER, axis, accADCRaw[axis]);
-        acc.accSmooth[axis] = accADCRaw[axis];
+        DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]);
+        acc.accSmooth[axis] = acc.dev.ADCRaw[axis];
     }
 
     if (accLpfCutHz) {