Added updating of scheduled command time when motors become idle.
This commit is contained in:
parent
d49948f1fd
commit
7f107e2d9b
|
@ -37,13 +37,14 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL;
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
|
FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
|
||||||
|
#define DSHOT_INITIAL_DELAY_US 10000
|
||||||
#define DSHOT_COMMAND_DELAY_US 1000
|
#define DSHOT_COMMAND_DELAY_US 1000
|
||||||
#define DSHOT_ESCINFO_DELAY_US 5000
|
#define DSHOT_ESCINFO_DELAY_US 12000
|
||||||
#define DSHOT_BEEP_DELAY_US 100000
|
#define DSHOT_BEEP_DELAY_US 100000
|
||||||
|
|
||||||
typedef struct dshotCommandControl_s {
|
typedef struct dshotCommandControl_s {
|
||||||
timeUs_t nextCommandAt;
|
timeUs_t nextCommandAtUs;
|
||||||
timeUs_t delayAfterCommand;
|
timeUs_t delayAfterCommandUs;
|
||||||
bool waitingForIdle;
|
bool waitingForIdle;
|
||||||
uint8_t repeats;
|
uint8_t repeats;
|
||||||
uint8_t command[MAX_SUPPORTED_MOTORS];
|
uint8_t command[MAX_SUPPORTED_MOTORS];
|
||||||
|
@ -387,14 +388,27 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool allMotorsAreIdle(uint8_t motorCount)
|
||||||
|
{
|
||||||
|
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)
|
FAST_CODE bool pwmDshotCommandIsQueued(void)
|
||||||
{
|
{
|
||||||
return dshotCommandControl.nextCommandAt;
|
return dshotCommandControl.nextCommandAtUs;
|
||||||
}
|
}
|
||||||
|
|
||||||
FAST_CODE bool pwmDshotCommandIsProcessing(void)
|
FAST_CODE bool pwmDshotCommandIsProcessing(void)
|
||||||
{
|
{
|
||||||
return dshotCommandControl.nextCommandAt && !dshotCommandControl.waitingForIdle;
|
return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle;
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
|
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
|
||||||
|
@ -406,7 +420,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t repeats = 1;
|
uint8_t repeats = 1;
|
||||||
timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US;
|
timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
|
||||||
|
|
||||||
switch (command) {
|
switch (command) {
|
||||||
case DSHOT_CMD_SPIN_DIRECTION_1:
|
case DSHOT_CMD_SPIN_DIRECTION_1:
|
||||||
|
@ -417,7 +431,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
|
||||||
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
|
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
|
||||||
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
|
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
|
||||||
repeats = 10;
|
repeats = 10;
|
||||||
timeDelayUs = DSHOT_COMMAND_DELAY_US;
|
delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
|
||||||
break;
|
break;
|
||||||
case DSHOT_CMD_BEACON1:
|
case DSHOT_CMD_BEACON1:
|
||||||
case DSHOT_CMD_BEACON2:
|
case DSHOT_CMD_BEACON2:
|
||||||
|
@ -425,13 +439,14 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
|
||||||
case DSHOT_CMD_BEACON4:
|
case DSHOT_CMD_BEACON4:
|
||||||
case DSHOT_CMD_BEACON5:
|
case DSHOT_CMD_BEACON5:
|
||||||
repeats = 1;
|
repeats = 1;
|
||||||
timeDelayUs = DSHOT_BEEP_DELAY_US;
|
delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (blocking) {
|
if (blocking) {
|
||||||
|
delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
|
||||||
for (; repeats; repeats--) {
|
for (; repeats; repeats--) {
|
||||||
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
|
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
|
||||||
|
|
||||||
|
@ -445,17 +460,20 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
|
||||||
|
|
||||||
pwmCompleteDshotMotorUpdate(0);
|
pwmCompleteDshotMotorUpdate(0);
|
||||||
}
|
}
|
||||||
delayMicroseconds(timeDelayUs);
|
delayMicroseconds(delayAfterCommandUs);
|
||||||
} else {
|
} else {
|
||||||
dshotCommandControl.repeats = repeats;
|
dshotCommandControl.repeats = repeats;
|
||||||
dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US;
|
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
|
||||||
dshotCommandControl.delayAfterCommand = timeDelayUs;
|
dshotCommandControl.delayAfterCommandUs = delayAfterCommandUs;
|
||||||
dshotCommandControl.waitingForIdle = true;
|
|
||||||
for (unsigned i = 0; i < motorCount; i++) {
|
for (unsigned i = 0; i < motorCount; i++) {
|
||||||
if (index == i || index == ALL_MOTORS) {
|
if (index == i || index == ALL_MOTORS) {
|
||||||
dshotCommandControl.command[i] = command;
|
dshotCommandControl.command[i] = command;
|
||||||
|
} else {
|
||||||
|
dshotCommandControl.command[i] = command;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -469,15 +487,8 @@ FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
|
||||||
timeUs_t timeNowUs = micros();
|
timeUs_t timeNowUs = micros();
|
||||||
|
|
||||||
if (dshotCommandControl.waitingForIdle) {
|
if (dshotCommandControl.waitingForIdle) {
|
||||||
bool allMotorsIdle = true;
|
if (allMotorsAreIdle(motorCount)) {
|
||||||
for (unsigned i = 0; i < motorCount; i++) {
|
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
|
||||||
const motorDmaOutput_t *motor = getMotorDmaOutput(i);
|
|
||||||
if (motor->value) {
|
|
||||||
allMotorsIdle = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (allMotorsIdle) {
|
|
||||||
dshotCommandControl.waitingForIdle = false;
|
dshotCommandControl.waitingForIdle = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -485,24 +496,22 @@ FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) {
|
if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAtUs) < 0) {
|
||||||
//Skip motor update because it isn't time yet for a new command
|
//Skip motor update because it isn't time yet for a new command
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
//Timed motor update happening with dshot command
|
//Timed motor update happening with dshot command
|
||||||
if (dshotCommandControl.repeats > 0) {
|
if (dshotCommandControl.repeats > 0) {
|
||||||
dshotCommandControl.repeats--;
|
if (dshotCommandControl.repeats > 1) {
|
||||||
dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US;
|
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
|
||||||
if (dshotCommandControl.repeats == 0) {
|
|
||||||
dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand;
|
|
||||||
}
|
|
||||||
} else {
|
} else {
|
||||||
for (unsigned i = 0; i < motorCount; i++) {
|
dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs;
|
||||||
dshotCommandControl.command[i] = 0;
|
|
||||||
}
|
}
|
||||||
dshotCommandControl.nextCommandAt = 0;
|
|
||||||
dshotCommandControl.delayAfterCommand = 0;
|
dshotCommandControl.repeats--;
|
||||||
|
} else {
|
||||||
|
dshotCommandControl.nextCommandAtUs = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|
Loading…
Reference in New Issue