From e0764136480ae3606f8a7fe2e10b84c55fef6657 Mon Sep 17 00:00:00 2001
From: Arnaud Taffanel <arnaud@bitcraze.se>
Date: Wed, 27 Apr 2016 16:32:19 +0200
Subject: [PATCH] #102: Sensors: acquire pressure sensor only 100 times per
 seconds

---
 src/modules/interface/stabilizer_types.h |  3 ---
 src/modules/src/sensors_stock.c          | 27 ++++++++++++------------
 src/modules/src/stabilizer.c             |  2 +-
 3 files changed, 14 insertions(+), 18 deletions(-)

diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index 2a8f8561ad..def69f97bf 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -140,9 +140,6 @@ typedef struct setpointZ_s {
   bool isUpdate; // True = small update of setpoint, false = completely new
 } setpointZ_t;
 
-#include "FreeRTOS.h"
-#include "task.h"
-
 #define _RATE_SKIP_HZ(T, X) ((T % (1000/X)) != 0)
 
 #define RATE_SKIP_500HZ(tick) _RATE_SKIP_HZ(tick, 500)
diff --git a/src/modules/src/sensors_stock.c b/src/modules/src/sensors_stock.c
index c80825f756..e202d6a145 100644
--- a/src/modules/src/sensors_stock.c
+++ b/src/modules/src/sensors_stock.c
@@ -48,21 +48,20 @@ bool sensorsTest(void)
 
 bool sensorsAcquire(sensorData_t *sensors, const uint32_t tick)
 {
- if (RATE_SKIP_500HZ(tick)) {
-   return imu6IsCalibrated();
- }
+  if (!RATE_SKIP_500HZ(tick)) {
+    imu9Read(&sensors->gyro, &sensors->acc, &sensors->mag);
+  }
 
- imu9Read(&sensors->gyro, &sensors->acc, &sensors->mag);
- if (imuHasBarometer()) {
-   #ifdef PLATFORM_CF1
-     ms5611GetData(&sensors->baro.pressure,
-                   &sensors->baro.temperature,
-                   &sensors->baro.asl);
-   #else
-     lps25hGetData(&sensors->baro.pressure,
-                   &sensors->baro.temperature,
-                   &sensors->baro.asl);
-   #endif
+ if (!RATE_SKIP_100HZ(tick) && imuHasBarometer()) {
+#ifdef PLATFORM_CF1
+   ms5611GetData(&sensors->baro.pressure,
+                 &sensors->baro.temperature,
+                 &sensors->baro.asl);
+#else
+   lps25hGetData(&sensors->baro.pressure,
+                 &sensors->baro.temperature,
+                 &sensors->baro.asl);
+#endif
  }
  // Get the position
 
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 77e0bf9613..162bc0eb86 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -88,7 +88,7 @@ bool stabilizerTest(void)
  */
 static void stabilizerTask(void* param)
 {
-  uint32_t tick;
+  uint32_t tick = 0;
   uint32_t lastWakeTime;
   vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);