Merge branch 'feature/brushless-prop-test' of https://github.com/coll…

…mot/crazyflie-firmware into collmot-feature/brushless-prop-test

src/drivers/interface/motors.h

@@ -216,6 +216,13 @@ typedef struct
   void (*preloadConfig)(TIM_TypeDef* TIMx, uint16_t TIM_OCPreload);
 } MotorPerifDef;
 
+typedef struct {
+  uint16_t onPeriodMsec;
+  uint16_t offPeriodMsec;
+  uint16_t varianceMeasurementStartMsec;
+  uint16_t onPeriodPWMRatio;
+} MotorHealthTestDef;
+
 /**
  * Motor mapping configurations
  */
@@ -273,5 +280,11 @@ void motorsTestTask(void* params);
  * */
 void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio);
 
+/**
+ * Retrieve the health test settings of the given motor. This allows us to use
+ * different health test timings and PWM ratios for brushed and brushless motors.
+ */
+const MotorHealthTestDef* motorsGetHealthTestSettings(uint32_t id);
+
 #endif /* __MOTORS_H__ */
 

src/drivers/src/motors.c

@@ -61,6 +61,27 @@ const uint32_t MOTORS[] = { MOTOR_M1, MOTOR_M2, MOTOR_M3, MOTOR_M4 };
 
 const uint16_t testsound[NBR_OF_MOTORS] = {A4, A5, F5, D5 };
 
+const MotorHealthTestDef brushedMotorHealthTestSettings = {
+  /* onPeriodMsec = */ 50,
+  /* offPeriodMsec = */ 950,
+  /* varianceMeasurementStartMsec = */ 0,
+  /* onPeriodPWMRatio = */ 0xFFFF,
+};
+
+const MotorHealthTestDef brushlessMotorHealthTestSettings = {
+  /* onPeriodMsec = */ 2000,
+  /* offPeriodMsec = */ 1000,
+  /* varianceMeasurementStartMsec = */ 1000,
+  /* onPeriodPWMRatio = */ 0 /* user must set health.propTestPWMRatio explicitly */
+};
+
+const MotorHealthTestDef unknownMotorHealthTestSettings = {
+  /* onPeriodMsec = */ 0,
+  /* offPeriodMseec = */ 0,
+  /* varianceMeasurementStartMsec = */ 0,
+  /* onPeriodPWMRatio = */ 0
+};
+
 static bool isInit = false;
 
 /* Private functions */
@@ -374,6 +395,27 @@ void motorsPlayMelody(uint16_t *notes)
     motorsPlayTone(note, duration);
   } while (duration != 0);
 }
+
+const MotorHealthTestDef* motorsGetHealthTestSettings(uint32_t id)
+{
+  if (id >= NBR_OF_MOTORS)
+  {
+    return &unknownMotorHealthTestSettings;
+  }
+  else if (motorMap[id]->drvType == BRUSHLESS)
+  {
+    return &brushlessMotorHealthTestSettings;
+  }
+  else if (motorMap[id]->drvType == BRUSHED)
+  {
+    return &brushedMotorHealthTestSettings;
+  }
+  else
+  {
+    return &unknownMotorHealthTestSettings;
+  }
+}
+
 /**
  * Logging variables of the motors PWM output
  */

src/modules/src/health.c

@@ -47,11 +47,17 @@
 
 #include "static_mem.h"
 
+#ifndef DEFAULT_PROP_TEST_PWM_RATIO
+#  define DEFAULT_PROP_TEST_PWM_RATIO 0
+#endif
+
 #define PROPTEST_NBR_OF_VARIANCE_VALUES   100
 
 static bool startPropTest = false;
 static bool startBatTest = false;
 
+static uint16_t propTestPWMRatio = DEFAULT_PROP_TEST_PWM_RATIO;
+
 static uint32_t i = 0;
 NO_DMA_CCM_SAFE_ZERO_INIT static float accX[PROPTEST_NBR_OF_VARIANCE_VALUES];
 NO_DMA_CCM_SAFE_ZERO_INIT static float accY[PROPTEST_NBR_OF_VARIANCE_VALUES];
@@ -139,6 +145,9 @@ bool healthShallWeRunTest(void)
 
 void healthRunTests(sensorData_t *sensors)
 {
+  const MotorHealthTestDef* healthTestSettings;
+  int32_t sampleIndex;
+
   /* Propeller test */
   if (testState == configureAcc)
   {
@@ -167,31 +176,25 @@ void healthRunTests(sensorData_t *sensors)
                   (double)accVarXnf + (double)accVarYnf, (double)accVarZnf);
       testState = measureProp;
     }
-
   }
   else if (testState == measureProp)
   {
-    if (i < PROPTEST_NBR_OF_VARIANCE_VALUES)
+    healthTestSettings = motorsGetHealthTestSettings(motorToTest);
+
+    sampleIndex = ((int32_t) i) - healthTestSettings->varianceMeasurementStartMsec;
+    if (sampleIndex >= 0 && sampleIndex < PROPTEST_NBR_OF_VARIANCE_VALUES)
     {
-      accX[i] = sensors->acc.x;
-      accY[i] = sensors->acc.y;
-      accZ[i] = sensors->acc.z;
+      accX[sampleIndex] = sensors->acc.x;
+      accY[sampleIndex] = sensors->acc.y;
+      accZ[sampleIndex] = sensors->acc.z;
       if (pmGetBatteryVoltage() < minSingleLoadedVoltage[motorToTest])
       {
         minSingleLoadedVoltage[motorToTest] = pmGetBatteryVoltage();
       }
     }
     i++;
 
-    if (i == 1)
-    {
-      motorsSetRatio(motorToTest, 0xFFFF);
-    }
-    else if (i == 50)
-    {
-      motorsSetRatio(motorToTest, 0);
-    }
-    else if (i == PROPTEST_NBR_OF_VARIANCE_VALUES)
+    if (sampleIndex == PROPTEST_NBR_OF_VARIANCE_VALUES)
     {
       accVarX[motorToTest] = variance(accX, PROPTEST_NBR_OF_VARIANCE_VALUES);
       accVarY[motorToTest] = variance(accY, PROPTEST_NBR_OF_VARIANCE_VALUES);
@@ -202,7 +205,16 @@ void healthRunTests(sensorData_t *sensors)
                    (double)accVarZ[motorToTest],
                    (double)(idleVoltage - minSingleLoadedVoltage[motorToTest]));
     }
-    else if (i >= 1000)
+
+    if (i == 1 && healthTestSettings->onPeriodMsec > 0)
+    {
+      motorsSetRatio(motorToTest, propTestPWMRatio > 0 ? propTestPWMRatio : healthTestSettings->onPeriodPWMRatio);
+    }
+    else if (i == healthTestSettings->onPeriodMsec)
+    {
+      motorsSetRatio(motorToTest, 0);
+    }
+    else if (i >= healthTestSettings->onPeriodMsec + healthTestSettings->offPeriodMsec)
     {
       i = 0;
       motorToTest++;
@@ -323,6 +335,11 @@ PARAM_ADD_CORE(PARAM_UINT8, startPropTest, &startPropTest)
  */
 PARAM_ADD_CORE(PARAM_UINT8, startBatTest, &startBatTest)
 
+/**
+ * @brief PWM ratio to use when testing propellers. Required for brushless motors.
+ */
+PARAM_ADD_CORE(PARAM_UINT16, propTestPWMRatio, &propTestPWMRatio)
+
 PARAM_GROUP_STOP(health)
 
 /**

tools/make/config.mk.example

@@ -107,6 +107,8 @@
 # CFLAGS += -DSTART_DISARMED
 # IDLE motor drive when armed, 0 = 0%, 65535 = 100% (the motors runs as long as the Crazyflie is armed)
 # CFLAGS += -DDEFAULT_IDLE_THRUST=5000
+# Default PWM ratio to use when testing the propellers
+# CFLAGS += -DDEFAULT_PROP_TEST_PWM_RATIO=10000
 
 ## Lighthouse handling
 # If lighthouse will need to act as a ground truth (so not entered in the kalman filter)