#495 Initialize kalman estimator correctly

bitcraze · Nov 3, 2019 · 821e249 · 821e249
1 parent 173ca50
commit 821e249
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 32 deletions.
diff --git a/src/modules/interface/estimator_kalman.h b/src/modules/interface/estimator_kalman.h
@@ -62,6 +62,9 @@ bool estimatorKalmanTest(void);
 void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
 
 
+void estimatorKalmanTaskInit();
+bool estimatorKalmanTaskTest();
+
 /**
  * The filter supports the incorporation of additional sensors into the state estimate via the following functions:
  */

diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c
@@ -161,7 +161,7 @@ static inline bool stateEstimatorHasYawErrorPacket(float *error) {
 static SemaphoreHandle_t measurementQueueSemaphore;
 
 // Mutex to protect data that is shared between the task and
-// function called byt he stabilizer loop
+// functions called by the stabilizer loop
 static SemaphoreHandle_t dataMutex;
 
 
@@ -254,13 +254,38 @@ static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_mat
 static inline float arm_sqrt(float32_t in)
   { float pOut = 0; arm_status result = arm_sqrt_f32(in, &pOut); configASSERT(ARM_MATH_SUCCESS == result); return pOut; }
 
+static void kalmanTask(void* parameters);
 static bool predictStateForward(uint32_t osTick, float dt);
 static bool updateQueuedMeasurments(const Axis3f *gyro);
 
 
 // --------------------------------------------------
 
-void kalmanTask(void* parameters) {
+void estimatorKalmanTaskInit() {
+  distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
+  posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
+  poseDataQueue = xQueueCreate(POSE_QUEUE_LENGTH, sizeof(poseMeasurement_t));
+  tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
+  flowDataQueue = xQueueCreate(FLOW_QUEUE_LENGTH, sizeof(flowMeasurement_t));
+  tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
+  heightDataQueue = xQueueCreate(HEIGHT_QUEUE_LENGTH, sizeof(heightMeasurement_t));
+  yawErrorDataQueue = xQueueCreate(YAW_ERROR_QUEUE_LENGTH, sizeof(float));
+
+  vSemaphoreCreateBinary(measurementQueueSemaphore);
+  xSemaphoreGive(measurementQueueSemaphore);
+
+  dataMutex = xSemaphoreCreateMutex();
+
+  xTaskCreate(kalmanTask, KALMAN_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL, KALMAN_TASK_PRI, NULL);
+
+  isInit = true;
+}
+
+bool estimatorKalmanTaskTest() {
+  return isInit;
+}
+
+static void kalmanTask(void* parameters) {
   systemWaitStart();
 
   uint32_t lastPrediction = xTaskGetTickCount();
@@ -539,27 +564,14 @@ static bool updateQueuedMeasurments(const Axis3f *gyro) {
 }
 
 void estimatorKalmanInit(void) {
-  if (!isInit)
-  {
-    distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
-    posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
-    poseDataQueue = xQueueCreate(POSE_QUEUE_LENGTH, sizeof(poseMeasurement_t));
-    tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
-    flowDataQueue = xQueueCreate(FLOW_QUEUE_LENGTH, sizeof(flowMeasurement_t));
-    tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
-    heightDataQueue = xQueueCreate(HEIGHT_QUEUE_LENGTH, sizeof(heightMeasurement_t));
-    yawErrorDataQueue = xQueueCreate(YAW_ERROR_QUEUE_LENGTH, sizeof(float));
-  }
-  else
-  {
-    xQueueReset(distDataQueue);
-    xQueueReset(posDataQueue);
-    xQueueReset(poseDataQueue);
-    xQueueReset(tdoaDataQueue);
-    xQueueReset(flowDataQueue);
-    xQueueReset(tofDataQueue);
-  }
+  xQueueReset(distDataQueue);
+  xQueueReset(posDataQueue);
+  xQueueReset(poseDataQueue);
+  xQueueReset(tdoaDataQueue);
+  xQueueReset(flowDataQueue);
+  xQueueReset(tofDataQueue);
 
+  xSemaphoreTake(dataMutex, portMAX_DELAY);
   accAccumulator = (Axis3f){.axis={0}};
   gyroAccumulator = (Axis3f){.axis={0}};
   thrustAccumulator = 0;
@@ -569,26 +581,17 @@ void estimatorKalmanInit(void) {
   gyroAccumulatorCount = 0;
   thrustAccumulatorCount = 0;
   baroAccumulatorCount = 0;
+  xSemaphoreGive(dataMutex);
 
   kalmanCoreInit(&coreData);
 
-  vSemaphoreCreateBinary(measurementQueueSemaphore);
-  xSemaphoreGive(measurementQueueSemaphore);
-
-  dataMutex = xSemaphoreCreateMutex();
-
   const uint32_t now_ms = T2M(xTaskGetTickCount());
   statsCntReset(&statsUpdates, now_ms);
   statsCntReset(&statsPredictions, now_ms);
   statsCntReset(&statsBaroUpdates, now_ms);
   statsCntReset(&statsFinalize, now_ms);
   statsCntReset(&statsUMeasurementAppended, now_ms);
   statsCntReset(&statsUMeasurementNotAppended, now_ms);
-
-  xTaskCreate(kalmanTask, KALMAN_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL,
-                    KALMAN_TASK_PRI, NULL);
-
-  isInit = true;
 }
 
 static bool appendMeasurement(xQueueHandle queue, void *measurement)

diff --git a/src/modules/src/system.c b/src/modules/src/system.c
@@ -61,6 +61,7 @@
 #include "buzzer.h"
 #include "sound.h"
 #include "sysload.h"
+#include "estimator_kalman.h"
 #include "deck.h"
 #include "extrx.h"
 #include "app.h"
@@ -166,6 +167,7 @@ void systemTask(void *arg)
   commanderInit();
 
   StateEstimatorType estimator = anyEstimator;
+  estimatorKalmanTaskInit();
   deckInit();
   estimator = deckGetRequiredEstimator();
   stabilizerInit(estimator);
@@ -186,6 +188,7 @@ void systemTask(void *arg)
   pass &= commTest();
   pass &= commanderTest();
   pass &= stabilizerTest();
+  pass &= estimatorKalmanTaskTest();
   pass &= deckTest();
   pass &= soundTest();
   pass &= memTest();