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

Add dshot command queue and limit max notch filter frequency

This commit is contained in:
Thorsten Laux 2019-01-12 12:22:14 +01:00
parent 10ae62efed
commit 7859e6d540
8 changed files with 93 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,7 @@ FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
#define DSHOT_COMMAND_DELAY_US 1000
#define DSHOT_ESCINFO_DELAY_US 12000
#define DSHOT_BEEP_DELAY_US 100000
#define DSHOT_MAX_COMMANDS 3
typedef struct dshotCommandControl_s {
timeUs_t nextCommandAtUs;
@ -53,7 +54,9 @@ typedef struct dshotCommandControl_s {
uint8_t command[MAX_SUPPORTED_MOTORS];
} dshotCommandControl_t;
static dshotCommandControl_t dshotCommandControl;
static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
static uint8_t commandQueueHead;
static uint8_t commandQueueTail;
#endif
#ifdef USE_SERVOS
@ -436,21 +439,53 @@ bool allMotorsAreIdle(uint8_t motorCount)
return allMotorsIdle;
}
FAST_CODE bool pwmDshotCommandQueueFull()
{
return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
}
FAST_CODE bool pwmDshotCommandIsQueued(void)
{
return dshotCommandControl.nextCommandAtUs;
return commandQueueHead != commandQueueTail;
}
FAST_CODE bool pwmDshotCommandIsProcessing(void)
{
return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0;
if (!pwmDshotCommandIsQueued()) {
return false;
}
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
return command->nextCommandAtUs && !command->waitingForIdle && command->repeats > 0;
}
FAST_CODE void pwmDshotCommandQueueUpdate(void)
{
if (!pwmDshotCommandIsQueued()) {
return;
}
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
if (!command->nextCommandAtUs && !command->waitingForIdle && !command->repeats) {
commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
}
}
static dshotCommandControl_t* addCommand()
{
int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1);
if (newHead == commandQueueTail) {
return NULL;
}
dshotCommandControl_t* control = &commandQueue[commandQueueHead];
commandQueueHead = newHead;
return control;
}
void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
{
timeUs_t timeNowUs = micros();
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) {
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandQueueFull()) {
return;
}
@ -500,56 +535,60 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
}
delayMicroseconds(delayAfterCommandUs);
} else {
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_t *commandControl = addCommand();
if (commandControl) {
commandControl->repeats = repeats;
commandControl->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
commandControl->delayAfterCommandUs = delayAfterCommandUs;
for (unsigned i = 0; i < motorCount; i++) {
if (index == i || index == ALL_MOTORS) {
commandControl->command[i] = command;
} else {
commandControl->command[i] = command;
}
}
commandControl->waitingForIdle = !allMotorsAreIdle(motorCount);
}
dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount);
}
}
uint8_t pwmGetDshotCommand(uint8_t index)
{
return dshotCommandControl.command[index];
return commandQueue[commandQueueTail].command[index];
}
FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
{
timeUs_t timeNowUs = micros();
if (dshotCommandControl.waitingForIdle) {
if (allMotorsAreIdle(motorCount)) {
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
dshotCommandControl.waitingForIdle = false;
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
if (pwmDshotCommandIsQueued()) {
if (command->waitingForIdle) {
if (allMotorsAreIdle(motorCount)) {
command->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
command->waitingForIdle = false;
}
// Send normal motor output while waiting for motors to go idle
return true;
}
// Send normal motor output while waiting for motors to go idle
return true;
}
if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAtUs) < 0) {
if (cmpTimeUs(timeNowUs, command->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--;
if (command->repeats > 0) {
command->repeats--;
if (dshotCommandControl.repeats > 0) {
dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
if (command->repeats > 0) {
command->nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
} else {
dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs;
command->nextCommandAtUs = timeNowUs + command->delayAfterCommandUs;
}
} else {
dshotCommandControl.nextCommandAtUs = 0;
command->nextCommandAtUs = 0;
}
return true;

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

@ -237,6 +237,7 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount);
#endif
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
void pwmDshotCommandQueueUpdate(void);
bool pwmDshotCommandIsQueued(void);
bool pwmDshotCommandIsProcessing(void);
uint8_t pwmGetDshotCommand(uint8_t index);

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

@ -324,6 +324,7 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
dmaMotorTimers[i].timerDmaSources = 0;
}
}
pwmDshotCommandQueueUpdate();
}
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)

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

@ -120,6 +120,7 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
dmaMotorTimers[i].timerDmaSources = 0;
}
}
pwmDshotCommandQueueUpdate();
}
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)

17
src/main/fc/core.c
View File

@ -382,14 +382,6 @@ void tryArm(void)
const timeUs_t currentTimeUs = micros();
#ifdef USE_DSHOT
#ifdef USE_DSHOT_TELEMETRY
pwmWriteDshotCommand(
255, getMotorCount(),
motorConfig()->dev.useDshotTelemetry ?
DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY :
DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true);
#endif
if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) {
if (tryingToArm == ARMING_DELAYED_DISARMED) {
if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
@ -404,6 +396,15 @@ void tryArm(void)
}
return;
}
#ifdef USE_DSHOT_TELEMETRY
if (isMotorProtocolDshot()) {
pwmWriteDshotCommand(
255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ?
DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false);
}
#endif
if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
flipOverAfterCrashActive = false;

12
src/main/fc/init.c
View File

@ -214,10 +214,8 @@ void activateDshotTelemetry(struct dispatchEntry_s* self)
if (!ARMING_FLAG(ARMED))
{
pwmWriteDshotCommand(
255, getMotorCount(),
motorConfig()->dev.useDshotTelemetry ?
DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY :
DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true);
255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ?
DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false);
}
}
@ -772,8 +770,10 @@ void init(void)
// TODO: potentially delete when feature is stable. Activation when arming is enough for flight.
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
dispatchEnable();
dispatchAdd(&activateDshotTelemetryEntry, 5000000);
if (motorConfig()->dev.useDshotTelemetry) {
dispatchEnable();
dispatchAdd(&activateDshotTelemetryEntry, 5000000);
}
#endif
fcTasksInit();

3
src/main/sensors/rpm_filter.c
View File

@ -177,6 +177,9 @@ void rpmFilterUpdate()
frequency = currentFilter->minHz;
}
}
if (frequency > deactivateFreq) {
frequency = deactivateFreq;
}
biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above

4
src/main/target/common_post.h
View File

@ -247,3 +247,7 @@
#ifndef USE_CMS
#undef USE_CMS_FAILSAFE_MENU
#endif
#ifndef USE_DSHOT_TELEMETRY
#undef USE_RPM_FILTER
#endif