Skip to content

Commit

Permalink
Updating backport
Browse files Browse the repository at this point in the history
  • Loading branch information
haslinghuis committed Jun 19, 2023
1 parent b1b881b commit 9b7d166
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 10 deletions.
14 changes: 7 additions & 7 deletions src/main/drivers/dshot_command.c
Expand Up @@ -182,7 +182,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot

uint8_t repeats = 1;
timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
motorVTable_t vTable = motorGetVTable();
motorVTable_t *vTable = motorGetVTable();

switch (command) {
case DSHOT_CMD_SPIN_DIRECTION_1:
Expand Down Expand Up @@ -218,27 +218,27 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
delayMicroseconds(DSHOT_COMMAND_DELAY_US);

// Initialise the output buffers
if (vTable.updateInit) {
vTable.updateInit();
if (vTable->updateInit) {
vTable->updateInit();
}

for (uint8_t i = 0; i < motorDeviceCount(); i++) {
motorDmaOutput_t *const motor = getMotorDmaOutput(i);
motor->protocolControl.requestTelemetry = true;
vTable.writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
vTable->writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
}

// Don't attempt to write commands to the motors if telemetry is still being received
if (vTable.telemetryWait) {
(void)vTable.telemetryWait();
(void)vTable->telemetryWait();
}

vTable.updateComplete();
vTable->updateComplete();

// Perform the decode of the last data received
// New data will be received once the send of motor data, triggered above, completes
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
vTable.decodeTelemetry();
vTable->decodeTelemetry();
#endif
}
delayMicroseconds(delayAfterCommandUs);
Expand Down
4 changes: 2 additions & 2 deletions src/main/drivers/motor.c
Expand Up @@ -114,9 +114,9 @@ unsigned motorDeviceCount(void)
return motorDevice->count;
}

motorVTable_t motorGetVTable(void)
motorVTable_t *motorGetVTable(void)
{
return motorDevice->vTable;
return &motorDevice->vTable;
}

// This is not motor generic anymore; should be moved to analog pwm module
Expand Down
2 changes: 1 addition & 1 deletion src/main/drivers/motor.h
Expand Up @@ -86,7 +86,7 @@ uint16_t motorConvertToExternal(float motorValue);
struct motorDevConfig_s; // XXX Shouldn't be needed once pwm_output* is really cleaned up.
void motorDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount);
unsigned motorDeviceCount(void);
motorVTable_t motorGetVTable(void);
motorVTable_t *motorGetVTable(void);
bool checkMotorProtocolEnabled(const motorDevConfig_t *motorConfig, bool *protocolIsDshot);
bool isMotorProtocolDshot(void);
bool isMotorProtocolEnabled(void);
Expand Down

0 comments on commit 9b7d166

Please sign in to comment.