diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index be025a6dfa..06e364090a 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -55,6 +55,7 @@ #include "drivers/compass/compass.h" #include "drivers/display.h" #include "drivers/dshot.h" +#include "drivers/dshot_command.h" #include "drivers/flash.h" #include "drivers/io.h" #include "drivers/motor.h" @@ -3002,6 +3003,34 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, } break; +#ifdef USE_DSHOT + case MSP2_SEND_DSHOT_COMMAND: + { + const bool armed = ARMING_FLAG(ARMED); + + if (!armed) { + const uint8_t commandType = sbufReadU8(src); + const uint8_t motorIndex = sbufReadU8(src); + const uint8_t commandCount = sbufReadU8(src); + + if (DSHOT_CMD_TYPE_BLOCKING == commandType) { + motorDisable(); + } + + for (uint8_t i = 0; i < commandCount; i++) { + const uint8_t commandIndex = sbufReadU8(src); + dshotCommandWrite(motorIndex, getMotorCount(), commandIndex, commandType); + delay(1); + } + + if (DSHOT_CMD_TYPE_BLOCKING == commandType) { + motorEnable(); + } + } + } + break; +#endif + #ifdef USE_CAMERA_CONTROL case MSP_CAMERA_CONTROL: { diff --git a/src/main/msp/msp_protocol_v2_betaflight.h b/src/main/msp/msp_protocol_v2_betaflight.h index ba07b5b747..fd11c5873f 100644 --- a/src/main/msp/msp_protocol_v2_betaflight.h +++ b/src/main/msp/msp_protocol_v2_betaflight.h @@ -21,3 +21,5 @@ #define MSP2_BETAFLIGHT_BIND 0x3000 #define MSP2_MOTOR_OUTPUT_REORDERING 0x3001 #define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002 +#define MSP2_SEND_DSHOT_COMMAND 0x3003 +