#110 Added params to externally set the Crazyflie position

This is experimental and intended to test integrated controller
bitcraze · May 2, 2016 · db55d3b · db55d3b
1 parent e076413
commit db55d3b
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 6 deletions.
diff --git a/src/modules/src/estimator_complementary.c b/src/modules/src/estimator_complementary.c
@@ -41,6 +41,12 @@ void stateEstimator(state_t *state, const sensorData_t *sensorData, const uint32
   }
 
   if (!RATE_SKIP_100HZ(tick)) {
-    positionEstimate(state, sensorData->baro.asl, POS_UPDATE_DT);
+    // If position sensor data is preset, pass it throught
+    // FIXME: The position sensor shall be used as an input of the estimator
+    if (sensorData->position.timestamp) {
+      state->position = sensorData->position;
+    } else {
+      positionEstimate(state, sensorData->baro.asl, POS_UPDATE_DT);
+    }
   }
 }
diff --git a/src/modules/src/sensors_stock.c b/src/modules/src/sensors_stock.c
@@ -32,6 +32,10 @@
   #include "lps25h.h"
 #endif
 
+#include "param.h"
+
+static point_t position;
+
 void sensorsInit(void)
 {
  imu6Init();
@@ -52,18 +56,28 @@ bool sensorsAcquire(sensorData_t *sensors, const uint32_t tick)
     imu9Read(&sensors->gyro, &sensors->acc, &sensors->mag);
   }
 
- if (!RATE_SKIP_100HZ(tick) && imuHasBarometer()) {
+  if (!RATE_SKIP_100HZ(tick) && imuHasBarometer()) {
 #ifdef PLATFORM_CF1
-   ms5611GetData(&sensors->baro.pressure,
+    ms5611GetData(&sensors->baro.pressure,
                  &sensors->baro.temperature,
                  &sensors->baro.asl);
 #else
-   lps25hGetData(&sensors->baro.pressure,
+    lps25hGetData(&sensors->baro.pressure,
                  &sensors->baro.temperature,
                  &sensors->baro.asl);
 #endif
- }
- // Get the position
+    // Experimental: receive the position from parameters
+    if (position.timestamp) {
+      sensors->position = position;
+    }
+  }
 
  return imu6IsCalibrated();
 }
+
+PARAM_GROUP_START(lps)
+PARAM_ADD(PARAM_UINT32, t, &position.timestamp)
+PARAM_ADD(PARAM_FLOAT, x, &position.x)
+PARAM_ADD(PARAM_FLOAT, y, &position.y)
+PARAM_ADD(PARAM_FLOAT, z, &position.z)
+PARAM_GROUP_STOP(sensorfusion6)