#495 Added deinit functionality to estimators to allow the kalman fil…

…ter task to sleep when not used

src/modules/interface/estimator.h

@@ -37,6 +37,7 @@ typedef enum {
 
 void stateEstimatorInit(StateEstimatorType estimator);
 bool stateEstimatorTest(void);
+void stateEstimatorSwitchTo(StateEstimatorType estimator);
 void stateEstimator(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
 StateEstimatorType getStateEstimator(void);
 const char* stateEstimatorGetName();

src/modules/interface/estimator_kalman.h

@@ -58,6 +58,7 @@
 #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);
 

src/modules/src/estimator.c

@@ -9,10 +9,12 @@
 #define DEFAULT_ESTIMATOR complementaryEstimator
 static StateEstimatorType currentEstimator = anyEstimator;
 
-static void initEstimator();
+static void initEstimator(const StateEstimatorType estimator);
+static void deinitEstimator(const StateEstimatorType estimator);
 
 typedef struct {
   void (*init)(void);
+  void (*deinit)(void);
   bool (*test)(void);
   void (*update)(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);
   const char* name;
@@ -30,9 +32,10 @@ typedef struct {
 
 static EstimatorFcns estimatorFunctions[] = {
   {
-    .init = 0,
-    .test = 0,
-    .update = 0,
+    .init = NOT_IMPLEMENTED,
+    .deinit = NOT_IMPLEMENTED,
+    .test = NOT_IMPLEMENTED,
+    .update = NOT_IMPLEMENTED,
     .name = "None",
     .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
     .estimatorEnqueuePosition = NOT_IMPLEMENTED,
@@ -45,6 +48,7 @@ static EstimatorFcns estimatorFunctions[] = {
   }, // Any estimator
   {
     .init = estimatorComplementaryInit,
+    .deinit = NOT_IMPLEMENTED,
     .test = estimatorComplementaryTest,
     .update = estimatorComplementary,
     .name = "Complementary",
@@ -59,6 +63,7 @@ static EstimatorFcns estimatorFunctions[] = {
   },
   {
     .init = estimatorKalmanInit,
+    .deinit = estimatorKalmanDeinit,
     .test = estimatorKalmanTest,
     .update = estimatorKalman,
     .name = "Kalman",
@@ -75,23 +80,30 @@ static EstimatorFcns estimatorFunctions[] = {
 
 
 void stateEstimatorInit(StateEstimatorType estimator) {
+  stateEstimatorSwitchTo(estimator);
+}
+
+void stateEstimatorSwitchTo(StateEstimatorType estimator) {
   if (estimator < 0 || estimator >= StateEstimatorTypeCount) {
     return;
   }
 
-  currentEstimator = estimator;
+  StateEstimatorType newEstimator = estimator;
 
-  if (anyEstimator == currentEstimator) {
-    currentEstimator = DEFAULT_ESTIMATOR;
+  if (anyEstimator == newEstimator) {
+    newEstimator = DEFAULT_ESTIMATOR;
   }
 
   StateEstimatorType forcedEstimator = ESTIMATOR_NAME;
   if (forcedEstimator != anyEstimator) {
     DEBUG_PRINT("Estimator type forced\n");
-    currentEstimator = forcedEstimator;
+    newEstimator = forcedEstimator;
   }
 
-  initEstimator();
+  initEstimator(newEstimator);
+  StateEstimatorType previousEstimator = currentEstimator;
+  currentEstimator = newEstimator;
+  deinitEstimator(previousEstimator);
 
   DEBUG_PRINT("Using %s (%d) estimator\n", stateEstimatorGetName(), currentEstimator);
 }
@@ -100,8 +112,16 @@ StateEstimatorType getStateEstimator(void) {
   return currentEstimator;
 }
 
-static void initEstimator() {
-  estimatorFunctions[currentEstimator].init();
+static void initEstimator(const StateEstimatorType estimator) {
+  if (estimatorFunctions[estimator].init) {
+    estimatorFunctions[estimator].init();
+  }
+}
+
+static void deinitEstimator(const StateEstimatorType estimator) {
+  if (estimatorFunctions[estimator].deinit) {
+    estimatorFunctions[estimator].deinit();
+  }
 }
 
 bool stateEstimatorTest(void) {

src/modules/src/estimator_kalman.c

@@ -209,6 +209,8 @@ static kalmanCoreData_t coreData;
  */
 
 static bool isInit = false;
+static volatile bool isEstimatorActive = false;
+
 static Axis3f accAccumulator;
 static float thrustAccumulator;
 static Axis3f gyroAccumulator;
@@ -234,6 +236,7 @@ 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;
@@ -261,6 +264,7 @@ static bool updateQueuedMeasurments(const Axis3f *gyro);
 
 // --------------------------------------------------
 
+// Called one time during system startup
 void estimatorKalmanTaskInit() {
   distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
   posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
@@ -284,6 +288,7 @@ 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;
@@ -303,6 +308,8 @@ 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) {
@@ -407,6 +414,12 @@ 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);
@@ -576,6 +589,7 @@ static bool updateQueuedMeasurments(const Axis3f *gyro) {
   return doneUpdate;
 }
 
+// Called when this estimator is activated
 void estimatorKalmanInit(void) {
   xQueueReset(distDataQueue);
   xQueueReset(posDataQueue);
@@ -597,6 +611,13 @@ 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)

src/modules/src/stabilizer.c

@@ -258,7 +258,7 @@ static void stabilizerTask(void* param)
     } else {
       // allow to update estimator dynamically
       if (getStateEstimator() != estimatorType) {
-        stateEstimatorInit(estimatorType);
+        stateEstimatorSwitchTo(estimatorType);
         estimatorType = getStateEstimator();
       }
       // allow to update controller dynamically
@@ -269,7 +269,7 @@ static void stabilizerTask(void* param)
 
       stateEstimator(&state, &sensorData, &control, tick);
       compressState();
-      
+
       commanderGetSetpoint(&setpoint, &state);
       compressSetpoint();