Add measurement update with elevation laser ranging in the EKF

src/deck/drivers/src/vl53l0x.c

@@ -35,9 +35,16 @@
 #include "log.h"
 
 #include "i2cdev.h"
-
 #include "vl53l0x.h"
 
+#include "stabilizer_types.h"
+#ifdef ESTIMATOR_TYPE_kalman
+#include "estimator_kalman.h"
+#include "arm_math.h"
+#endif
+
+//#define UPDATE_KALMAN_WITH_RANGING
+
 static uint8_t devAddr;
 static I2C_Dev *I2Cx;
 static bool isInit;
@@ -120,6 +127,7 @@ static bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val);
 /** Default constructor, uses default I2C address.
  * @see VL53L0X_DEFAULT_ADDRESS
  */
+
 void vl53l0xInit(DeckInfo* info)
 {
   if (isInit)
@@ -128,7 +136,6 @@ void vl53l0xInit(DeckInfo* info)
   i2cdevInit(I2C1_DEV);
   I2Cx = I2C1_DEV;
   devAddr = VL53L0X_DEFAULT_ADDRESS;
-
   xTaskCreate(vl53l0xTask, "vl53l0x", 2*configMINIMAL_STACK_SIZE, NULL, 3, NULL);
 
   isInit = true;
@@ -154,10 +161,21 @@ void vl53l0xTask(void* arg)
 
   vl53l0xSetVcselPulsePeriod(VcselPeriodPreRange, 18);
   vl53l0xSetVcselPulsePeriod(VcselPeriodFinalRange, 14);
-  vl53l0xStartContinuous(0);
+  vl53l0xStartContinuous(100);
   while (1) {
     xLastWakeTime = xTaskGetTickCount();
     range_last = vl53l0xReadRangeContinuousMillimeters();
+#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING)
+    // check if range is feasible and push into the kalman filter
+    float distance = (float)range_last;
+    if (distance < 2.0f){
+      tofMeasurement_t tofData;
+      tofData.timestamp = 100;
+      tofData.distance = distance;
+      tofData.stdDev = 0.0025;
+      stateEstimatorEnqueueTOF(&tofData);
+    }
+#endif
     vTaskDelayUntil(&xLastWakeTime, M2T(measurement_timing_budget_ms));
   }
 }
@@ -1105,8 +1123,8 @@ bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val)
 
 // TODO: Decide on vid:pid and set the used pins
 static const DeckDriver vl53l0x_deck = {
-  .vid = 0,
-  .pid = 0,
+  .vid = 0, // Changed this from 0
+  .pid = 0, // Changed this from 0
   .name = "vl53l0x",
   .usedGpio = 0,
 
@@ -1116,7 +1134,3 @@ static const DeckDriver vl53l0x_deck = {
 
 DECK_DRIVER(vl53l0x_deck);
 
-LOG_GROUP_START(range)
-LOG_ADD(LOG_UINT16, range, &range_last)
-LOG_GROUP_STOP(range)
-

src/modules/interface/estimator_kalman.h

@@ -67,3 +67,4 @@ bool stateEstimatorTest(void);
 bool stateEstimatorEnqueueTDOA(tdoaMeasurement_t *uwb);
 bool stateEstimatorEnqueuePosition(positionMeasurement_t *pos);
 bool stateEstimatorEnqueueDistance(distanceMeasurement_t *dist);
+bool stateEstimatorEnqueueTOF(tofMeasurement_t *tof);

src/modules/interface/stabilizer_types.h

@@ -173,6 +173,13 @@ typedef struct setpointZ_s {
   bool isUpdate; // True = small update of setpoint, false = completely new
 } setpointZ_t;
 
+/** TOF measurement**/
+typedef struct tofMeasurement_s {
+  uint32_t timestamp;
+  float distance;
+  float stdDev;
+} tofMeasurement_t;
+
 // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number.
 #define RATE_1000_HZ 1000
 #define RATE_500_HZ 500

src/modules/src/estimator_kalman.c

@@ -141,6 +141,15 @@ static inline bool stateEstimatorHasTDOAPacket(tdoaMeasurement_t *uwb) {
   return (pdTRUE == xQueueReceive(tdoaDataQueue, uwb, 0));
 }
 
+// Measurements of TOF from laser sensor
+static xQueueHandle tofDataQueue;
+#define TOF_QUEUE_LENGTH (10)
+
+static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof);
+
+static inline bool stateEstimatorHasTOFPacket(tofMeasurement_t *tof) {
+  return (pdTRUE == xQueueReceive(tofDataQueue, tof, 0));
+}
 
 /**
  * Constants used in the estimator
@@ -442,6 +451,12 @@ void stateEstimatorUpdate(state_t *state, sensorData_t *sensors, control_t *cont
     doneUpdate = true;
   }
 
+  tofMeasurement_t tof;
+  while (stateEstimatorHasTOFPacket(&tof))
+  {
+    stateEstimatorUpdateWithTof(&tof);
+    doneUpdate = true;
+  }
 
   /**
    * If an update has been made, the state is finalized:
@@ -909,6 +924,28 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa)
   tdoaCount++;
 }
 
+// TODO remove the temporary test variable (used for logging)
+static float testRange;
+
+static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof)
+{
+  // Updates the filter with a measured distance in the zb direction using the
+  float h[STATE_DIM] = {0};
+  arm_matrix_instance_f32 H = {1, STATE_DIM, h};
+
+  // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0)
+  if (fabs(R[2][2]) > 0.1){
+    float predictedDistance = S[STATE_Z] / R[2][2];
+    float measuredDistance = tof->distance / 1000.0; // scale from [mm] to [m]
+    testRange = measuredDistance;
+    //Measurement equation; h = z/((R*z_b)\dot z_b) = z/R[2][2]
+    h[STATE_Z] = 1 / R[2][2];
+
+    // Scalar update
+    stateEstimatorScalarUpdate(&H, measuredDistance-predictedDistance, tof->stdDev);
+  }
+}
+
 static void stateEstimatorFinalize(sensorData_t *sensors, uint32_t tick)
 {
   // Matrix to rotate the attitude covariances once updated
@@ -1093,12 +1130,14 @@ void stateEstimatorInit(void) {
     distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
     posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
     tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
+    tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
   }
   else
   {
     xQueueReset(distDataQueue);
     xQueueReset(posDataQueue);
     xQueueReset(tdoaDataQueue);
+    xQueueReset(tofDataQueue);
   }
 
   lastPrediction = xTaskGetTickCount();
@@ -1193,12 +1232,22 @@ bool stateEstimatorEnqueueDistance(distanceMeasurement_t *dist)
   return stateEstimatorEnqueueExternalMeasurement(distDataQueue, (void *)dist);
 }
 
+bool stateEstimatorEnqueueTOF(tofMeasurement_t *tof)
+{
+  // A distance (distance) [m] to the ground along the z_B axis.
+  return stateEstimatorEnqueueExternalMeasurement(tofDataQueue, (void *)tof);
+}
+
 bool stateEstimatorTest(void)
 {
   // TODO: Figure out what we could test?
   return isInit;
 }
 
+LOG_GROUP_START(tof)
+  LOG_ADD(LOG_FLOAT, range, &testRange)
+LOG_GROUP_STOP(tof)
+
 LOG_GROUP_START(kalman)
   LOG_ADD(LOG_UINT8, inFlight, &quadIsFlying)
   LOG_ADD(LOG_FLOAT, stateX, &S[STATE_X])