#461 Added infrastructure to feed Lighthouse angles into the kalman f…

…ilter

src/modules/interface/estimator.h

@@ -51,5 +51,6 @@ bool estimatorEnqueueTOF(const tofMeasurement_t *tof);
 bool estimatorEnqueueAbsoluteHeight(const heightMeasurement_t *height);
 bool estimatorEnqueueFlow(const flowMeasurement_t *flow);
 bool estimatorEnqueueYawError(const yawErrorMeasurement_t *error);
+bool estimatorEnqueueSweepAngles(const sweepAngleMeasurement_t *angles);
 
 #endif //__ESTIMATOR_H__

src/modules/interface/estimator_kalman.h

@@ -76,6 +76,7 @@ bool estimatorKalmanEnqueueTOF(const tofMeasurement_t *tof);
 bool estimatorKalmanEnqueueAbsoluteHeight(const heightMeasurement_t *height);
 bool estimatorKalmanEnqueueFlow(const flowMeasurement_t *flow);
 bool estimatorKalmanEnqueueYawError(const yawErrorMeasurement_t* error);
+bool estimatorKalmanEnqueueSweepAngles(const sweepAngleMeasurement_t *angles);
 
 void estimatorKalmanGetEstimatedPos(point_t* pos);
 

src/modules/interface/kalman_core.h

@@ -128,6 +128,9 @@ void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof);
 // Measurement of yaw error (outside measurement Vs current estimation)
 void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t *error);
 
+// Measurement of sweep angles from a Lighthouse base station
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles);
+
 /**
  * Primary Kalman filter functions
  *

src/modules/interface/stabilizer_types.h

@@ -29,6 +29,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "imu_types.h"
+#include "lighthouse_geometry.h"
 
 /* Data structure used by the stabilizer subsystem.
  * All have a timestamp to be set when the data is calculated.
@@ -239,6 +240,16 @@ typedef struct {
   float stdDev;
 } yawErrorMeasurement_t;
 
+/** Sweep angle measurement */
+typedef struct {
+  uint32_t timestamp;
+  baseStationGeometry_t geometry;
+  float angleX;
+  float angleY;
+  float stdDevX;
+  float stdDevY;
+} sweepAngleMeasurement_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.c

@@ -26,55 +26,59 @@ typedef struct {
   bool (*estimatorEnqueueAbsoluteHeight)(const heightMeasurement_t *height);
   bool (*estimatorEnqueueFlow)(const flowMeasurement_t *flow);
   bool (*estimatorEnqueueYawError)(const yawErrorMeasurement_t *error);
+  bool (*estimatorEnqueueSweepAngles)(const sweepAngleMeasurement_t *angles);
 } EstimatorFcns;
 
 #define NOT_IMPLEMENTED ((void*)0)
 
 static EstimatorFcns estimatorFunctions[] = {
-  {
-    .init = NOT_IMPLEMENTED,
-    .deinit = NOT_IMPLEMENTED,
-    .test = NOT_IMPLEMENTED,
-    .update = NOT_IMPLEMENTED,
-    .name = "None",
-    .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
-    .estimatorEnqueuePosition = NOT_IMPLEMENTED,
-    .estimatorEnqueuePose = NOT_IMPLEMENTED,
-    .estimatorEnqueueDistance = NOT_IMPLEMENTED,
-    .estimatorEnqueueTOF = NOT_IMPLEMENTED,
-    .estimatorEnqueueAbsoluteHeight = NOT_IMPLEMENTED,
-    .estimatorEnqueueFlow = NOT_IMPLEMENTED,
-    .estimatorEnqueueYawError = NOT_IMPLEMENTED,
-  }, // Any estimator
-  {
-    .init = estimatorComplementaryInit,
-    .deinit = NOT_IMPLEMENTED,
-    .test = estimatorComplementaryTest,
-    .update = estimatorComplementary,
-    .name = "Complementary",
-    .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
-    .estimatorEnqueuePosition = NOT_IMPLEMENTED,
-    .estimatorEnqueuePose = NOT_IMPLEMENTED,
-    .estimatorEnqueueDistance = NOT_IMPLEMENTED,
-    .estimatorEnqueueTOF = estimatorComplementaryEnqueueTOF,
-    .estimatorEnqueueAbsoluteHeight = NOT_IMPLEMENTED,
-    .estimatorEnqueueFlow = NOT_IMPLEMENTED,
-    .estimatorEnqueueYawError = NOT_IMPLEMENTED,
-  },
-  {
-    .init = estimatorKalmanInit,
-    .deinit = NOT_IMPLEMENTED,
-    .test = estimatorKalmanTest,
-    .update = estimatorKalman,
-    .name = "Kalman",
-    .estimatorEnqueueTDOA = estimatorKalmanEnqueueTDOA,
-    .estimatorEnqueuePosition = estimatorKalmanEnqueuePosition,
-    .estimatorEnqueuePose = estimatorKalmanEnqueuePose,
-    .estimatorEnqueueDistance = estimatorKalmanEnqueueDistance,
-    .estimatorEnqueueTOF = estimatorKalmanEnqueueTOF,
-    .estimatorEnqueueAbsoluteHeight = estimatorKalmanEnqueueAbsoluteHeight,
-    .estimatorEnqueueFlow = estimatorKalmanEnqueueFlow,
-    .estimatorEnqueueYawError = estimatorKalmanEnqueueYawError,
+    {
+        .init = NOT_IMPLEMENTED,
+        .deinit = NOT_IMPLEMENTED,
+        .test = NOT_IMPLEMENTED,
+        .update = NOT_IMPLEMENTED,
+        .name = "None",
+        .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
+        .estimatorEnqueuePosition = NOT_IMPLEMENTED,
+        .estimatorEnqueuePose = NOT_IMPLEMENTED,
+        .estimatorEnqueueDistance = NOT_IMPLEMENTED,
+        .estimatorEnqueueTOF = NOT_IMPLEMENTED,
+        .estimatorEnqueueAbsoluteHeight = NOT_IMPLEMENTED,
+        .estimatorEnqueueFlow = NOT_IMPLEMENTED,
+        .estimatorEnqueueYawError = NOT_IMPLEMENTED,
+        .estimatorEnqueueSweepAngles = NOT_IMPLEMENTED,
+    }, // Any estimator
+    {
+        .init = estimatorComplementaryInit,
+        .deinit = NOT_IMPLEMENTED,
+        .test = estimatorComplementaryTest,
+        .update = estimatorComplementary,
+        .name = "Complementary",
+        .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
+        .estimatorEnqueuePosition = NOT_IMPLEMENTED,
+        .estimatorEnqueuePose = NOT_IMPLEMENTED,
+        .estimatorEnqueueDistance = NOT_IMPLEMENTED,
+        .estimatorEnqueueTOF = estimatorComplementaryEnqueueTOF,
+        .estimatorEnqueueAbsoluteHeight = NOT_IMPLEMENTED,
+        .estimatorEnqueueFlow = NOT_IMPLEMENTED,
+        .estimatorEnqueueYawError = NOT_IMPLEMENTED,
+        .estimatorEnqueueSweepAngles = NOT_IMPLEMENTED,
+    },
+    {
+        .init = estimatorKalmanInit,
+        .deinit = NOT_IMPLEMENTED,
+        .test = estimatorKalmanTest,
+        .update = estimatorKalman,
+        .name = "Kalman",
+        .estimatorEnqueueTDOA = estimatorKalmanEnqueueTDOA,
+        .estimatorEnqueuePosition = estimatorKalmanEnqueuePosition,
+        .estimatorEnqueuePose = estimatorKalmanEnqueuePose,
+        .estimatorEnqueueDistance = estimatorKalmanEnqueueDistance,
+        .estimatorEnqueueTOF = estimatorKalmanEnqueueTOF,
+        .estimatorEnqueueAbsoluteHeight = estimatorKalmanEnqueueAbsoluteHeight,
+        .estimatorEnqueueFlow = estimatorKalmanEnqueueFlow,
+        .estimatorEnqueueYawError = estimatorKalmanEnqueueYawError,
+        .estimatorEnqueueSweepAngles = estimatorKalmanEnqueueSweepAngles,
     },
 };
 
@@ -199,3 +203,11 @@ bool estimatorEnqueueFlow(const flowMeasurement_t *flow) {
 
   return false;
 }
+
+bool estimatorEnqueueSweepAngles(const sweepAngleMeasurement_t *angles) {
+  if (estimatorFunctions[currentEstimator].estimatorEnqueueSweepAngles) {
+    return estimatorFunctions[currentEstimator].estimatorEnqueueSweepAngles(angles);
+  }
+
+  return false;
+}

src/modules/src/estimator_kalman.c

@@ -158,6 +158,14 @@ static inline bool stateEstimatorHasYawErrorPacket(yawErrorMeasurement_t *error)
   return (pdTRUE == xQueueReceive(yawErrorDataQueue, error, 0));
 }
 
+static xQueueHandle sweepAnglesDataQueue;
+#define SWEEP_ANGLES_QUEUE_LENGTH (10)
+
+static inline bool stateEstimatorHasSweepAnglesPacket(sweepAngleMeasurement_t *angles)
+{
+  return (pdTRUE == xQueueReceive(sweepAnglesDataQueue, angles, 0));
+}
+
 // Semaphore to signal that we got data from the stabilzer loop to process
 static SemaphoreHandle_t runTaskSemaphore;
 
@@ -273,6 +281,7 @@ void estimatorKalmanTaskInit() {
   tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
   heightDataQueue = xQueueCreate(HEIGHT_QUEUE_LENGTH, sizeof(heightMeasurement_t));
   yawErrorDataQueue = xQueueCreate(YAW_ERROR_QUEUE_LENGTH, sizeof(yawErrorMeasurement_t));
+  sweepAnglesDataQueue = xQueueCreate(SWEEP_ANGLES_QUEUE_LENGTH, sizeof(sweepAngleMeasurement_t));
 
   vSemaphoreCreateBinary(runTaskSemaphore);
 
@@ -578,6 +587,13 @@ static bool updateQueuedMeasurments(const Axis3f *gyro) {
     doneUpdate = true;
   }
 
+  sweepAngleMeasurement_t angles;
+  while (stateEstimatorHasSweepAnglesPacket(&angles))
+  {
+    kalmanCoreUpdateWithSweepAngles(&coreData, &angles);
+    doneUpdate = true;
+  }
+
   return doneUpdate;
 }
 
@@ -681,6 +697,12 @@ bool estimatorKalmanEnqueueYawError(const yawErrorMeasurement_t* error)
   return appendMeasurement(yawErrorDataQueue, (void *)error);
 }
 
+bool estimatorKalmanEnqueueSweepAngles(const sweepAngleMeasurement_t *angles)
+{
+  ASSERT(isInit);
+  return appendMeasurement(sweepAnglesDataQueue, (void *)&angles);
+}
+
 bool estimatorKalmanTest(void)
 {
   return isInit;

src/modules/src/kalman_core.c

@@ -565,6 +565,11 @@ void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t
     scalarUpdate(this, &H, this->S[KC_STATE_D2] - error->yawError, error->stdDev);
 }
 
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles)
+{
+  // TODO krri
+}
+
 void kalmanCorePredict(kalmanCoreData_t* this, float cmdThrust, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying)
 {
   /* Here we discretize (euler forward) and linearise the quadrocopter dynamics in order