From 880ba379f0330e03d7aa5c25a68b96f8afeb7a8a Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Mon, 4 Nov 2019 18:14:22 +0100 Subject: [PATCH] #495 Added deinit functionality to estimators to allow the kalman filter task to sleep when not used --- src/modules/interface/estimator.h | 1 + src/modules/interface/estimator_kalman.h | 1 + src/modules/src/estimator.c | 42 +++++++++++++++++------- src/modules/src/estimator_kalman.c | 21 ++++++++++++ src/modules/src/stabilizer.c | 4 +-- 5 files changed, 56 insertions(+), 13 deletions(-) diff --git a/src/modules/interface/estimator.h b/src/modules/interface/estimator.h index 9ba238f3b3..e065bf88e4 100644 --- a/src/modules/interface/estimator.h +++ b/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(); diff --git a/src/modules/interface/estimator_kalman.h b/src/modules/interface/estimator_kalman.h index 039371334f..a8573523a0 100644 --- a/src/modules/interface/estimator_kalman.h +++ b/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); diff --git a/src/modules/src/estimator.c b/src/modules/src/estimator.c index dbcc8e9a6a..a1d799731b 100644 --- a/src/modules/src/estimator.c +++ b/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) { diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index 7a3df9c25c..b961b15341 100644 --- a/src/modules/src/estimator_kalman.c +++ b/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) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index e8e9dcb8c7..f54e38d831 100644 --- a/src/modules/src/stabilizer.c +++ b/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();