diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 2b36a8d18a..acdddac37f 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -21,6 +21,7 @@ #include #include "platform.h" +#include "system.h" #include "io.h" #include "pwm_output.h" @@ -29,6 +30,8 @@ #define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) +#define DSHOT_MAX_COMMAND 47 + static pwmWriteFuncPtr pwmWritePtr; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL; @@ -315,6 +318,27 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) return MOTOR_DSHOT150_MHZ * 1000000; } } + +void pwmWriteDshotCommand(uint8_t index, uint8_t command) +{ + if (command <= DSHOT_MAX_COMMAND) { + motorDmaOutput_t *const motor = getMotorDmaOutput(index); + + unsigned repeats; + if ((command >= 7 && command <= 10) || command == 12) { + repeats = 10; + } else { + repeats = 1; + } + for (; repeats; repeats--) { + motor->requestTelemetry = true; + pwmWritePtr(index, command); + pwmCompleteMotorUpdate(0); + + delay(1); + } + } +} #endif #ifdef USE_SERVOS diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index b71d1ddfe8..9dc3b0e325 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -139,6 +139,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn #ifdef USE_DSHOT uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); +void pwmWriteDshotCommand(uint8_t index, uint8_t command); void pwmWriteDigital(uint8_t index, uint16_t value); void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); diff --git a/src/main/drivers/serial_escserial.h b/src/main/drivers/serial_escserial.h index 41bbf07556..5f2fa0b68c 100644 --- a/src/main/drivers/serial_escserial.h +++ b/src/main/drivers/serial_escserial.h @@ -33,6 +33,8 @@ typedef enum { PROTOCOL_COUNT } escProtocol_e; +#define ALL_ESCS 255 + serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_t options, uint8_t mode); // serialPort API diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 841da95360..542c716a78 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3221,67 +3221,134 @@ static void cliGpsPassthrough(char *cmdline) } #endif -#ifdef USE_ESCSERIAL -static void cliEscPassthrough(char *cmdline) -{ - uint8_t mode = 0; - int index = 0; - int i = 0; - char *pch = NULL; - char *saveptr; +#if defined(USE_ESCSERIAL) || defined(USE_DSHOT) - if (isEmpty(cmdline)) { +#ifndef ALL_ESCS +#define ALL_ESCS 255 +#endif + +static int parseEscNumber(char *pch, bool allowAllEscs) { + int escNumber = atoi(pch); + if ((escNumber >= 0) && (escNumber < getMotorCount())) { + printf("Programming on ESC %d.\r\n", escNumber); + } else if (allowAllEscs && escNumber == ALL_ESCS) { + printf("Programming on all ESCs.\r\n"); + } else { + printf("Invalid ESC number, range: 0 to %d.\r\n", getMotorCount() - 1); + + return -1; + } + + return escNumber; +} +#endif + +#ifdef USE_DSHOT +static void cliDshotProg(char *cmdline) +{ + if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) { cliShowParseError(); + return; } - pch = strtok_r(cmdline, " ", &saveptr); + char *saveptr; + char *pch = strtok_r(cmdline, " ", &saveptr); + int pos = 0; + int escNumber = 0; while (pch != NULL) { - switch (i) { + switch (pos) { case 0: - if(strncasecmp(pch, "sk", strlen(pch)) == 0) - { - mode = 0; + escNumber = parseEscNumber(pch, true); + if (escNumber == -1) { + return; } - else if(strncasecmp(pch, "bl", strlen(pch)) == 0) - { - mode = 1; + + break; + default: + motorControlEnable = false; + + int command = atoi(pch); + if (command >= 0 && command < DSHOT_MIN_THROTTLE) { + if (escNumber == ALL_ESCS) { + for (unsigned i = 0; i < getMotorCount(); i++) { + pwmWriteDshotCommand(i, command); + } + } else { + pwmWriteDshotCommand(escNumber, command); + } + + if (command <= 5) { + delay(10); // wait for sound output to finish + } + + printf("Command %d written.\r\n", command); + } else { + printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1); } - else if(strncasecmp(pch, "ki", strlen(pch)) == 0) - { - mode = 2; - } - else if(strncasecmp(pch, "cc", strlen(pch)) == 0) - { - mode = 4; - } - else - { + + break; + } + + pos++; + pch = strtok_r(NULL, " ", &saveptr); + } + + motorControlEnable = true; +} +#endif + +#ifdef USE_ESCSERIAL +static void cliEscPassthrough(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliShowParseError(); + + return; + } + + char *saveptr; + char *pch = strtok_r(cmdline, " ", &saveptr); + int pos = 0; + uint8_t mode = 0; + int escNumber = 0; + while (pch != NULL) { + switch (pos) { + case 0: + if(strncasecmp(pch, "sk", strlen(pch)) == 0) { + mode = PROTOCOL_SIMONK; + } else if(strncasecmp(pch, "bl", strlen(pch)) == 0) { + mode = PROTOCOL_BLHELI; + } else if(strncasecmp(pch, "ki", strlen(pch)) == 0) { + mode = PROTOCOL_KISS; + } else if(strncasecmp(pch, "cc", strlen(pch)) == 0) { + mode = PROTOCOL_KISSALL; + } else { cliShowParseError(); + return; } break; case 1: - index = atoi(pch); - if(mode == 2 && index == 255) - { - printf("passthrough on all outputs enabled\r\n"); - } - else{ - if ((index >= 0) && (index < USABLE_TIMER_CHANNEL_COUNT)) { - printf("passthrough on output %d enabled\r\n", index); - } - else { - printf("invalid output, range: 1 to %d\r\n", USABLE_TIMER_CHANNEL_COUNT); - return; - } + escNumber = parseEscNumber(pch, mode == PROTOCOL_KISS); + if (escNumber == -1) { + return; } + break; + default: + cliShowParseError(); + + return; + + break; + } - i++; + pos++; pch = strtok_r(NULL, " ", &saveptr); } - escEnablePassthrough(cliPort,index,mode); + + escEnablePassthrough(cliPort, escNumber, mode); } #endif @@ -3356,13 +3423,13 @@ static void cliMotor(char *cmdline) if (index == 2) { if (motor_value < PWM_RANGE_MIN || motor_value > PWM_RANGE_MAX) { cliShowArgumentRangeError("value", 1000, 2000); - return; } else { motor_disarmed[motor_index] = convertExternalToMotor(motor_value); + + cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); } } - cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index])); } #ifndef MINIMAL_CLI @@ -4216,6 +4283,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("bl", "reboot into bootloader", NULL, cliBootloader), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|profile|rates|all] {showdefaults}", cliDiff), +#ifdef USE_DSHOT + CLI_COMMAND_DEF("dshotprog", "program DShot ESC(s)", " +", cliDshotProg), +#endif CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile|rates|all] {showdefaults}", cliDump), #ifdef USE_ESCSERIAL diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 1b83302eb4..e96621b6da 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -24,6 +24,8 @@ extern int16_t magHold; extern bool isRXDataNew; extern int16_t headFreeModeHold; +extern uint8_t motorControlEnable; + typedef struct throttleCorrectionConfig_s { uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8c73f00514..f3de87d9ea 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -230,7 +230,7 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr case PROTOCOL_KISS: case PROTOCOL_KISSALL: case PROTOCOL_CASTLE: - if (escPortIndex < USABLE_TIMER_CHANNEL_COUNT || (escMode == PROTOCOL_KISS && escPortIndex == 255)) { + if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_ESCS)) { sbufWriteU8(dst, 1); if (mspPostProcessFn) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 831b3dd978..1cfc0620e6 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -39,8 +39,8 @@ 3D Mode: 0 = stop - 48 (low) - 1047 (high) -> positive direction - 1048 (low) - 2047 (high) -> negative direction + 48 (low) - 1047 (high) -> negative direction + 1048 (low) - 2047 (high) -> positive direction */ // Digital protocol has fixed values