Merge pull request #1192 from bitcraze/tobba/battery_test

Adjusted battery test and made it configurable + clean-up

src/config/config.h

@@ -225,7 +225,7 @@
  * and measure the voltage sag. The threshold is very experimental and dependent on stock configuration. It is
  * fairly constant over the battery voltage range but testing with fully changed battery is best.
  */
-#define BAT_LOADING_SAG_THRESHOLD  0.95f
+#define BAT_LOADING_SAG_THRESHOLD  0.70f
 
 /**
  * \def ACTIVATE_STARTUP_SOUND

src/drivers/interface/motors.h

@@ -246,7 +246,8 @@ typedef struct {
   uint16_t onPeriodMsec;
   uint16_t offPeriodMsec;
   uint16_t varianceMeasurementStartMsec;
-  uint16_t onPeriodPWMRatio;
+  uint16_t onPeriodPWMRatioProp;
+  uint16_t onPeriodPWMRatioBat;
 } MotorHealthTestDef;
 
 /**

src/drivers/src/motors.c

@@ -39,6 +39,7 @@
 #include "power_distribution.h"
 #include "nvicconf.h"
 #include "usec_time.h"
+#include "platform_defaults.h"
 //FreeRTOS includes
 #include "task.h"
 
@@ -71,24 +72,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,
+   .onPeriodMsec = HEALTH_BRUSHED_ON_PERIOD_MSEC,
+   .offPeriodMsec = HEALTH_BRUSHED_OFF_PERIOD_MSEC,
+   .varianceMeasurementStartMsec = HEALTH_BRUSHED_VARIANCE_START_MSEC,
+   .onPeriodPWMRatioProp = HEALTH_BRUSHED_PROP_ON_PERIOD_PWM_RATIO,
+   .onPeriodPWMRatioBat = HEALTH_BRUSHED_BAT_ON_PERIOD_PWM_RATIO,
 };
 
 const MotorHealthTestDef brushlessMotorHealthTestSettings = {
-  /* onPeriodMsec = */ 2000,
-  /* offPeriodMsec = */ 1000,
-  /* varianceMeasurementStartMsec = */ 1000,
-  /* onPeriodPWMRatio = */ 0 /* user must set health.propTestPWMRatio explicitly */
+    .onPeriodMsec = HEALTH_BRUSHLESS_ON_PERIOD_MSEC,
+    .offPeriodMsec = HEALTH_BRUSHLESS_OFF_PERIOD_MSEC,
+    .varianceMeasurementStartMsec = HEALTH_BRUSHLESS_VARIANCE_START_MSEC,
+    .onPeriodPWMRatioProp = 0, /* user must set health.propTestPWMRatio explicitly */
+    .onPeriodPWMRatioBat = 0, /* user must set health.batTestPWMRatio explicitly */
 };
 
 const MotorHealthTestDef unknownMotorHealthTestSettings = {
-  /* onPeriodMsec = */ 0,
-  /* offPeriodMseec = */ 0,
-  /* varianceMeasurementStartMsec = */ 0,
-  /* onPeriodPWMRatio = */ 0
+    .onPeriodMsec = 0,
+    .offPeriodMsec = 0,
+    .varianceMeasurementStartMsec = 0,
+    .onPeriodPWMRatioProp = 0,
+    .onPeriodPWMRatioBat = 0,
 };
 
 static bool isInit = false;

src/modules/src/Kconfig

@@ -156,6 +156,18 @@ config MOTORS_DEFAULT_PROP_TEST_PWM_RATIO
 
         The value specified here can also be overridden with parameters.
 
+config MOTORS_DEFAULT_BAT_TEST_PWM_RATIO
+    int "Override default PWM ratio to use during battery tests"
+    range 0 65535
+    default 0
+    help
+        PWM ratio to use during battery test, expressed as an integer in the range
+        0 to 65535. Zero means not to override the PWM ratio because there a differences
+        between brushed and brushless;. Brushless is 0 for safety reasons. Different motors
+        needs different value, hance this config variable.
+
+        The value specified here can also be overridden with parameters.
+
 choice
     prompt "Type of power distribution"
     default POWER_DISTRIBUTION_QUADROTOR

src/modules/src/health.c

@@ -54,8 +54,9 @@ static bool startPropTest = false;
 static bool startBatTest = false;
 
 static uint16_t propTestPWMRatio = CONFIG_MOTORS_DEFAULT_PROP_TEST_PWM_RATIO;
+static uint16_t batTestPWMRatio = CONFIG_MOTORS_DEFAULT_BAT_TEST_PWM_RATIO;
 
-static uint32_t i = 0;
+static uint32_t tick = 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];
 NO_DMA_CCM_SAFE_ZERO_INIT static float accZ[PROPTEST_NBR_OF_VARIANCE_VALUES];
@@ -135,6 +136,7 @@ bool healthShallWeRunTest(void)
   } else if (startBatTest != false) {
     testState = testBattery;
     startBatTest = false;
+    tick = 0;
   }
 
   return (testState != testDone);
@@ -161,13 +163,13 @@ void healthRunTests(sensorData_t *sensors)
   }
   if (testState == measureNoiseFloor)
   {
-    accX[i] = sensors->acc.x;
-    accY[i] = sensors->acc.y;
-    accZ[i] = sensors->acc.z;
+    accX[tick] = sensors->acc.x;
+    accY[tick] = sensors->acc.y;
+    accZ[tick] = sensors->acc.z;
 
-    if (++i >= PROPTEST_NBR_OF_VARIANCE_VALUES)
+    if (++tick >= PROPTEST_NBR_OF_VARIANCE_VALUES)
     {
-      i = 0;
+      tick = 0;
       accVarXnf = variance(accX, PROPTEST_NBR_OF_VARIANCE_VALUES);
       accVarYnf = variance(accY, PROPTEST_NBR_OF_VARIANCE_VALUES);
       accVarZnf = variance(accZ, PROPTEST_NBR_OF_VARIANCE_VALUES);
@@ -180,7 +182,7 @@ void healthRunTests(sensorData_t *sensors)
   {
     healthTestSettings = motorsGetHealthTestSettings(motorToTest);
 
-    sampleIndex = ((int32_t) i) - healthTestSettings->varianceMeasurementStartMsec;
+    sampleIndex = ((int32_t) tick) - healthTestSettings->varianceMeasurementStartMsec;
     if (sampleIndex >= 0 && sampleIndex < PROPTEST_NBR_OF_VARIANCE_VALUES)
     {
       accX[sampleIndex] = sensors->acc.x;
@@ -191,7 +193,7 @@ void healthRunTests(sensorData_t *sensors)
         minSingleLoadedVoltage[motorToTest] = pmGetBatteryVoltage();
       }
     }
-    i++;
+    tick++;
 
     if (sampleIndex == PROPTEST_NBR_OF_VARIANCE_VALUES)
     {
@@ -205,64 +207,65 @@ void healthRunTests(sensorData_t *sensors)
                    (double)(idleVoltage - minSingleLoadedVoltage[motorToTest]));
     }
 
-    if (i == 1 && healthTestSettings->onPeriodMsec > 0)
+    if (tick == 1 && healthTestSettings->onPeriodMsec > 0)
     {
-      motorsSetRatio(motorToTest, propTestPWMRatio > 0 ? propTestPWMRatio : healthTestSettings->onPeriodPWMRatio);
+      motorsSetRatio(motorToTest, propTestPWMRatio > 0 ? propTestPWMRatio : healthTestSettings->onPeriodPWMRatioProp);
     }
-    else if (i == healthTestSettings->onPeriodMsec)
+    else if (tick == healthTestSettings->onPeriodMsec)
     {
       motorsSetRatio(motorToTest, 0);
     }
-    else if (i >= healthTestSettings->onPeriodMsec + healthTestSettings->offPeriodMsec)
+    else if (tick >= healthTestSettings->onPeriodMsec + healthTestSettings->offPeriodMsec)
     {
-      i = 0;
+      tick = 0;
       motorToTest++;
       if (motorToTest >= NBR_OF_MOTORS)
       {
-        i = 0;
+        tick = 0;
         motorToTest = 0;
         testState = evaluatePropResult;
         sensorsSetAccMode(ACC_MODE_FLIGHT);
       }
     }
   }
-  /* Experimental battery test, i should count up each ms */
+  /* Experimental battery test, tick should count up each ms */
   else if (testState == testBattery)
   {
-    if (i == 0)
+    healthTestSettings = motorsGetHealthTestSettings(0);
+
+    if (tick == 0)
     {
       batteryPass = 0;
       minLoadedVoltage = idleVoltage = pmGetBatteryVoltage();
     }
-    if (i == 1)
+    if (tick == 1)
     {
-      motorsSetRatio(MOTOR_M1, 0xFFFF);
-      motorsSetRatio(MOTOR_M2, 0xFFFF);
-      motorsSetRatio(MOTOR_M3, 0xFFFF);
-      motorsSetRatio(MOTOR_M4, 0xFFFF);
+      motorsSetRatio(MOTOR_M1, batTestPWMRatio > 0 ? batTestPWMRatio : healthTestSettings->onPeriodPWMRatioBat);
+      motorsSetRatio(MOTOR_M2, batTestPWMRatio > 0 ? batTestPWMRatio : healthTestSettings->onPeriodPWMRatioBat);
+      motorsSetRatio(MOTOR_M3, batTestPWMRatio > 0 ? batTestPWMRatio : healthTestSettings->onPeriodPWMRatioBat);
+      motorsSetRatio(MOTOR_M4, batTestPWMRatio > 0 ? batTestPWMRatio : healthTestSettings->onPeriodPWMRatioBat);
     }
-    else if (i < 50)
+    else if (tick < 50)
     {
       if (pmGetBatteryVoltage() < minLoadedVoltage)
         minLoadedVoltage = pmGetBatteryVoltage();
     }
-    else if (i == 50)
+    else if (tick == 50)
     {
       motorsStop();
       testState = evaluateBatResult;
-      i = 0;
     }
-    i++;
+    tick++;
   }
   else if (testState == restartBatTest)
   {
     // Mainly used for testing
-    if (i++ > 2000)
+    if (tick++ > 2000)
     {
       DEBUG_PRINT("Idle:%.2f sag: %.2f\n", (double)idleVoltage,
                   (double)(idleVoltage - minLoadedVoltage));
       testState = testBattery;
-      i = 0;
+      tick = 0;
     }
   }
   else if (testState == evaluateBatResult)
@@ -336,6 +339,11 @@ PARAM_ADD_CORE(PARAM_UINT8, startBatTest, &startBatTest)
  */
 PARAM_ADD_CORE(PARAM_UINT16 | PARAM_PERSISTENT, propTestPWMRatio, &propTestPWMRatio)
 
+/**
+ * @brief PWM ratio to use when testing the battery. [0 - UINT16_MAX]
+ */
+PARAM_ADD_CORE(PARAM_UINT16 | PARAM_PERSISTENT, batTestPWMRatio, &batTestPWMRatio)
+
 PARAM_GROUP_STOP(health)
 
 /**

src/platform/interface/platform_defaults.h

@@ -112,4 +112,34 @@
 // Tumble detection enabled by default
 #ifndef SUPERVISOR_TUMBLE_CHECK_ENABLE
     #define SUPERVISOR_TUMBLE_CHECK_ENABLE true
-#endif
+#endif
+
+
+// Health test parameters
+#ifndef HEALTH_BRUSHED_ON_PERIOD_MSEC
+    #define HEALTH_BRUSHED_ON_PERIOD_MSEC 50
+#endif
+#ifndef HEALTH_BRUSHED_OFF_PERIOD_MSEC
+    #define HEALTH_BRUSHED_OFF_PERIOD_MSEC 950
+#endif
+#ifndef HEALTH_BRUSHED_VARIANCE_START_MSEC
+    #define HEALTH_BRUSHED_VARIANCE_START_MSEC 0
+#endif
+#ifndef HEALTH_BRUSHED_PROP_ON_PERIOD_PWM_RATIO
+    #define HEALTH_BRUSHED_PROP_ON_PERIOD_PWM_RATIO 0xFFFF
+#endif
+#ifndef HEALTH_BRUSHED_BAT_ON_PERIOD_PWM_RATIO
+    #define HEALTH_BRUSHED_BAT_ON_PERIOD_PWM_RATIO 40000
+#endif
+
+#ifndef HEALTH_BRUSHLESS_ON_PERIOD_MSEC
+    #define HEALTH_BRUSHLESS_ON_PERIOD_MSEC 2000
+#endif
+#ifndef HEALTH_BRUSHLESS_OFF_PERIOD_MSEC
+    #define HEALTH_BRUSHLESS_OFF_PERIOD_MSEC 1000
+#endif
+#ifndef HEALTH_BRUSHLESS_VARIANCE_START_MSEC
+    #define HEALTH_BRUSHLESS_VARIANCE_START_MSEC 1000
+#endif
+
+