Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 20:10:18 +03:00
Code Issues Wiki Activity

Added blocked dshot commands, Added post command delay

Browse source 
Moved the dshotcommand to happen in pwm_output_dshot.
Simplified the timing and repeats to happen on a group basis instead works better for dshotburst
This commit is contained in:
Bryce Johnson 2018-04-17 21:48:16 -06:00 • committed by Michael Keller
parent f37a8184d5
commit 0045b36320
8 changed files with 131 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

0
1.8 Normal file
View file

85
src/main/drivers/pwm_output.c
View file

@ -37,6 +37,18 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL;
#ifdef USE_DSHOT
FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
#define DSHOT_COMMAND_DELAY_US 1000
#define DSHOT_ESCINFO_DELAY_US 5000
#define DSHOT_BEEP_DELAY_US 100000
typedef struct dshotCommandControl_s {
timeUs_t nextCommandAt;
timeUs_t delayAfterCommand;
uint8_t command[MAX_SUPPORTED_MOTORS];
uint8_t repeats;
} dshotCommandControl_t;
dshotCommandControl_t dshotCommandControl;
#endif
#ifdef USE_SERVOS
@ -374,10 +386,17 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
}
}
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command)
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
{
if (isDshot && (command <= DSHOT_MAX_COMMAND)) {
unsigned repeats;
timeUs_t timeNowUs = micros();
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND)) {
return;
}
uint8_t repeats = 1;
timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US;
switch (command) {
case DSHOT_CMD_SPIN_DIRECTION_1:
case DSHOT_CMD_SPIN_DIRECTION_2:
@ -387,14 +406,24 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command)
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
repeats = 10;
timeDelayUs = DSHOT_COMMAND_DELAY_US;
break;
case DSHOT_CMD_BEACON1:
case DSHOT_CMD_BEACON2:
case DSHOT_CMD_BEACON3:
case DSHOT_CMD_BEACON4:
case DSHOT_CMD_BEACON5:
repeats = 1;
timeDelayUs = DSHOT_BEEP_DELAY_US;
break;
default:
repeats = 1;
break;
}
if (blocking) {
for (; repeats; repeats--) {
for (uint8_t i = 0; i < motorCount; i++) {
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
if ((i == index) || (index == ALL_MOTORS)) {
motorDmaOutput_t *const motor = getMotorDmaOutput(i);
motor->requestTelemetry = true;
@ -403,9 +432,55 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command)
}
pwmCompleteDshotMotorUpdate(0);
delay(1);
}
delayMicroseconds(timeDelayUs);
} else {
for (uint8_t i = 0; i < motorCount; i++) {
if ((i == index) || (index == ALL_MOTORS)) {
if (dshotCommandControl.command[i] == 0) {
dshotCommandControl.command[i] = command;
dshotCommandControl.repeats = repeats;
dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US;
dshotCommandControl.delayAfterCommand = timeDelayUs;
}
}
}
}
}
FAST_RAM bool pwmIsProcessingDshotCommand(void)
{
return dshotCommandControl.nextCommandAt;
}
uint8_t pwmGetDshotCommand(uint8_t index)
{
return dshotCommandControl.command[index];
}
bool pwmProcessDshotCommand(uint8_t motorCount)
{
timeUs_t timeNowUs = micros();
if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) {
return true; //Skip motor update because it isn't time yet for a new command
}
//Timed motor update happening with dshot command
if (dshotCommandControl.repeats > 0) {
dshotCommandControl.repeats--;
dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US;
if (dshotCommandControl.repeats == 0) {
dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand;
}
} else {
for (uint8_t i = 0; i < motorCount; i++) {
dshotCommandControl.command[i] = 0;
}
dshotCommandControl.nextCommandAt = 0;
dshotCommandControl.delayAfterCommand = 0;
}
return false;
}
FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)

7
src/main/drivers/pwm_output.h
View file

@ -196,10 +196,15 @@ uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
extern loadDmaBufferFn *loadDmaBuffer;
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command);
void pwmWriteDshotCommandControl(uint8_t index);
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
bool pwmIsProcessingDshotCommand(void);
uint8_t pwmGetDshotCommand(uint8_t index);
bool pwmProcessDshotCommand(uint8_t motorCount);
#endif
#ifdef USE_BEEPER

14
src/main/drivers/pwm_output_dshot.c
View file

@ -37,6 +37,7 @@
#endif
#include "pwm_output.h"
#include "drivers/nvic.h"
#include "drivers/time.h"
#include "dma.h"
#include "rcc.h"
@ -57,7 +58,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
}
}
dmaMotorTimers[dmaMotorTimerCount++].timer = timer;
return dmaMotorTimerCount-1;
return dmaMotorTimerCount - 1;
}
void pwmWriteDshotInt(uint8_t index, uint16_t value)
@ -68,6 +69,12 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
return;
}
/*If there is a command ready to go overwrite the value and send that instead*/
if (pwmIsProcessingDshotCommand()) {
value = pwmGetDshotCommand(index);
motor->requestTelemetry = true;
}
uint16_t packet = prepareDshotPacket(motor, value);
uint8_t bufferSize;
@ -89,6 +96,11 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
{
UNUSED(motorCount);
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (pwmProcessDshotCommand(motorCount)) {
return; //Skip motor update
}
for (int i = 0; i < dmaMotorTimerCount; i++) {
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {

13
src/main/drivers/pwm_output_dshot_hal.c
View file

@ -30,6 +30,7 @@
#include "timer.h"
#include "pwm_output.h"
#include "drivers/nvic.h"
#include "drivers/time.h"
#include "dma.h"
#include "rcc.h"
@ -61,6 +62,12 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
return;
}
/*If there is a command ready to go overwrite the value and send that instead*/
if (pwmIsProcessingDshotCommand()) {
value = pwmGetDshotCommand(index);
motor->requestTelemetry = true;
}
uint16_t packet = prepareDshotPacket(motor, value);
uint8_t bufferSize;
@ -82,6 +89,12 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
{
UNUSED(motorCount);
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (pwmProcessDshotCommand(motorCount)) {
return; //Skip motor update
}
for (int i = 0; i < dmaMotorTimerCount; i++) {
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {

14
src/main/fc/fc_core.c
View file

@ -329,10 +329,7 @@ void disarm(void)
if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && !feature(FEATURE_3D)) {
flipOverAfterCrashMode = false;
if (!feature(FEATURE_3D)) {
pwmDisableMotors();
delay(1);
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL);
pwmEnableMotors();
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
}
#endif
@ -357,13 +354,10 @@ void tryArm(void)
}
#ifdef USE_DSHOT
if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
pwmDisableMotors();
delay(1);
if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
flipOverAfterCrashMode = false;
if (!feature(FEATURE_3D)) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL);
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
} else {
flipOverAfterCrashMode = true;
@ -371,11 +365,9 @@ void tryArm(void)
runawayTakeoffCheckDisabled = false;
#endif
if (!feature(FEATURE_3D)) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED);
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
}
}
pwmEnableMotors();
}
#endif

7
src/main/interface/cli.c
View file

@ -2891,7 +2891,7 @@ static void executeEscInfoCommand(uint8_t escIndex)
startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE);
pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO);
pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true);
delay(10);
@ -2938,7 +2938,7 @@ static void cliDshotProg(char *cmdline)
}
if (command != DSHOT_CMD_ESC_INFO) {
pwmWriteDshotCommand(escIndex, getMotorCount(), command);
pwmWriteDshotCommand(escIndex, getMotorCount(), command, true);
} else {
#if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO)
if (feature(FEATURE_ESC_SENSOR)) {
@ -2958,9 +2958,6 @@ static void cliDshotProg(char *cmdline)
cliPrintLinef("Command Sent: %d", command);
if (command <= 5) {
delay(20); // wait for sound output to finish
}
} else {
cliPrintErrorLinef("Invalid command. Range: 1 - %d.", DSHOT_MIN_THROTTLE - 1);
}

7
src/main/io/beeper.c
View file

@ -394,12 +394,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
if (!areMotorsRunning()
&& ((currentBeeperEntry->mode == BEEPER_RX_SET && beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET))
|| (currentBeeperEntry->mode == BEEPER_RX_LOST && beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_LOST)))) {
pwmDisableMotors();
delay(1);
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone);
pwmEnableMotors();
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false);
}
#endif