diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 09946a4ab0..d829f9babb 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -893,6 +893,61 @@ void finishBlackbox(void) } } +/** + * Test Motors Blackbox Logging + */ +bool startedLoggingInTestMode = false; + +void startInTestMode(void) +{ + if(!startedLoggingInTestMode) { + if (masterConfig.blackbox_device == BLACKBOX_DEVICE_SERIAL) { + serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); + if (sharedBlackboxAndMspPort) { + return; // When in test mode, we cannot share the MSP and serial logger port! + } + } + startBlackbox(); + startedLoggingInTestMode = true; + } +} +void stopInTestMode(void) +{ + if(startedLoggingInTestMode) { + finishBlackbox(); + startedLoggingInTestMode = false; + } +} +/** + * We are going to monitor the MSP_SET_MOTOR target variables motor_disarmed[] for values other than minthrottle + * on reading a value (i.e. the user is testing the motors), then we enable test mode logging; + * we monitor when the values return to minthrottle and start a delay timer (5 seconds); if + * the test motors are left at minimum throttle for this delay timer, then we assume we are done testing and + * shutdown the logger. + * + * Of course, after the 5 seconds and shutdown of the logger, the system will be re-enabled to allow the + * test mode to trigger again; its just that the data will be in a second, third, fourth etc log file. + */ +bool inMotorTestMode(void) { + static uint32_t resetTime = 0; + uint16_t activeMinimumCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.escAndServoConfig.mincommand); + int i; + bool motorsNotAtMin = false; + + // set disarmed motor values + for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) + motorsNotAtMin |= (motor_disarmed[i] != activeMinimumCommand); + + if(motorsNotAtMin) { + resetTime = millis() + 5000; // add 5 seconds + return true; + } else { + // Monitor the duration at minimum + return (millis() < resetTime); + } + return false; +} + #ifdef GPS static void writeGPSHomeFrame() { @@ -1533,7 +1588,8 @@ void handleBlackbox(void) break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 - if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) { + // Prevent the Pausing of the log on the mode switch if in Motor Test Mode + if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) { blackboxSetState(BLACKBOX_STATE_PAUSED); } else { blackboxLogIteration(); @@ -1562,6 +1618,22 @@ void handleBlackbox(void) // Did we run out of room on the device? Stop! if (isBlackboxDeviceFull()) { blackboxSetState(BLACKBOX_STATE_STOPPED); + // ensure we reset the test mode flag if we stop due to full memory card + if (startedLoggingInTestMode) startedLoggingInTestMode = false; + } else { // Only log in test mode if there is room! + + if(masterConfig.blackbox_on_motor_test) { + // Handle Motor Test Mode + if(inMotorTestMode()) { + if(blackboxState==BLACKBOX_STATE_STOPPED) { + startInTestMode(); + } + } else { + if(blackboxState!=BLACKBOX_STATE_STOPPED) { + stopInTestMode(); + } + } + } } } diff --git a/src/main/config/config.c b/src/main/config/config.c index 361173a7b7..87aa098e1c 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -680,6 +680,7 @@ void createDefaultConfig(master_t *config) config->blackbox_rate_num = 1; config->blackbox_rate_denom = 1; + config->blackbox_on_motor_test = 0; // default off #endif // BLACKBOX #ifdef SERIALRX_UART diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 0f79ee2f44..aa572fac0a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -159,6 +159,7 @@ typedef struct master_t { uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; uint8_t blackbox_device; + uint8_t blackbox_on_motor_test; #endif uint32_t beeper_off_flags; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f8e378e67b..98c92a69ca 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -885,6 +885,8 @@ const clivalue_t valueTable[] = { { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, + { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_on_motor_test, .config.lookup = { TABLE_OFF_ON } }, + #endif #ifdef VTX