From 043d0efbb0909ec6965ea9c49a0b3bc4205fe80d Mon Sep 17 00:00:00 2001
From: Jan Soeren Schell <jansoeren.schell@bosch-sensortec.com>
Date: Thu, 18 May 2017 11:34:41 +0200
Subject: [PATCH] move missed header files & bugfixes in sensors_bosch.c

---
 src/config/config.h                           |  2 +-
 .../interface/bstdr_comm_support.h            |  0
 .../{ => bosch}/interface/bstdr_types.h       |  0
 src/hal/src/sensors_bosch.c                   | 33 +++++++++++--------
 4 files changed, 21 insertions(+), 14 deletions(-)
 rename src/drivers/{ => bosch}/interface/bstdr_comm_support.h (100%)
 rename src/drivers/{ => bosch}/interface/bstdr_types.h (100%)

diff --git a/src/config/config.h b/src/config/config.h
index 222b1ce1ee..4e579ef763 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -46,7 +46,7 @@
 #include "trace.h"
 #include "usec_time.h"
 
-#define PROTOCOL_VERSION 3
+#define PROTOCOL_VERSION 3
 
 #ifdef STM32F4XX
   #define P_NAME "Crazyflie 2.0"
diff --git a/src/drivers/interface/bstdr_comm_support.h b/src/drivers/bosch/interface/bstdr_comm_support.h
similarity index 100%
rename from src/drivers/interface/bstdr_comm_support.h
rename to src/drivers/bosch/interface/bstdr_comm_support.h
diff --git a/src/drivers/interface/bstdr_types.h b/src/drivers/bosch/interface/bstdr_types.h
similarity index 100%
rename from src/drivers/interface/bstdr_types.h
rename to src/drivers/bosch/interface/bstdr_types.h
diff --git a/src/hal/src/sensors_bosch.c b/src/hal/src/sensors_bosch.c
index 4fb530c1ea..e752db5c91 100644
--- a/src/hal/src/sensors_bosch.c
+++ b/src/hal/src/sensors_bosch.c
@@ -307,10 +307,13 @@ static void sensorsTaskInit(void)
 }
 
 static void sensorsGyroGet(Axis3i16* dataOut, uint8_t device) {
+  static struct bmi160_sensor_data temp;
   switch(device) {
     case SENSORS_BMI160:
-      bmi160_get_sensor_data(BMI160_GYRO_ONLY, NULL,
-                             (struct bmi160_sensor_data*)dataOut, &bmi160Dev);
+      bmi160_get_sensor_data(BMI160_GYRO_ONLY, NULL, &temp, &bmi160Dev);
+      dataOut->x = temp.x;
+      dataOut->y = temp.y;
+      dataOut->z = temp.z;
       break;
     case SENSORS_BMI055:
       bmi055_get_gyro_data(
@@ -320,11 +323,13 @@ static void sensorsGyroGet(Axis3i16* dataOut, uint8_t device) {
 }
 
 static void sensorsAccelGet(Axis3i16* dataOut, uint8_t device) {
+  static struct bmi160_sensor_data temp;
   switch(device) {
     case SENSORS_BMI160:
-      bmi160_get_sensor_data(BMI160_ACCEL_ONLY,
-                             (struct bmi160_sensor_data*)dataOut, NULL,
-                             &bmi160Dev);
+      bmi160_get_sensor_data(BMI160_ACCEL_ONLY, &temp, NULL, &bmi160Dev);
+      dataOut->x = temp.x;
+      dataOut->y = temp.y;
+      dataOut->z = temp.z;
       break;
     case SENSORS_BMI055:
       bmi055_get_accel_data(
@@ -451,8 +456,8 @@ static void sensorsTask(void *param)
           sensorsGyroGet(&gyroPrim, gyroPrimInUse);
           sensorsAccelGet(&accelPrim, accelPrimInUse);
 #ifdef LOG_SEC_IMU
-          sensorsGyroGet(&gyroSecRaw, gyroSecInUse);
-          sensorsAccelGet(&gyroSecRaw, accelSecInUse);
+          sensorsGyroGet(&gyroSec, gyroSecInUse);
+          sensorsAccelGet(&accelSec, accelSecInUse);
 #endif
           /* FIXME: for sensor deck v1 realignment has to be added her */
 
@@ -475,12 +480,12 @@ static void sensorsTask(void *param)
 
           switch(accelPrimInUse) {
             case SENSORS_BMI160:
-              sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrim,
+              sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF,
                                        &bmi160AccelBias.value,
                                        SENSORS_BMI160_G_PER_LSB_CFG);
               break;
             case SENSORS_BMI055:
-              sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrim,
+              sensorsApplyBiasAndScale(&accelPrimScaled, &accelPrimLPF,
                                        &bmi055AccelBias.value,
                                        SENSORS_BMI055_G_PER_LSB_CFG);
               break;
@@ -508,12 +513,12 @@ static void sensorsTask(void *param)
 
           switch(accelSecInUse) {
             case SENSORS_BMI160:
-              sensorsApplyBiasAndScale(&accelSecScaled, &accelSec,
+              sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF,
                                        &bmi160AccelBias.value,
                                        SENSORS_BMI160_G_PER_LSB_CFG);
               break;
             case SENSORS_BMI055:
-              sensorsApplyBiasAndScale(&accelSecScaled, &accelSec,
+              sensorsApplyBiasAndScale(&accelSecScaled, &accelSecLPF,
                                        &bmi055AccelBias.value,
                                        SENSORS_BMI055_G_PER_LSB_CFG);
               break;
@@ -629,7 +634,8 @@ static void calcMean(BiasObj* bias, Axis3f* mean) {
   Axis3i16* elem;
   int64_t sum[GYRO_NBR_OF_AXES] = {0};
 
-  for (elem = bias->bufStart; elem != (bias->bufStart+SENSORS_NBR_OF_BIAS_SAMPLES); elem++)
+  for (elem = bias->bufStart;
+       elem != (bias->bufStart+SENSORS_NBR_OF_BIAS_SAMPLES); elem++)
     {
       sum[0] += elem->x;
       sum[1] += elem->y;
@@ -644,7 +650,8 @@ static void calcMean(BiasObj* bias, Axis3f* mean) {
 /**
  * Calculates the variance and mean for the bias buffer.
  */
-static void calcVarianceAndMean(BiasObj* bias, Axis3f* variance, Axis3f* mean) {
+static void calcVarianceAndMean(BiasObj* bias, Axis3f* variance, Axis3f* mean)
+{
   Axis3i16* elem;
   int64_t sumSquared[GYRO_NBR_OF_AXES] = {0};