Merge branch 'optimize/estimator_switch_optimize' into 'master'

choose estimator based on sensors onboard

See merge request ae_group/esp-college/esp-drone!25

components/core/crazyflie/modules/interface/estimator.h

@@ -35,6 +35,8 @@ typedef enum {
   StateEstimatorTypeCount,
 } StateEstimatorType;
 
+bool registerRequiredEstimator(StateEstimatorType estimator);
+StateEstimatorType deckGetRequiredEstimator();
 void stateEstimatorInit(StateEstimatorType estimator);
 bool stateEstimatorTest(void);
 void stateEstimatorSwitchTo(StateEstimatorType estimator);

components/core/crazyflie/modules/src/crtp_commander_rpyt.c

@@ -26,13 +26,13 @@
  */
 #include <math.h>
 #include <stdbool.h>
+#include "freertos/FreeRTOS.h"
 
 #include "crtp_commander.h"
-
 #include "commander.h"
+#include "estimator.h"
 #include "crtp.h"
 #include "param.h"
-#include "freertos/FreeRTOS.h"
 #include "num.h"
 #include "stm32_legacy.h"
 #define DEBUG_MODULE "MODE"
@@ -95,21 +95,25 @@ void setCommandermode(FlightMode mode){
     altHoldMode = true;
     posHoldMode = false;
     posSetMode = false;
+    registerRequiredEstimator(complementaryEstimator);
     break;
   case POSHOLD_MODE:
     altHoldMode = true;
     posHoldMode = true;
-    posSetMode = false; 
+    posSetMode = false;
+    registerRequiredEstimator(kalmanEstimator); 
     break;
   case POSSET_MODE:
     altHoldMode = false;
     posHoldMode = false;
-    posSetMode = true;  
+    posSetMode = true;
+    registerRequiredEstimator(kalmanEstimator); 
     break;
   default:
     altHoldMode = false;
     posHoldMode = false;
-    posSetMode = false;  
+    posSetMode = false;
+    registerRequiredEstimator(complementaryEstimator);   
     break;
   }
   DEBUG_PRINTI("FlightMode = %u",mode);

components/core/crazyflie/modules/src/estimator.c

@@ -8,6 +8,7 @@
 #define ESTIMATOR_NAME anyEstimator
 #define DEFAULT_ESTIMATOR complementaryEstimator
 static StateEstimatorType currentEstimator = anyEstimator;
+static StateEstimatorType requiredEstimator = anyEstimator;
 
 static void initEstimator(const StateEstimatorType estimator);
 static void deinitEstimator(const StateEstimatorType estimator);
@@ -82,6 +83,30 @@ static EstimatorFcns estimatorFunctions[] = {
     },
 };
 
+bool registerRequiredEstimator(StateEstimatorType estimator)
+{
+  bool isError = false;
+
+  if (anyEstimator != estimator) {
+    if (anyEstimator == requiredEstimator) {
+      requiredEstimator = estimator;
+    }
+    else {
+      if (requiredEstimator != estimator) {
+        isError = true;
+        DEBUG_PRINTW("WARNING: Two decks require different estimators\n");
+      }
+    }
+  }
+
+  return isError;
+}
+
+StateEstimatorType deckGetRequiredEstimator()
+{
+  return requiredEstimator;
+}
+
 void stateEstimatorInit(StateEstimatorType estimator) {
   stateEstimatorSwitchTo(estimator);
 }

components/core/crazyflie/modules/src/stabilizer.c

@@ -184,6 +184,9 @@ void stabilizerInit(StateEstimatorType estimator)
     return;
 
   sensorsInit();
+  if (estimator == anyEstimator) {
+    estimator = deckGetRequiredEstimator();
+  }
   stateEstimatorInit(estimator);
   controllerInit(ControllerTypeAny);
   powerDistributionInit();

components/core/crazyflie/modules/src/system.c

@@ -134,7 +134,7 @@ bool systemTest()
   DEBUG_PRINTI("pmTest = %d", pass);
   pass &= workerTest();
   DEBUG_PRINTI("workerTest = %d", pass);
-  //pass &= buzzerTest();
+  pass &= buzzerTest();
   return pass;
 }
 
@@ -164,7 +164,7 @@ void systemTask(void *arg)
   commInit();
   commanderInit();
 
-  StateEstimatorType estimator = kalmanEstimator;
+  StateEstimatorType estimator = anyEstimator;
   estimatorKalmanTaskInit();
   //deckInit();
   //estimator = deckGetRequiredEstimator();