diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h index a62d121bd5..e161740699 100644 --- a/src/drivers/interface/motors.h +++ b/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__ */ diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c index 24bccfb734..d2d8bd390a 100644 --- a/src/drivers/src/motors.c +++ b/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 */ diff --git a/src/modules/src/health.c b/src/modules/src/health.c index ec778f18bf..da00fd87f7 100644 --- a/src/modules/src/health.c +++ b/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,15 +176,17 @@ 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(); @@ -183,15 +194,7 @@ void healthRunTests(sensorData_t *sensors) } 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) /** diff --git a/tools/make/config.mk.example b/tools/make/config.mk.example index 9f4b7c452e..0e81129c8f 100644 --- a/tools/make/config.mk.example +++ b/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)