#211 Added inital support for z-ranger in complementary filter

src/deck/drivers/interface/vl53l0x.h

@@ -27,6 +27,10 @@
 #ifndef _VL53L0X_H_
 #define _VL53L0X_H_
 
+#include "stabilizer_types.h"
+#include "deck_core.h"
+
+
 #define VL53L0X_DEFAULT_ADDRESS 0b0101001
 
 #define VL53L0X_RA_SYSRANGE_START                              0x00
@@ -142,6 +146,8 @@ void vl53l0xInit(DeckInfo* info);
 bool vl53l0xTest(void);
 void vl53l0xTask(void* arg);
 
+bool vl53l0xReadRange(zDistance_t* zrange, const uint32_t tick);
+
 /** Verify the I2C connection.
  * Make sure the device is connected and responds as expected.
  * @return True if connection is valid, false otherwise

src/deck/drivers/src/vl53l0x.c

@@ -45,7 +45,6 @@
 
 //#define UPDATE_KALMAN_WITH_RANGING // uncomment to push into the kalman
 #ifdef UPDATE_KALMAN_WITH_RANGING
-#define RANGE_OUTLIER_LIMIT 1500 // the measured range is in [mm]
 // Measurement noise model
 static float expPointA = 1.0f;
 static float expStdA = 0.0025f; // STD at elevation expPointA [m]
@@ -55,6 +54,8 @@ static float expCoeff;
 #endif // UPDATE_KALMAN_WITH_RANGING
 #endif // ESTIMATOR_TYPE_kalman
 
+#define RANGE_OUTLIER_LIMIT 3000 // the measured range is in [mm]
+
 static uint8_t devAddr;
 static I2C_Dev *I2Cx;
 static bool isInit;
@@ -63,7 +64,7 @@ static uint16_t io_timeout = 0;
 static bool did_timeout;
 static uint16_t timeout_start_ms;
 
-static uint16_t range_last = 0;
+uint16_t range_last = 0;
 
 // Record the current time to check an upcoming timeout against
 #define startTimeout() (timeout_start_ms = xTaskGetTickCount())
@@ -198,6 +199,20 @@ void vl53l0xTask(void* arg)
   }
 }
 
+bool vl53l0xReadRange(zDistance_t* zrange, const uint32_t tick)
+{
+  bool updated = false;
+
+  if (isInit) {
+    if (range_last != 0 && range_last < RANGE_OUTLIER_LIMIT) {
+      zrange->distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
+      zrange->timestamp = tick;
+      updated = true;
+    }
+  }
+  return updated;
+}
+
 /** Verify the I2C connection.
  * Make sure the device is connected and responds as expected.
  * @return True if connection is valid, false otherwise

src/hal/src/sensors_cf2.c

@@ -38,6 +38,7 @@
 #include "lps25h.h"
 #include "mpu6500.h"
 #include "ak8963.h"
+#include "vl53l0x.h"
 
 #include "FreeRTOS.h"
 #include "semphr.h"
@@ -188,6 +189,7 @@ void sensorsAcquire(sensorData_t *sensors, const uint32_t tick)
   sensorsReadAcc(&sensors->acc);
   sensorsReadMag(&sensors->mag);
   sensorsReadBaro(&sensors->baro);
+  vl53l0xReadRange(&sensors->zrange, tick);
 }
 
 bool sensorsAreCalibrated() {

src/modules/interface/position_estimator.h

@@ -28,7 +28,7 @@
 
 #include "stabilizer_types.h"
 
-void positionEstimate(state_t* estimate, float asl, float dt);
+void positionEstimate(state_t* estimate, const sensorData_t* sensorData, float dt, uint32_t tick);
 void positionUpdateVelocity(float accWZ, float dt);
 
 #endif /* POSITION_ESTIMATOR_H_ */

src/modules/interface/stabilizer_types.h

@@ -113,11 +113,17 @@ typedef struct distanceMeasurement_s {
   float stdDev;
 } distanceMeasurement_t;
 
+typedef struct zDistance_s {
+  uint32_t timestamp;
+  float distance;
+} zDistance_t;
+
 typedef struct sensorData_s {
   Axis3f acc;
   Axis3f gyro;
   Axis3f mag;
   baro_t baro;
+  zDistance_t zrange;
   point_t position;
 } sensorData_t;
 

src/modules/src/estimator_complementary.c

@@ -46,7 +46,7 @@ void stateEstimator(state_t *state, const sensorData_t *sensorData, const uint32
     if (sensorData->position.timestamp) {
       state->position = sensorData->position;
     } else {
-      positionEstimate(state, sensorData->baro.asl, POS_UPDATE_DT);
+      positionEstimate(state, sensorData, POS_UPDATE_DT, tick);
     }
   }
 }

src/modules/src/position_estimator_altitude.c

@@ -34,7 +34,8 @@
 struct selfState_s {
   float estimatedZ; // The current Z estimate, has same offset as asl
   float velocityZ; // Vertical speed (world frame) integrated from vertical acceleration (m/s)
-  float estAlpha;
+  float estAlphaZrange;
+  float estAlphaAsl;
   float velocityFactor;
   float vAccDeadband; // Vertical acceleration deadband
   float velZAlpha;   // Blending factor to avoid vertical speed to accumulate error
@@ -44,36 +45,58 @@ struct selfState_s {
 static struct selfState_s state = {
   .estimatedZ = 0.0f,
   .velocityZ = 0.0f,
-  .estAlpha = 0.997f,
+  .estAlphaZrange = 0.93f,
+  .estAlphaAsl = 0.997f,
   .velocityFactor = 1.0f,
   .vAccDeadband = 0.04f,
   .velZAlpha = 0.995f,
   .estimatedVZ = 0.0f,
 };
 
-static void positionEstimateInternal(state_t* estimate, float asl, float dt, struct selfState_s* state);
+static void positionEstimateInternal(state_t* estimate, const sensorData_t* sensorData, float dt, uint32_t tick, struct selfState_s* state);
 static void positionUpdateVelocityInternal(float accWZ, float dt, struct selfState_s* state);
 
-void positionEstimate(state_t* estimate, float asl, float dt) {
-  positionEstimateInternal(estimate, asl, dt, &state);
+void positionEstimate(state_t* estimate, const sensorData_t* sensorData, float dt, uint32_t tick) {
+  positionEstimateInternal(estimate, sensorData, dt, tick, &state);
 }
 
 void positionUpdateVelocity(float accWZ, float dt) {
   positionUpdateVelocityInternal(accWZ, dt, &state);
 }
 
-static void positionEstimateInternal(state_t* estimate, float asl, float dt, struct selfState_s* state) {
+static void positionEstimateInternal(state_t* estimate, const sensorData_t* sensorData, float dt, uint32_t tick, struct selfState_s* state) {
+  float filteredZ;
   static float prev_estimatedZ = 0;
-  state->estimatedZ = state->estAlpha * state->estimatedZ +
-                     (1.0f - state->estAlpha) * asl +
-                     state->velocityFactor * state->velocityZ * dt;
+  static bool surfaceFollowingMode = false;
+
+  if (sensorData->zrange.timestamp == tick) {
+    surfaceFollowingMode = true;
+  }
+
+  if (surfaceFollowingMode) {
+    if (sensorData->zrange.timestamp == tick) {
+      // IIR filter zrange
+      filteredZ = (state->estAlphaZrange       ) * state->estimatedZ +
+                  (1.0f - state->estAlphaZrange) * sensorData->zrange.distance;
+      // Use zrange as base and add velocity changes.
+      state->estimatedZ = filteredZ + (state->velocityFactor * state->velocityZ * dt);
+    }
+  } else {
+    // IIR filter asl
+    filteredZ = (state->estAlphaAsl       ) * state->estimatedZ +
+                (1.0f - state->estAlphaAsl) * sensorData->baro.asl;
+    // Use asl as base and add velocity changes.
+    state->estimatedZ = filteredZ + (state->velocityFactor * state->velocityZ * dt);
+  }
+
 
   estimate->position.x = 0.0f;
   estimate->position.y = 0.0f;
   estimate->position.z = state->estimatedZ;
   estimate->velocity.z = (state->estimatedZ - prev_estimatedZ) / dt;
   state->estimatedVZ = estimate->velocity.z;
   prev_estimatedZ = state->estimatedZ;
+
 }
 
 static void positionUpdateVelocityInternal(float accWZ, float dt, struct selfState_s* state) {
@@ -88,7 +111,8 @@ LOG_ADD(LOG_FLOAT, velocityZ, &state.velocityZ)
 LOG_GROUP_STOP(posEstimatorAlt)
 
 PARAM_GROUP_START(posEst)
-PARAM_ADD(PARAM_FLOAT, estAlpha, &state.estAlpha)
+PARAM_ADD(PARAM_FLOAT, estAlphaAsl, &state.estAlphaAsl)
+PARAM_ADD(PARAM_FLOAT, estAlphaZr, &state.estAlphaZrange)
 PARAM_ADD(PARAM_FLOAT, velFactor, &state.velocityFactor)
 PARAM_ADD(PARAM_FLOAT, velZAlpha, &state.velZAlpha)
 PARAM_ADD(PARAM_FLOAT, vAccDeadband, &state.vAccDeadband)