rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6130 from mikeller/fix_dshot_commands_2 

Stop Dshot commands from running when ESCs not disarmed.
This commit is contained in:
Michael Keller 2018-06-17 12:05:20 +12:00 committed by GitHub
parent 319b85777b f067324844
commit 9c55617f05
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 91 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
src/main/drivers/pwm_output.c
View File

@ -37,18 +37,20 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL;
#ifdef USE_DSHOT
FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
#define DSHOT_INITIAL_DELAY_US 10000
#define DSHOT_COMMAND_DELAY_US 1000
#define DSHOT_ESCINFO_DELAY_US 5000
#define DSHOT_ESCINFO_DELAY_US 12000
#define DSHOT_BEEP_DELAY_US 100000
typedef struct dshotCommandControl_s {
timeUs_t nextCommandAt;
timeUs_t delayAfterCommand;
timeUs_t nextCommandAtUs;
timeUs_t delayAfterCommandUs;
bool waitingForIdle;
uint8_t repeats;
uint8_t command[MAX_SUPPORTED_MOTORS];
uint8_t repeats;
} dshotCommandControl_t;
dshotCommandControl_t dshotCommandControl;
static dshotCommandControl_t dshotCommandControl;
#endif
#ifdef USE_SERVOS
@ -386,21 +388,39 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
}
}
FAST_CODE bool pwmIsProcessingDshotCommand(void)
bool allMotorsAreIdle(uint8_t motorCount)
{
return dshotCommandControl.nextCommandAt;
bool allMotorsIdle = true;
for (unsigned i = 0; i < motorCount; i++) {
const motorDmaOutput_t *motor = getMotorDmaOutput(i);
if (motor->value) {
allMotorsIdle = false;
}
}
return allMotorsIdle;
}
FAST_CODE bool pwmDshotCommandIsQueued(void)
{
return dshotCommandControl.nextCommandAtUs;
}
FAST_CODE bool pwmDshotCommandIsProcessing(void)
{
return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0;
}
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
{
timeUs_t timeNowUs = micros();
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND)) {
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) {
return;
}
uint8_t repeats = 1;
timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US;
timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
switch (command) {
case DSHOT_CMD_SPIN_DIRECTION_1:
@ -411,24 +431,24 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
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;
delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
break;
default:
break;
}
if (blocking) {
delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
for (; repeats; repeats--) {
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
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;
@ -438,18 +458,20 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
pwmCompleteDshotMotorUpdate(0);
}
delayMicroseconds(timeDelayUs);
delayMicroseconds(delayAfterCommandUs);
} else {
if (!pwmIsProcessingDshotCommand()) {
for (uint8_t i = 0; i < motorCount; i++) {
if ((index == i) || (index == ALL_MOTORS)) {
dshotCommandControl.command[i] = command;
dshotCommandControl.repeats = repeats;
dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US;
dshotCommandControl.delayAfterCommand = timeDelayUs;
}
dshotCommandControl.repeats = repeats;
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
dshotCommandControl.delayAfterCommandUs = delayAfterCommandUs;
for (unsigned i = 0; i < motorCount; i++) {
if (index == i || index == ALL_MOTORS) {
dshotCommandControl.command[i] = command;
} else {
dshotCommandControl.command[i] = command;
}
}
dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount);
}
}
@ -458,34 +480,44 @@ uint8_t pwmGetDshotCommand(uint8_t index)
return dshotCommandControl.command[index];
}
FAST_CODE_NOINLINE bool pwmProcessDshotCommand(uint8_t motorCount)
FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
{
timeUs_t timeNowUs = micros();
if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) {
return false; //Skip motor update because it isn't time yet for a new command
if (dshotCommandControl.waitingForIdle) {
if (allMotorsAreIdle(motorCount)) {
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
dshotCommandControl.waitingForIdle = false;
}
// Send normal motor output while waiting for motors to go idle
return true;
}
if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAtUs) < 0) {
//Skip motor update because it isn't time yet for a new command
return false;
}
//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;
if (dshotCommandControl.repeats > 0) {
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
} else {
dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs;
}
} else {
for (uint8_t i = 0; i < motorCount; i++) {
dshotCommandControl.command[i] = 0;
}
dshotCommandControl.nextCommandAt = 0;
dshotCommandControl.delayAfterCommand = 0;
dshotCommandControl.nextCommandAtUs = 0;
}
return true;
}
FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor)
{
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
uint16_t packet = (motor->value << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum

8
src/main/drivers/pwm_output.h
View File

@ -191,7 +191,7 @@ bool isMotorProtocolDshot(void);
#ifdef USE_DSHOT
typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor);
extern loadDmaBufferFn *loadDmaBuffer;
@ -201,9 +201,11 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
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);
bool pwmDshotCommandIsQueued(void);
bool pwmDshotCommandIsProcessing(void);
uint8_t pwmGetDshotCommand(uint8_t index);
bool pwmProcessDshotCommand(uint8_t motorCount);
bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount);
#endif

16
src/main/drivers/pwm_output_dshot.c
View File

@ -70,12 +70,16 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
/*If there is a command ready to go overwrite the value and send that instead*/
if (pwmIsProcessingDshotCommand()) {
if (pwmDshotCommandIsProcessing()) {
value = pwmGetDshotCommand(index);
motor->requestTelemetry = true;
if (value) {
motor->requestTelemetry = true;
}
}
uint16_t packet = prepareDshotPacket(motor, value);
motor->value = value;
uint16_t packet = prepareDshotPacket(motor);
uint8_t bufferSize;
#ifdef USE_DSHOT_DMAR
@ -97,8 +101,8 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
UNUSED(motorCount);
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (pwmIsProcessingDshotCommand()) {
if (!pwmProcessDshotCommand(motorCount)) {
if (pwmDshotCommandIsQueued()) {
if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
return;
}
}

14
src/main/drivers/pwm_output_dshot_hal.c
View File

@ -63,12 +63,16 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
/*If there is a command ready to go overwrite the value and send that instead*/
if (pwmIsProcessingDshotCommand()) {
if (pwmDshotCommandIsProcessing()) {
value = pwmGetDshotCommand(index);
motor->requestTelemetry = true;
if (value) {
motor->requestTelemetry = true;
}
}
uint16_t packet = prepareDshotPacket(motor, value);
motor->value = value;
uint16_t packet = prepareDshotPacket(motor);
uint8_t bufferSize;
#ifdef USE_DSHOT_DMAR
@ -90,8 +94,8 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
UNUSED(motorCount);
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (pwmIsProcessingDshotCommand()) {
if (!pwmProcessDshotCommand(motorCount)) {
if (pwmDshotCommandIsQueued()) {
if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
return;
}
}