From ba7b1973185c6826e71b63036ff37c526243ae97 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 5 Nov 2019 12:50:25 +0100
Subject: [PATCH] #495 Synchronize task execution to the stabilizer loop

---
 src/modules/interface/estimator_kalman.h |  1 -
 src/modules/src/estimator.c              |  2 +-
 src/modules/src/estimator_kalman.c       | 44 +++++++-----------------
 3 files changed, 14 insertions(+), 33 deletions(-)

diff --git a/src/modules/interface/estimator_kalman.h b/src/modules/interface/estimator_kalman.h
index a8573523a0..039371334f 100644
--- a/src/modules/interface/estimator_kalman.h
+++ b/src/modules/interface/estimator_kalman.h
@@ -58,7 +58,6 @@
 #include "stabilizer_types.h"
 
 void estimatorKalmanInit(void);
-void estimatorKalmanDeinit();
 bool estimatorKalmanTest(void);
 void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
 
diff --git a/src/modules/src/estimator.c b/src/modules/src/estimator.c
index a1d799731b..68d84a570b 100644
--- a/src/modules/src/estimator.c
+++ b/src/modules/src/estimator.c
@@ -63,7 +63,7 @@ static EstimatorFcns estimatorFunctions[] = {
   },
   {
     .init = estimatorKalmanInit,
-    .deinit = estimatorKalmanDeinit,
+    .deinit = NOT_IMPLEMENTED,
     .test = estimatorKalmanTest,
     .update = estimatorKalman,
     .name = "Kalman",
diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c
index b961b15341..8905667aa6 100644
--- a/src/modules/src/estimator_kalman.c
+++ b/src/modules/src/estimator_kalman.c
@@ -157,8 +157,8 @@ static inline bool stateEstimatorHasYawErrorPacket(float *error) {
   return (pdTRUE == xQueueReceive(yawErrorDataQueue, error, 0));
 }
 
-// Semaphore to signal that measurement data has been queued
-static SemaphoreHandle_t measurementQueueSemaphore;
+// Semaphore to signal that we got data from the stabilzer loop to process
+static SemaphoreHandle_t runTaskSemaphore;
 
 // Mutex to protect data that is shared between the task and
 // functions called by the stabilizer loop
@@ -209,7 +209,6 @@ static kalmanCoreData_t coreData;
  */
 
 static bool isInit = false;
-static volatile bool isEstimatorActive = false;
 
 static Axis3f accAccumulator;
 static float thrustAccumulator;
@@ -236,7 +235,6 @@ static statsCntRate_t statsFinalize;
 static statsCntRate_t statsUMeasurementAppended;
 static statsCntRate_t statsUMeasurementNotAppended;
 static void updateStats(uint32_t now_ms);
-static void blockWhileInactive();
 
 #ifdef KALMAN_USE_BARO_UPDATE
 static const bool useBaroUpdate = true;
@@ -275,8 +273,7 @@ void estimatorKalmanTaskInit() {
   heightDataQueue = xQueueCreate(HEIGHT_QUEUE_LENGTH, sizeof(heightMeasurement_t));
   yawErrorDataQueue = xQueueCreate(YAW_ERROR_QUEUE_LENGTH, sizeof(float));
 
-  vSemaphoreCreateBinary(measurementQueueSemaphore);
-  xSemaphoreGive(measurementQueueSemaphore);
+  vSemaphoreCreateBinary(runTaskSemaphore);
 
   dataMutex = xSemaphoreCreateMutex();
 
@@ -288,7 +285,6 @@ void estimatorKalmanTaskInit() {
   statsCntReset(&statsUMeasurementAppended, now_ms);
   statsCntReset(&statsUMeasurementNotAppended, now_ms);
 
-  isEstimatorActive = false;
   xTaskCreate(kalmanTask, KALMAN_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL, KALMAN_TASK_PRI, NULL);
 
   isInit = true;
@@ -308,13 +304,7 @@ static void kalmanTask(void* parameters) {
   uint32_t nextStatsUpdate = 0;
 
   while (true) {
-    blockWhileInactive();
-
-    // Wake up when it is time for next prediction, unless we get queued meassurements
-    uint32_t maxSleepTics = nextPrediction - xTaskGetTickCount();
-    if (maxSleepTics > 0) {
-      xSemaphoreTake(measurementQueueSemaphore, maxSleepTics);
-    }
+    xSemaphoreTake(runTaskSemaphore, portMAX_DELAY);
 
     // If the client triggers an estimator reset via parameter update
     if (coreData.resetEstimation) {
@@ -346,8 +336,13 @@ static void kalmanTask(void* parameters) {
     /**
      * Add process noise every loop, rather than every prediction
      */
-    kalmanCoreAddProcessNoise(&coreData, T2S(osTick - lastPNUpdate));
-    lastPNUpdate = osTick;
+    {
+      float dt = T2S(osTick - lastPNUpdate);
+      if (dt > 0.0f) {
+        kalmanCoreAddProcessNoise(&coreData, dt);
+        lastPNUpdate = osTick;
+      }
+    }
 
     /**
      * Update the state estimate with the barometer measurements
@@ -414,12 +409,6 @@ static void kalmanTask(void* parameters) {
   }
 }
 
-static void blockWhileInactive() {
-  while(! isEstimatorActive) {
-    vTaskDelay(M2T(300));
-  }
-}
-
 static void updateStats(uint32_t now_ms) {
   statsCntRate(&statsUpdates, now_ms);
   statsCntRate(&statsPredictions, now_ms);
@@ -471,6 +460,8 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
   // Copy the latest state, calculated by the task
   memcpy(state, &taskEstimatorState, sizeof(state_t));
   xSemaphoreGive(dataMutex);
+
+  xSemaphoreGive(runTaskSemaphore);
 }
 
 static bool predictStateForward(uint32_t osTick, float dt) {
@@ -611,13 +602,6 @@ void estimatorKalmanInit(void) {
   xSemaphoreGive(dataMutex);
 
   kalmanCoreInit(&coreData);
-
-  isEstimatorActive = true;
-}
-
-// Called when another estimator is activated, and this estimator no longer is the active estimator
-void estimatorKalmanDeinit(void) {
-  isEstimatorActive = false;
 }
 
 static bool appendMeasurement(xQueueHandle queue, void *measurement)
@@ -628,14 +612,12 @@ static bool appendMeasurement(xQueueHandle queue, void *measurement)
   if (isInInterrupt) {
     portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
     result = xQueueSendFromISR(queue, measurement, &xHigherPriorityTaskWoken);
-    xSemaphoreGiveFromISR(measurementQueueSemaphore, &xHigherPriorityTaskWoken);
     if(xHigherPriorityTaskWoken == pdTRUE)
     {
       portYIELD();
     }
   } else {
     result = xQueueSend(queue, measurement, 0);
-    xSemaphoreGive(measurementQueueSemaphore);
   }
 
   if (result == pdTRUE) {