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

Motor code refactor (Phase 1)

This commit is contained in:
jflyper 2019-06-29 03:30:05 +09:00
parent f4bb75180e
commit 542146c702
41 changed files with 1543 additions and 975 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
make/source.mk
View File

@ -10,6 +10,9 @@ COMMON_SRC = \
cli/cli.c \
cli/settings.c \
drivers/adc.c \
drivers/dshot.c \
drivers/dshot_dpwm.c \
drivers/dshot_command.c \
drivers/buf_writer.c \
drivers/bus.c \
drivers/bus_i2c_config.c \
@ -26,6 +29,7 @@ COMMON_SRC = \
drivers/io.c \
drivers/light_led.c \
drivers/mco.c \
drivers/motor.c \
drivers/pinio.c \
drivers/resource.c \
drivers/rcc.c \

28
src/main/cli/cli.c
View File

@ -71,7 +71,10 @@ uint8_t cliMode = 0;
#include "drivers/io.h"
#include "drivers/io_impl.h"
#include "drivers/light_led.h"
#include "drivers/pwm_output.h"
#include "drivers/motor.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/sdcard.h"
#include "drivers/sensor.h"
@ -1424,9 +1427,11 @@ static void cliSerialPassthrough(char *cmdline)
serialSetCtrlLineStateCb(ports[0].port, cbCtrlLine, (void *)(intptr_t)(port1PinioDtr));
}
// XXX Review ESC pass through under refactored motor handling
#ifdef USE_PWM_OUTPUT
if (escSensorPassthrough) {
pwmDisableMotors();
// pwmDisableMotors();
motorDisable();
delay(5);
unsigned motorsCount = getMotorCount();
for (unsigned i = 0; i < motorsCount; i++) {
@ -3363,7 +3368,7 @@ static void cliRebootEx(rebootTarget_e rebootTarget)
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
waitForSerialPortToFinishTransmitting(cliPort);
stopPwmAllMotors();
motorShutdown();
switch (rebootTarget) {
case REBOOT_TARGET_BOOTLOADER_ROM:
@ -3664,7 +3669,7 @@ static void executeEscInfoCommand(uint8_t escIndex)
startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE);
pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true);
dshotCommandWrite(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true);
delay(10);
@ -3672,6 +3677,9 @@ static void executeEscInfoCommand(uint8_t escIndex)
}
#endif // USE_ESC_SENSOR && USE_ESC_SENSOR_INFO
// XXX Review dshotprog command under refactored motor handling
static void cliDshotProg(char *cmdline)
{
if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) {
@ -3699,7 +3707,8 @@ static void cliDshotProg(char *cmdline)
int command = atoi(pch);
if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
if (firstCommand) {
pwmDisableMotors();
// pwmDisableMotors();
motorDisable();
if (command == DSHOT_CMD_ESC_INFO) {
delay(5); // Wait for potential ESC telemetry transmission to finish
@ -3711,7 +3720,7 @@ static void cliDshotProg(char *cmdline)
}
if (command != DSHOT_CMD_ESC_INFO) {
pwmWriteDshotCommand(escIndex, getMotorCount(), command, true);
dshotCommandWrite(escIndex, getMotorCount(), command, true);
} else {
#if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO)
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
@ -3743,7 +3752,7 @@ static void cliDshotProg(char *cmdline)
pch = strtok_r(NULL, " ", &saveptr);
}
pwmEnableMotors();
motorEnable();
}
#endif // USE_DSHOT
@ -3875,7 +3884,7 @@ static void cliMotor(char *cmdline)
if (motorValue < PWM_RANGE_MIN || motorValue > PWM_RANGE_MAX) {
cliShowArgumentRangeError("VALUE", 1000, 2000);
} else {
uint32_t motorOutputValue = convertExternalToMotor(motorValue);
uint32_t motorOutputValue = motorConvertFromExternal(motorValue);
if (motorIndex != ALL_MOTORS) {
motor_disarmed[motorIndex] = motorOutputValue;
@ -5294,7 +5303,6 @@ static void cliDma(char* cmdline)
}
#endif
static void cliResource(char *cmdline)
{
char *pch = NULL;
@ -5910,7 +5918,7 @@ static void cliMsc(char *cmdline)
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
waitForSerialPortToFinishTransmitting(cliPort);
stopPwmAllMotors();
motorShutdown();
systemResetToMsc(timezoneOffsetMinutes);
} else {

3
src/main/cli/settings.c
View File

@ -35,6 +35,7 @@
#include "drivers/adc.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/dshot_command.h"
#include "drivers/camera_control.h"
#include "drivers/light_led.h"
#include "drivers/pinio.h"
@ -580,7 +581,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug),
LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource)
LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
};
#undef LOOKUP_TABLE_ENTRY

1
src/main/cli/settings.h
View File

@ -135,6 +135,7 @@ typedef enum {
#endif
TABLE_GYRO_FILTER_DEBUG,
TABLE_POSITION_ALT_SOURCE,
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;

3
src/main/cms/cms.c
View File

@ -50,6 +50,7 @@
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/motor.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/feature.h"
@ -808,7 +809,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
stopPwmAllMotors();
motorShutdown();
delay(200);
systemReset();

134
src/main/drivers/dshot.c Normal file
View File

@ -0,0 +1,134 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: jflyper
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#ifdef USE_DSHOT
#include "build/atomic.h"
#include "common/maths.h"
#include "common/time.h"
#include "config/feature.h"
#include "drivers/motor.h"
#include "drivers/timer.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h" // for motorDmaOutput_t, should be gone
#include "drivers/dshot_command.h"
#include "drivers/nvic.h"
#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
#include "fc/rc_controls.h" // for flight3DConfig_t
#include "pg/motor.h"
#include "rx/rx.h"
void dshotInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) {
float outputLimitOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) * (1 - outputLimit);
*disarm = DSHOT_CMD_MOTOR_STOP;
if (featureIsEnabled(FEATURE_3D)) {
*outputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
*outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
*deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
*deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
} else {
*outputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
*outputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
}
}
float dshotConvertFromExternal(uint16_t externalValue)
{
uint16_t motorValue;
externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
if (featureIsEnabled(FEATURE_3D)) {
if (externalValue == PWM_RANGE_MIDDLE) {
motorValue = DSHOT_CMD_MOTOR_STOP;
} else if (externalValue < PWM_RANGE_MIDDLE) {
motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MIDDLE - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
} else {
motorValue = scaleRange(externalValue, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
} else {
motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
return (float)motorValue;
}
uint16_t dshotConvertToExternal(float motorValue)
{
uint16_t externalValue;
if (featureIsEnabled(FEATURE_3D)) {
if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
externalValue = PWM_RANGE_MIDDLE;
} else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MIDDLE - 1, PWM_RANGE_MIN);
} else {
externalValue = scaleRange(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIDDLE + 1, PWM_RANGE_MAX);
}
} else {
externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
}
return externalValue;
}
FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb)
{
uint16_t packet;
ATOMIC_BLOCK(NVIC_PRIO_DSHOT_DMA) {
packet = (pcb->value << 1) | (pcb->requestTelemetry ? 1 : 0);
pcb->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
}
// compute checksum
unsigned csum = 0;
unsigned csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
// append checksum
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
csum = ~csum;
}
#endif
csum &= 0xf;
packet = (packet << 4) | csum;
return packet;
}
#endif // USE_DSHOT

47
src/main/drivers/dshot.h Normal file
View File

@ -0,0 +1,47 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define DSHOT_MIN_THROTTLE 48
#define DSHOT_MAX_THROTTLE 2047
#define DSHOT_3D_FORWARD_MIN_THROTTLE 1048
typedef struct dshotProtocolControl_s {
uint16_t value;
bool requestTelemetry;
} dshotProtocolControl_t;
void dshotInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow);
float dshotConvertFromExternal(uint16_t externalValue);
uint16_t dshotConvertToExternal(float motorValue);
FAST_CODE uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb);
#ifdef USE_DSHOT_TELEMETRY
extern bool useDshotTelemetry;
extern uint32_t dshotInvalidPacketCount;
#endif
uint16_t getDshotTelemetry(uint8_t index);
bool isDshotMotorTelemetryActive(uint8_t motorIndex);
bool isDshotTelemetryActive(void);
int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);

298
src/main/drivers/dshot_command.c Normal file
View File

@ -0,0 +1,298 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#ifdef USE_DSHOT
#include "common/time.h"
#include "drivers/io.h"
#include "drivers/motor.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "drivers/pwm_output.h"
#define DSHOT_INITIAL_DELAY_US 10000
#define DSHOT_COMMAND_DELAY_US 1000
#define DSHOT_ESCINFO_DELAY_US 12000
#define DSHOT_BEEP_DELAY_US 100000
#define DSHOT_MAX_COMMANDS 3
typedef enum {
DSHOT_COMMAND_STATE_IDLEWAIT, // waiting for motors to go idle
DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands
DSHOT_COMMAND_STATE_ACTIVE, // actively sending the command (with optional repeated output)
DSHOT_COMMAND_STATE_POSTDELAY // delay period after the command has been sent
} dshotCommandState_e;
typedef struct dshotCommandControl_s {
dshotCommandState_e state;
uint32_t nextCommandCycleDelay;
timeUs_t delayAfterCommandUs;
uint8_t repeats;
uint8_t command[MAX_SUPPORTED_MOTORS];
} dshotCommandControl_t;
static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0
// gets set to the actual value when the PID loop is initialized
// XXX Optimization opportunity here.
// https://github.com/betaflight/betaflight/pull/8534#pullrequestreview-258947278
// @ledvinap: queue entry is quite large - it may be better to handle empty/full queue using different mechanism (magic value for Head or Tail / explicit element count)
// Explicit element count will make code below simpler, but care has to be taken to avoid race conditions
static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
static uint8_t commandQueueHead;
static uint8_t commandQueueTail;
void dshotSetPidLoopTime(uint32_t pidLoopTime)
{
dshotCommandPidLoopTimeUs = pidLoopTime;
}
static FAST_CODE bool dshotCommandQueueFull()
{
return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
}
FAST_CODE bool dshotCommandQueueEmpty(void)
{
return commandQueueHead == commandQueueTail;
}
static FAST_CODE bool isLastDshotCommand(void)
{
return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead);
}
FAST_CODE bool dshotCommandIsProcessing(void)
{
if (dshotCommandQueueEmpty()) {
return false;
}
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY
|| command->state == DSHOT_COMMAND_STATE_ACTIVE
|| (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand());
return commandIsProcessing;
}
static FAST_CODE bool dshotCommandQueueUpdate(void)
{
if (!dshotCommandQueueEmpty()) {
commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
if (!dshotCommandQueueEmpty()) {
// There is another command in the queue so update it so it's ready to output in
// sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass
// the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states.
dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail];
nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE;
nextCommand->nextCommandCycleDelay = 0;
return true;
}
}
return false;
}
static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs)
{
// Find the minimum number of motor output cycles needed to
// provide at least delayUs time delay
return (delayUs + dshotCommandPidLoopTimeUs - 1) / dshotCommandPidLoopTimeUs;
}
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;
}
static bool allMotorsAreIdle(void)
{
for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
const motorDmaOutput_t *motor = getMotorDmaOutput(i);
if (motor->protocolControl.value) {
return false;
}
}
return true;
}
void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
{
UNUSED(motorCount);
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) {
return;
}
uint8_t repeats = 1;
timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
switch (command) {
case DSHOT_CMD_SPIN_DIRECTION_1:
case DSHOT_CMD_SPIN_DIRECTION_2:
case DSHOT_CMD_3D_MODE_OFF:
case DSHOT_CMD_3D_MODE_ON:
case DSHOT_CMD_SAVE_SETTINGS:
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
case DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE:
case DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY:
repeats = 10;
break;
case DSHOT_CMD_BEACON1:
case DSHOT_CMD_BEACON2:
case DSHOT_CMD_BEACON3:
case DSHOT_CMD_BEACON4:
case DSHOT_CMD_BEACON5:
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);
#ifdef USE_DSHOT_TELEMETRY
timeUs_t timeoutUs = micros() + 1000;
while (!pwmStartDshotMotorUpdate() &&
cmpTimeUs(timeoutUs, micros()) > 0);
#endif
for (uint8_t i = 0; i < dshotPwmDevice.count; i++) {
if ((i == index) || (index == ALL_MOTORS)) {
motorDmaOutput_t *const motor = getMotorDmaOutput(i);
motor->protocolControl.requestTelemetry = true;
dshotPwmDevice.vTable.writeInt(i, command);
}
}
dshotPwmDevice.vTable.updateComplete();
}
delayMicroseconds(delayAfterCommandUs);
} else {
dshotCommandControl_t *commandControl = addCommand();
if (commandControl) {
commandControl->repeats = repeats;
commandControl->delayAfterCommandUs = delayAfterCommandUs;
for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
if (index == i || index == ALL_MOTORS) {
commandControl->command[i] = command;
} else {
commandControl->command[i] = DSHOT_CMD_MOTOR_STOP;
}
}
if (allMotorsAreIdle()) {
// we can skip the motors idle wait state
commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
} else {
commandControl->state = DSHOT_COMMAND_STATE_IDLEWAIT;
commandControl->nextCommandCycleDelay = 0; // will be set after idle wait completes
}
}
}
}
uint8_t pwmGetDshotCommand(uint8_t index)
{
return commandQueue[commandQueueTail].command[index];
}
// This function is used to synchronize the dshot command output timing with
// the normal motor output timing tied to the PID loop frequency. A "true" result
// allows the motor output to be sent, "false" means delay until next loop. So take
// the example of a dshot command that needs to repeat 10 times at 1ms intervals.
// If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output.
FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(uint8_t motorCount)
{
UNUSED(motorCount);
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
switch (command->state) {
case DSHOT_COMMAND_STATE_IDLEWAIT:
if (allMotorsAreIdle()) {
command->state = DSHOT_COMMAND_STATE_STARTDELAY;
command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
}
break;
case DSHOT_COMMAND_STATE_STARTDELAY:
if (command->nextCommandCycleDelay) {
--command->nextCommandCycleDelay;
return false; // Delay motor output until the start of the command seequence
}
command->state = DSHOT_COMMAND_STATE_ACTIVE;
command->nextCommandCycleDelay = 0; // first iteration of the repeat happens now
FALLTHROUGH;
case DSHOT_COMMAND_STATE_ACTIVE:
if (command->nextCommandCycleDelay) {
--command->nextCommandCycleDelay;
return false; // Delay motor output until the next command repeat
}
command->repeats--;
if (command->repeats) {
command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
} else {
command->state = DSHOT_COMMAND_STATE_POSTDELAY;
command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs);
if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) {
// Account for the 1 extra motor output loop between commands.
// Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop.
command->nextCommandCycleDelay--;
}
}
break;
case DSHOT_COMMAND_STATE_POSTDELAY:
if (command->nextCommandCycleDelay) {
--command->nextCommandCycleDelay;
return false; // Delay motor output until the end of the post-command delay
}
if (dshotCommandQueueUpdate()) {
// Will be true if the command queue is not empty and we
// want to wait for the next command to start in sequence.
return false;
}
}
return true;
}
#endif // USE_DSHOT

70
src/main/drivers/dshot_command.h Normal file
View File

@ -0,0 +1,70 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define DSHOT_MAX_COMMAND 47
/*
DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
3D Mode:
0 = stop
48 (low) - 1047 (high) -> negative direction
1048 (low) - 2047 (high) -> positive direction
*/
typedef enum {
DSHOT_CMD_MOTOR_STOP = 0,
DSHOT_CMD_BEACON1,
DSHOT_CMD_BEACON2,
DSHOT_CMD_BEACON3,
DSHOT_CMD_BEACON4,
DSHOT_CMD_BEACON5,
DSHOT_CMD_ESC_INFO, // V2 includes settings
DSHOT_CMD_SPIN_DIRECTION_1,
DSHOT_CMD_SPIN_DIRECTION_2,
DSHOT_CMD_3D_MODE_OFF,
DSHOT_CMD_3D_MODE_ON,
DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
DSHOT_CMD_SAVE_SETTINGS,
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
DSHOT_CMD_LED0_ON, // BLHeli32 only
DSHOT_CMD_LED1_ON, // BLHeli32 only
DSHOT_CMD_LED2_ON, // BLHeli32 only
DSHOT_CMD_LED3_ON, // BLHeli32 only
DSHOT_CMD_LED0_OFF, // BLHeli32 only
DSHOT_CMD_LED1_OFF, // BLHeli32 only
DSHOT_CMD_LED2_OFF, // BLHeli32 only
DSHOT_CMD_LED3_OFF, // BLHeli32 only
DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32,
DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 33,
DSHOT_CMD_MAX = 47
} dshotCommands_e;
void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking);
void dshotSetPidLoopTime(uint32_t pidLoopTime);
bool dshotCommandQueueEmpty(void);
bool dshotCommandIsProcessing(void);
uint8_t pwmGetDshotCommand(uint8_t index);
bool dshotCommandOutputIsEnabled(uint8_t motorCount);

197
src/main/drivers/dshot_dpwm.c Normal file
View File

@ -0,0 +1,197 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: jflyper
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "platform.h"
#ifdef USE_DSHOT
#include "drivers/pwm_output.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/motor.h"
#include "pg/motor.h"
// XXX TODO: Share a single region among dshotDmaBuffer and dshotBurstDmaBuffer
DSHOT_DMA_BUFFER_ATTRIBUTE DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
#ifdef USE_DSHOT_DMAR
DSHOT_DMA_BUFFER_ATTRIBUTE DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
#endif
#ifdef USE_DSHOT_DMAR
FAST_RAM_ZERO_INIT bool useBurstDshot = false;
#endif
#ifdef USE_DSHOT_TELEMETRY
FAST_RAM_ZERO_INIT bool useDshotTelemetry = false;
#endif
FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
{
int i;
for (i = 0; i < 16; i++) {
dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
dmaBuffer[i++ * stride] = 0;
dmaBuffer[i++ * stride] = 0;
return DSHOT_DMA_BUFFER_SIZE;
}
FAST_CODE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
{
int i;
for (i = 0; i < 4; i++) {
dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
dmaBuffer[i++ * stride] = 0;
dmaBuffer[i++ * stride] = 0;
return PROSHOT_DMA_BUFFER_SIZE;
}
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_PROSHOT1000):
return MOTOR_PROSHOT1000_HZ;
case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_HZ;
case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300):
return MOTOR_DSHOT300_HZ;
default:
case(PWM_TYPE_DSHOT150):
return MOTOR_DSHOT150_HZ;
}
}
static void dshotPwmShutdown(void)
{
// DShot signal is only generated if write to motors happen,
// and that is prevented by enabled checking at write.
// Hence there's no special processing required here.
return;
}
static void dshotPwmDisableMotors(void)
{
// No special processing required
return;
}
static bool dshotPwmEnableMotors(void)
{
// No special processing required
return true;
}
static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value)
{
pwmWriteDshotInt(index, value);
}
static FAST_CODE void dshotWrite(uint8_t index, float value)
{
pwmWriteDshotInt(index, lrintf(value));
}
static motorVTable_t dshotPwmVTable = {
.enable = dshotPwmEnableMotors,
.disable = dshotPwmDisableMotors,
.updateStart = motorUpdateStartNull, // May be updated after copying
.write = dshotWrite,
.writeInt = dshotWriteInt,
.updateComplete = pwmCompleteDshotMotorUpdate,
.convertExternalToMotor = dshotConvertFromExternal,
.convertMotorToExternal = dshotConvertToExternal,
.shutdown = dshotPwmShutdown,
};
FAST_RAM_ZERO_INIT motorDevice_t dshotPwmDevice;
motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
{
UNUSED(idlePulse);
UNUSED(useUnsyncedPwm);
dshotPwmDevice.vTable = dshotPwmVTable;
#ifdef USE_DSHOT_TELEMETRY
useDshotTelemetry = motorConfig->useDshotTelemetry;
dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
#endif
switch (motorConfig->motorPwmProtocol) {
case PWM_TYPE_PROSHOT1000:
loadDmaBuffer = loadDmaBufferProshot;
break;
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
loadDmaBuffer = loadDmaBufferDshot;
#ifdef USE_DSHOT_DMAR
if (motorConfig->useBurstDshot) {
useBurstDshot = true;
}
#endif
break;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
const timerHardware_t *timerHardware = timerGetByTag(tag);
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
dshotPwmDevice.vTable.write = motorWriteNull;
dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
/* TODO: block arming and add reason system cannot arm */
return NULL;
}
motors[motorIndex].io = IOGetByTag(tag);
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true;
}
return &dshotPwmDevice;
}
#endif // USE_DSHOT

162
src/main/drivers/dshot_dpwm.h Normal file
View File

@ -0,0 +1,162 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: jflyper
*/
#pragma once
#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 20
#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
#define PROSHOT_BASE_SYMBOL 24 // 1uS
#define PROSHOT_BIT_WIDTH 3
#define MOTOR_NIBBLE_LENGTH_PROSHOT (PROSHOT_BASE_SYMBOL * 4) // 4uS
#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
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
extern FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet);
FAST_CODE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet);
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
struct motorDevConfig_s;
motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm);
/* Motor DMA related, moved from pwm_output.h */
#define MAX_DMA_TIMERS 8
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
#define DSHOT_TELEMETRY_INPUT_LEN 32
#define PROSHOT_TELEMETRY_INPUT_LEN 8
// For H7, DMA buffer is placed in a dedicated segment for coherency management
#ifdef STM32H7
#define DSHOT_DMA_BUFFER_ATTRIBUTE DMA_RAM
#else
#define DSHOT_DMA_BUFFER_ATTRIBUTE // None
#endif
#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
#define DSHOT_DMA_BUFFER_UNIT uint32_t
#else
#define DSHOT_DMA_BUFFER_UNIT uint8_t
#endif
#ifdef USE_DSHOT_TELEMETRY
STATIC_ASSERT(DSHOT_TELEMETRY_INPUT_LEN >= DSHOT_DMA_BUFFER_SIZE, dshotBufferSizeConstrait);
#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_TELEMETRY_INPUT_LEN
#else
#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_DMA_BUFFER_SIZE
#endif
extern DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
#ifdef USE_DSHOT_DMAR
extern DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
#endif
typedef struct {
TIM_TypeDef *timer;
#if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
#if defined(STM32F7) || defined(STM32H7)
TIM_HandleTypeDef timHandle;
DMA_HandleTypeDef hdma_tim;
#endif
#ifdef STM32F3
DMA_Channel_TypeDef *dmaBurstRef;
#else
DMA_Stream_TypeDef *dmaBurstRef;
#endif
uint16_t dmaBurstLength;
uint32_t *dmaBurstBuffer;
timeUs_t inputDirectionStampUs;
#endif
uint16_t timerDmaSources;
} motorDmaTimer_t;
typedef struct motorDmaOutput_s {
dshotProtocolControl_t protocolControl;
ioTag_t ioTag;
const timerHardware_t *timerHardware;
#ifdef USE_DSHOT
uint16_t timerDmaSource;
uint8_t timerDmaIndex;
bool configured;
#ifdef STM32H7
TIM_HandleTypeDef TimHandle;
DMA_HandleTypeDef hdma_tim;
#endif
uint8_t output;
uint8_t index;
#ifdef USE_DSHOT_TELEMETRY
bool useProshot;
volatile bool isInput;
volatile bool hasTelemetry;
uint16_t dshotTelemetryValue;
timeDelta_t dshotTelemetryDeadtimeUs;
bool dshotTelemetryActive;
#ifdef USE_HAL_DRIVER
LL_TIM_OC_InitTypeDef ocInitStruct;
LL_TIM_IC_InitTypeDef icInitStruct;
LL_DMA_InitTypeDef dmaInitStruct;
uint32_t llChannel;
#else
TIM_OCInitTypeDef ocInitStruct;
TIM_ICInitTypeDef icInitStruct;
DMA_InitTypeDef dmaInitStruct;
#endif
uint8_t dmaInputLen;
#endif
#ifdef STM32F3
DMA_Channel_TypeDef *dmaRef;
#else
DMA_Stream_TypeDef *dmaRef;
#endif
#endif
motorDmaTimer_t *timer;
DSHOT_DMA_BUFFER_UNIT *dmaBuffer;
} motorDmaOutput_t;
motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
bool isMotorProtocolDshot(void);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
#ifdef USE_DSHOT_TELEMETRY
bool pwmStartDshotMotorUpdate(void);
#endif
void pwmCompleteDshotMotorUpdate(void);
extern bool useBurstDshot;
extern motorDevice_t dshotPwmDevice;

253
src/main/drivers/motor.c Normal file
View File

@ -0,0 +1,253 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: jflyper
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#ifdef USE_MOTOR
#include "common/maths.h"
#include "config/feature.h"
#include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future
#include "drivers/motor.h"
#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
#include "drivers/time.h"
#include "drivers/dshot_dpwm.h"
#include "fc/rc_controls.h" // for flight3DConfig_t
#include "pg/motor.h"
static FAST_RAM_ZERO_INIT motorDevice_t *motorDevice;
void motorShutdown(void)
{
motorDevice->vTable.shutdown();
motorDevice->enabled = false;
motorDevice->initialized = false;
delayMicroseconds(1500);
}
void motorWriteAll(float *values)
{
#ifdef USE_PWM_OUTPUT
if (motorDevice->enabled) {
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
if (!motorDevice->vTable.updateStart()) {
return;
}
#endif
for (int i = 0; i < motorDevice->count; i++) {
motorDevice->vTable.write(i, values[i]);
}
motorDevice->vTable.updateComplete();
}
#endif
}
int motorCount(void)
{
return motorDevice->count;
}
// This is not motor generic anymore; should be moved to analog pwm module
static void analogInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) {
if (featureIsEnabled(FEATURE_3D)) {
float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - outputLimit) / 2;
*disarm = flight3DConfig()->neutral3d;
*outputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
*outputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
*deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
*deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
} else {
*disarm = motorConfig()->mincommand;
*outputLow = motorConfig()->minthrottle;
*outputHigh = motorConfig()->maxthrottle - ((motorConfig()->maxthrottle - motorConfig()->minthrottle) * (1 - outputLimit));
}
}
// End point initialization is called from mixerInit before motorDevInit; can't use vtable...
void motorInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
{
switch (motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
dshotInitEndpoints(outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
break;
#endif
default:
analogInitEndpoints(outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
break;
}
}
float motorConvertFromExternal(uint16_t externalValue)
{
return motorDevice->vTable.convertExternalToMotor(externalValue);
}
uint16_t motorConvertToExternal(float motorValue)
{
return motorDevice->vTable.convertMotorToExternal(motorValue);
}
static bool isDshot = false; // XXX Should go somewhere else
static bool motorEnableNull(void)
{
return false;
}
static void motorDisableNull(void)
{
}
bool motorUpdateStartNull(void)
{
return true;
}
void motorWriteNull(uint8_t index, float value)
{
UNUSED(index);
UNUSED(value);
}
static void motorWriteIntNull(uint8_t index, uint16_t value)
{
UNUSED(index);
UNUSED(value);
}
void motorUpdateCompleteNull(void)
{
}
static void motorShutdownNull(void)
{
}
static float motorConvertFromExternalNull(uint16_t value)
{
UNUSED(value);
return 0.0f ;
}
static uint16_t motorConvertToExternalNull(float value)
{
UNUSED(value);
return 0;
}
static const motorVTable_t motorNullVTable = {
.enable = motorEnableNull,
.disable = motorDisableNull,
.updateStart = motorUpdateStartNull,
.write = motorWriteNull,
.writeInt = motorWriteIntNull,
.updateComplete = motorUpdateCompleteNull,
.convertExternalToMotor = motorConvertFromExternalNull,
.convertMotorToExternal = motorConvertToExternalNull,
.shutdown = motorShutdownNull,
};
static motorDevice_t motorNullDevice = {
.initialized = false,
.enabled = false,
};
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) {
memset(motors, 0, sizeof(motors));
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
switch (motorConfig->motorPwmProtocol) {
default:
case PWM_TYPE_STANDARD:
case PWM_TYPE_ONESHOT125:
case PWM_TYPE_ONESHOT42:
case PWM_TYPE_MULTISHOT:
case PWM_TYPE_BRUSHED:
motorDevice = motorPwmDevInit(motorConfig, idlePulse, motorCount, useUnsyncedPwm);
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT150:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_PROSHOT1000:
motorDevice = dshotPwmDevInit(motorConfig, idlePulse, motorCount, useUnsyncedPwm);
isDshot = true;
break;
#endif
#if 0 // not yet
case PWM_TYPE_DSHOT_UART:
//motorDevice = dshotSerialInit(motorConfig, idlePulse, motorCount, useUnsyncedPwm);
break;
#endif
}
if (motorDevice) {
motorDevice->count = motorCount;
motorDevice->initialized = true;
motorDevice->enabled = false;
} else {
motorNullDevice.vTable = motorNullVTable;
motorDevice = &motorNullDevice;
}
}
void motorDisable(void)
{
motorDevice->vTable.disable();
motorDevice->enabled = false;
}
void motorEnable(void)
{
if (motorDevice->initialized && motorDevice->vTable.enable()) {
motorDevice->enabled = true;
}
}
bool motorIsEnabled(void)
{
return motorDevice->enabled;
}
bool isMotorProtocolDshot(void)
{
return isDshot;
}
#endif // USE_MOTOR

83
src/main/drivers/motor.h Normal file
View File

@ -0,0 +1,83 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*
* Author: jflyper
*/
#pragma once
typedef enum {
PWM_TYPE_STANDARD = 0,
PWM_TYPE_ONESHOT125,
PWM_TYPE_ONESHOT42,
PWM_TYPE_MULTISHOT,
PWM_TYPE_BRUSHED,
#ifdef USE_DSHOT
PWM_TYPE_DSHOT150,
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT1200,
PWM_TYPE_PROSHOT1000,
#endif
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
typedef struct motorVTable_s {
// Common
float (*convertExternalToMotor)(uint16_t externalValue);
uint16_t (*convertMotorToExternal)(float motorValue);
bool (*enable)(void);
void (*disable)(void);
bool (*updateStart)(void);
void (*write)(uint8_t index, float value);
void (*writeInt)(uint8_t index, uint16_t value);
void (*updateComplete)(void);
void (*shutdown)(void);
// Digital commands
} motorVTable_t;
typedef struct motorDevice_s {
motorVTable_t vTable;
uint8_t count;
bool initialized;
bool enabled;
} motorDevice_t;
void motorWriteNull(uint8_t index, float value);
bool motorUpdateStartNull(void);
void motorUpdateCompleteNull(void);
void motorWriteAll(float *values);
void motorInitEndpoints(float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3DHigh, float *deadbandMotor3DLow);
float motorConvertFromExternal(uint16_t externalValue);
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);
bool isMotorProtocolDshot(void);
void motorDisable(void);
void motorEnable(void);
bool motorIsEnabled(void);
void motorShutdown(void); // Replaces stopPwmAllMotors

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

@ -26,6 +26,7 @@
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increase slightly
#define NVIC_PRIO_DSHOT_DMA NVIC_BUILD_PRIORITY(2, 1)
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)

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

@ -26,77 +26,16 @@
#include "platform.h"
#ifdef USE_PWM_OUTPUT
#include "drivers/time.h"
#include "drivers/io.h"
#include "pwm_output.h"
#include "timer.h"
#include "drivers/motor.h"
#include "drivers/pwm_output.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "pg/motor.h"
static FAST_RAM_ZERO_INIT pwmWriteFn *pwmWrite;
static FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL;
#ifdef USE_DSHOT_TELEMETRY
static FAST_RAM_ZERO_INIT pwmStartWriteFn *pwmStartWrite = NULL;
#endif
#ifdef USE_DSHOT
#ifdef STM32H7
// XXX dshotDmaBuffer can be embedded inside dshotBurstDmaBuffer
DMA_RAM uint32_t dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_SIZE];
#ifdef USE_DSHOT_DMAR
DMA_RAM uint32_t dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
#endif
#endif
FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
#define DSHOT_INITIAL_DELAY_US 10000
#define DSHOT_COMMAND_DELAY_US 1000
#define DSHOT_ESCINFO_DELAY_US 12000
#define DSHOT_BEEP_DELAY_US 100000
#define DSHOT_MAX_COMMANDS 3
typedef enum {
DSHOT_COMMAND_STATE_IDLEWAIT, // waiting for motors to go idle
DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands
DSHOT_COMMAND_STATE_ACTIVE, // actively sending the command (with optional repeated output)
DSHOT_COMMAND_STATE_POSTDELAY // delay period after the command has been sent
} dshotCommandState_e;
typedef struct dshotCommandControl_s {
dshotCommandState_e state;
uint32_t nextCommandCycleDelay;
timeUs_t delayAfterCommandUs;
uint8_t repeats;
uint8_t command[MAX_SUPPORTED_MOTORS];
} dshotCommandControl_t;
static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0
// gets set to the actual value when the PID loop is initialized
static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
static uint8_t commandQueueHead;
static uint8_t commandQueueTail;
#endif
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#endif
#ifdef USE_BEEPER
static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep = 0;
#endif
static bool pwmMotorsEnabled = false;
static bool isDshot = false;
#ifdef USE_DSHOT_DMAR
FAST_RAM_ZERO_INIT bool useBurstDshot = false;
#endif
#ifdef USE_DSHOT_TELEMETRY
FAST_RAM_ZERO_INIT bool useDshotTelemetry = false;
#endif
FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@ -169,6 +108,8 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
*channel->ccr = 0;
}
static FAST_RAM_ZERO_INIT motorDevice_t motorPwmDevice;
static void pwmWriteUnused(uint8_t index, float value)
{
UNUSED(index);
@ -181,50 +122,9 @@ static void pwmWriteStandard(uint8_t index, float value)
*motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
}
#ifdef USE_DSHOT
static FAST_CODE void pwmWriteDshot(uint8_t index, float value)
void pwmShutdownPulsesForAllMotors(void)
{
pwmWriteDshotInt(index, lrintf(value));
}
static FAST_CODE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
{
for (int i = 0; i < 16; i++) {
dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
dmaBuffer[16 * stride] = 0;
dmaBuffer[17 * stride] = 0;
return DSHOT_DMA_BUFFER_SIZE;
}
static uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
{
for (int i = 0; i < 4; i++) {
dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
dmaBuffer[4 * stride] = 0;
dmaBuffer[5 * stride] = 0;
return PROSHOT_DMA_BUFFER_SIZE;
}
void setDshotPidLoopTime(uint32_t pidLoopTime)
{
dshotCommandPidLoopTimeUs = pidLoopTime;
}
#endif
void pwmWriteMotor(uint8_t index, float value)
{
pwmWrite(index, value);
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
for (int index = 0; index < motorPwmDevice.count; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
if (motors[index].channel.ccr) {
*motors[index].channel.ccr = 0;
@ -234,37 +134,19 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
void pwmDisableMotors(void)
{
pwmShutdownPulsesForAllMotors(MAX_SUPPORTED_MOTORS);
pwmMotorsEnabled = false;
pwmShutdownPulsesForAllMotors();
}
void pwmEnableMotors(void)
static motorVTable_t motorPwmVTable;
bool pwmEnableMotors(void)
{
/* check motors can be enabled */
pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
return (motorPwmVTable.write != &pwmWriteUnused);
}
bool pwmAreMotorsEnabled(void)
static void pwmCompleteOneshotMotorUpdate(void)
{
return pwmMotorsEnabled;
}
#ifdef USE_DSHOT_TELEMETRY
static bool pwmStartWriteUnused(uint8_t motorCount)
{
UNUSED(motorCount);
return true;
}
#endif
static void pwmCompleteWriteUnused(uint8_t motorCount)
{
UNUSED(motorCount);
}
static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
for (int index = 0; index < motorPwmDevice.count; index++) {
if (motors[index].forceOverflow) {
timerForceOverflow(motors[index].channel.tim);
}
@ -274,23 +156,27 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
}
}
void pwmCompleteMotorUpdate(uint8_t motorCount)
static float pwmConvertFromExternal(uint16_t externalValue)
{
pwmCompleteWrite(motorCount);
return (float)externalValue;
}
#ifdef USE_DSHOT_TELEMETRY
bool pwmStartMotorUpdate(uint8_t motorCount)
static uint16_t pwmConvertToExternal(float motorValue)
{
return pwmStartWrite(motorCount);
return (uint16_t)motorValue;
}
#endif
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
static motorVTable_t motorPwmVTable = {
.enable = pwmEnableMotors,
.disable = pwmDisableMotors,
.shutdown = pwmShutdownPulsesForAllMotors,
.convertExternalToMotor = pwmConvertFromExternal,
.convertMotorToExternal = pwmConvertToExternal,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
{
memset(motors, 0, sizeof(motors));
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
motorPwmDevice.vTable = motorPwmVTable;
float sMin = 0;
float sLen = 0;
@ -319,45 +205,11 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
useUnsyncedPwm = true;
idlePulse = 0;
break;
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
pwmWrite = &pwmWriteDshot;
loadDmaBuffer = &loadDmaBufferProshot;
pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
#ifdef USE_DSHOT_TELEMETRY
pwmStartWrite = &pwmStartDshotMotorUpdate;
useDshotTelemetry = motorConfig->useDshotTelemetry;
#endif
isDshot = true;
break;
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmWrite = &pwmWriteDshot;
loadDmaBuffer = &loadDmaBufferDshot;
pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
#ifdef USE_DSHOT_TELEMETRY
pwmStartWrite = &pwmStartDshotMotorUpdate;
useDshotTelemetry = motorConfig->useDshotTelemetry;
#endif
isDshot = true;
#ifdef USE_DSHOT_DMAR
if (motorConfig->useBurstDshot) {
useBurstDshot = true;
}
#endif
break;
#endif
}
if (!isDshot) {
pwmWrite = &pwmWriteStandard;
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
#ifdef USE_DSHOT_TELEMETRY
pwmStartWrite = pwmStartWriteUnused;
#endif
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
@ -365,26 +217,15 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
pwmWrite = &pwmWriteUnused;
pwmCompleteWrite = &pwmCompleteWriteUnused;
motorPwmDevice.vTable.write = &pwmWriteUnused;
motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
/* TODO: block arming and add reason system cannot arm */
return;
return NULL;
}
motors[motorIndex].io = IOGetByTag(tag);
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
#ifdef USE_DSHOT
if (isDshot) {
pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true;
continue;
}
#endif
#if defined(STM32F1)
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
#else
@ -422,7 +263,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
motors[motorIndex].enabled = true;
}
pwmMotorsEnabled = true;
return &motorPwmDevice;
}
pwmOutputPort_t *pwmGetMotors(void)
@ -430,274 +271,9 @@ pwmOutputPort_t *pwmGetMotors(void)
return motors;
}
bool isMotorProtocolDshot(void)
{
return isDshot;
}
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_PROSHOT1000):
return MOTOR_PROSHOT1000_HZ;
case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_HZ;
case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300):
return MOTOR_DSHOT300_HZ;
default:
case(PWM_TYPE_DSHOT150):
return MOTOR_DSHOT150_HZ;
}
}
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 pwmDshotCommandQueueFull()
{
return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
}
FAST_CODE bool pwmDshotCommandIsQueued(void)
{
return commandQueueHead != commandQueueTail;
}
static FAST_CODE bool isLastDshotCommand(void)
{
return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead);
}
FAST_CODE bool pwmDshotCommandIsProcessing(void)
{
if (!pwmDshotCommandIsQueued()) {
return false;
}
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY
|| command->state == DSHOT_COMMAND_STATE_ACTIVE
|| (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand());
return commandIsProcessing;
}
static FAST_CODE bool pwmDshotCommandQueueUpdate(void)
{
if (pwmDshotCommandIsQueued()) {
commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
if (pwmDshotCommandIsQueued()) {
// There is another command in the queue so update it so it's ready to output in
// sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass
// the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states.
dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail];
nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE;
nextCommand->nextCommandCycleDelay = 0;
return true;
}
}
return false;
}
static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs)
{
// Find the minimum number of motor output cycles needed to
// provide at least delayUs time delay
uint32_t ret = delayUs / dshotCommandPidLoopTimeUs;
if (delayUs % dshotCommandPidLoopTimeUs) {
ret++;
}
return ret;
}
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)
{
if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandQueueFull()) {
return;
}
uint8_t repeats = 1;
timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
switch (command) {
case DSHOT_CMD_SPIN_DIRECTION_1:
case DSHOT_CMD_SPIN_DIRECTION_2:
case DSHOT_CMD_3D_MODE_OFF:
case DSHOT_CMD_3D_MODE_ON:
case DSHOT_CMD_SAVE_SETTINGS:
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
case DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE:
case DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY:
repeats = 10;
break;
case DSHOT_CMD_BEACON1:
case DSHOT_CMD_BEACON2:
case DSHOT_CMD_BEACON3:
case DSHOT_CMD_BEACON4:
case DSHOT_CMD_BEACON5:
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);
#ifdef USE_DSHOT_TELEMETRY
timeUs_t currentTimeUs = micros();
while (!pwmStartDshotMotorUpdate(motorCount) &&
cmpTimeUs(micros(), currentTimeUs) < 1000);
#endif
for (uint8_t i = 0; i < motorCount; i++) {
if ((i == index) || (index == ALL_MOTORS)) {
motorDmaOutput_t *const motor = getMotorDmaOutput(i);
motor->requestTelemetry = true;
pwmWriteDshotInt(i, command);
}
}
pwmCompleteDshotMotorUpdate(0);
}
delayMicroseconds(delayAfterCommandUs);
} else {
dshotCommandControl_t *commandControl = addCommand();
if (commandControl) {
commandControl->repeats = repeats;
commandControl->delayAfterCommandUs = delayAfterCommandUs;
for (unsigned i = 0; i < motorCount; i++) {
if (index == i || index == ALL_MOTORS) {
commandControl->command[i] = command;
} else {
commandControl->command[i] = DSHOT_CMD_MOTOR_STOP;
}
}
if (allMotorsAreIdle(motorCount)) {
// we can skip the motors idle wait state
commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
} else {
commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
commandControl->nextCommandCycleDelay = 0; // will be set after idle wait completes
}
}
}
}
uint8_t pwmGetDshotCommand(uint8_t index)
{
return commandQueue[commandQueueTail].command[index];
}
// This function is used to synchronize the dshot command output timing with
// the normal motor output timing tied to the PID loop frequency. A "true" result
// allows the motor output to be sent, "false" means delay until next loop. So take
// the example of a dshot command that needs to repeat 10 times at 1ms intervals.
// If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output.
FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
{
dshotCommandControl_t* command = &commandQueue[commandQueueTail];
switch (command->state) {
case DSHOT_COMMAND_STATE_IDLEWAIT:
if (allMotorsAreIdle(motorCount)) {
command->state = DSHOT_COMMAND_STATE_STARTDELAY;
command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
}
break;
case DSHOT_COMMAND_STATE_STARTDELAY:
if (command->nextCommandCycleDelay-- > 1) {
return false; // Delay motor output until the start of the command seequence
}
command->state = DSHOT_COMMAND_STATE_ACTIVE;
command->nextCommandCycleDelay = 0; // first iteration of the repeat happens now
FALLTHROUGH;
case DSHOT_COMMAND_STATE_ACTIVE:
if (command->nextCommandCycleDelay-- > 1) {
return false; // Delay motor output until the next command repeat
}
command->repeats--;
if (command->repeats) {
command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
} else {
command->state = DSHOT_COMMAND_STATE_POSTDELAY;
command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs);
if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) {
// Account for the 1 extra motor output loop between commands.
// Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop.
command->nextCommandCycleDelay--;
}
}
break;
case DSHOT_COMMAND_STATE_POSTDELAY:
if (command->nextCommandCycleDelay-- > 1) {
return false; // Delay motor output until the end of the post-command delay
}
if (pwmDshotCommandQueueUpdate()) {
// Will be true if the command queue is not empty and we
// want to wait for the next command to start in sequence.
return false;
}
}
return true;
}
FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor)
{
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
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
// append checksum
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
csum = ~csum;
}
#endif
csum &= 0xf;
packet = (packet << 4) | csum;
return packet;
}
#endif // USE_DSHOT
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
void pwmWriteServo(uint8_t index, float value)
{
if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
@ -735,50 +311,5 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
servos[servoIndex].enabled = true;
}
}
#endif // USE_SERVOS
#ifdef USE_BEEPER
void pwmWriteBeeper(bool onoffBeep)
{
if (!beeperPwm.io) {
return;
}
if (onoffBeep == true) {
*beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
void pwmToggleBeeper(void)
{
pwmWriteBeeper(!beeperPwm.enabled);
}
void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
{
const timerHardware_t *timer = timerGetByTag(tag);
IO_t beeperIO = IOGetByTag(tag);
if (beeperIO && timer) {
beeperPwm.io = beeperIO;
IOInit(beeperPwm.io, OWNER_BEEPER, RESOURCE_INDEX(0));
#if defined(STM32F1)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#else
IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
#endif
freqBeep = frequency;
pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
#endif // USE_BEEPER
#endif //USE_PWM_OUTPUT
#endif // USE_PWM_OUTPUT

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

@ -25,187 +25,20 @@
#include "common/time.h"
#include "drivers/io_types.h"
#include "drivers/motor.h"
#include "drivers/timer.h"
#define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 480
#define ALL_MOTORS 255
#define DSHOT_MAX_COMMAND 47
#define DSHOT_TELEMETRY_INPUT_LEN 32
#define PROSHOT_TELEMETRY_INPUT_LEN 8
#define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1
#define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100
/*
DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
3D Mode:
0 = stop
48 (low) - 1047 (high) -> negative direction
1048 (low) - 2047 (high) -> positive direction
*/
typedef enum {
DSHOT_CMD_MOTOR_STOP = 0,
DSHOT_CMD_BEACON1,
DSHOT_CMD_BEACON2,
DSHOT_CMD_BEACON3,
DSHOT_CMD_BEACON4,
DSHOT_CMD_BEACON5,
DSHOT_CMD_ESC_INFO, // V2 includes settings
DSHOT_CMD_SPIN_DIRECTION_1,
DSHOT_CMD_SPIN_DIRECTION_2,
DSHOT_CMD_3D_MODE_OFF,
DSHOT_CMD_3D_MODE_ON,
DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
DSHOT_CMD_SAVE_SETTINGS,
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
DSHOT_CMD_LED0_ON, // BLHeli32 only
DSHOT_CMD_LED1_ON, // BLHeli32 only
DSHOT_CMD_LED2_ON, // BLHeli32 only
DSHOT_CMD_LED3_ON, // BLHeli32 only
DSHOT_CMD_LED0_OFF, // BLHeli32 only
DSHOT_CMD_LED1_OFF, // BLHeli32 only
DSHOT_CMD_LED2_OFF, // BLHeli32 only
DSHOT_CMD_LED3_OFF, // BLHeli32 only
DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF = 30, // KISS audio Stream mode on/Off
DSHOT_CMD_SILENT_MODE_ON_OFF = 31, // KISS silent Mode on/Off
DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE = 32,
DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY = 33,
DSHOT_CMD_MAX = 47
} dshotCommands_e;
#define DSHOT_MIN_THROTTLE 48
#define DSHOT_MAX_THROTTLE 2047
typedef enum {
PWM_TYPE_STANDARD = 0,
PWM_TYPE_ONESHOT125,
PWM_TYPE_ONESHOT42,
PWM_TYPE_MULTISHOT,
PWM_TYPE_BRUSHED,
#ifdef USE_DSHOT
PWM_TYPE_DSHOT150,
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT1200,
PWM_TYPE_PROSHOT1000,
#endif
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
#define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 20
#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
#define PROSHOT_BASE_SYMBOL 24 // 1uS
#define PROSHOT_BIT_WIDTH 3
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
#endif
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
typedef struct {
TIM_TypeDef *timer;
#if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
#if defined(STM32F7) || defined(STM32H7)
TIM_HandleTypeDef timHandle;
DMA_HandleTypeDef hdma_tim;
#endif
#ifdef STM32F3
DMA_Channel_TypeDef *dmaBurstRef;
#else
DMA_Stream_TypeDef *dmaBurstRef;
#endif
uint16_t dmaBurstLength;
#ifdef STM32H7
uint32_t *dmaBurstBuffer;
#else
uint32_t dmaBurstBuffer[DSHOT_DMA_BUFFER_SIZE * 4];
#endif
timeUs_t inputDirectionStampUs;
#endif
uint16_t timerDmaSources;
} motorDmaTimer_t;
typedef struct {
ioTag_t ioTag;
const timerHardware_t *timerHardware;
uint16_t value;
#ifdef USE_DSHOT
uint16_t timerDmaSource;
uint8_t timerDmaIndex;
bool configured;
#ifdef STM32H7
TIM_HandleTypeDef TimHandle;
DMA_HandleTypeDef hdma_tim;
#endif
uint8_t output;
uint8_t index;
#ifdef USE_DSHOT_TELEMETRY
bool useProshot;
volatile bool isInput;
volatile bool hasTelemetry;
uint16_t dshotTelemetryValue;
timeDelta_t dshotTelemetryDeadtimeUs;
bool dshotTelemetryActive;
#ifdef USE_HAL_DRIVER
LL_TIM_OC_InitTypeDef ocInitStruct;
LL_TIM_IC_InitTypeDef icInitStruct;
LL_DMA_InitTypeDef dmaInitStruct;
uint32_t llChannel;
#else
TIM_OCInitTypeDef ocInitStruct;
TIM_ICInitTypeDef icInitStruct;
DMA_InitTypeDef dmaInitStruct;
#endif
uint8_t dmaInputLen;
#endif
#ifdef STM32F3
DMA_Channel_TypeDef *dmaRef;
#else
DMA_Stream_TypeDef *dmaRef;
#endif
#endif
motorDmaTimer_t *timer;
volatile bool requestTelemetry;
#ifdef USE_DSHOT_TELEMETRY
uint32_t dmaBuffer[DSHOT_TELEMETRY_INPUT_LEN];
#else
#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
#elif defined(STM32H7)
uint32_t *dmaBuffer;
#else
uint8_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
#endif
#endif
} motorDmaOutput_t;
motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
struct timerHardware_s;
typedef void pwmWriteFn(uint8_t index, float value); // function pointer used to write motors
typedef void pwmCompleteWriteFn(uint8_t motorCount); // function pointer used after motors are written
typedef bool pwmStartWriteFn(uint8_t motorCount); // function pointer used before motors are written
typedef struct {
volatile timCCR_t *ccr;
@ -221,14 +54,10 @@ typedef struct {
IO_t io;
} pwmOutputPort_t;
extern bool useBurstDshot;
#ifdef USE_DSHOT_TELEMETRY
extern bool useDshotTelemetry;
extern uint32_t dshotInvalidPacketCount;
#endif
extern FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
struct motorDevConfig_s;
void motorDevInit(const struct motorDevConfig_s *motorDevConfig, uint16_t idlePulse, uint8_t motorCount);
motorDevice_t *motorPwmDevInit(const struct motorDevConfig_s *motorDevConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm);
typedef struct servoDevConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
@ -241,61 +70,9 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
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);
#ifdef STM32H7
extern DMA_RAM uint32_t dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_SIZE];
#ifdef USE_DSHOT_DMAR
extern DMA_RAM uint32_t dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
#endif
#endif
extern loadDmaBufferFn *loadDmaBuffer;
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
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);
#ifdef USE_DSHOT_TELEMETRY
bool pwmStartDshotMotorUpdate(uint8_t motorCount);
#endif
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
bool pwmDshotCommandIsQueued(void);
bool pwmDshotCommandIsProcessing(void);
uint8_t pwmGetDshotCommand(uint8_t index);
bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount);
uint16_t getDshotTelemetry(uint8_t index);
bool isDshotMotorTelemetryActive(uint8_t motorIndex);
void setDshotPidLoopTime(uint32_t pidLoopTime);
#ifdef USE_DSHOT_TELEMETRY_STATS
int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
#endif
#endif
#ifdef USE_BEEPER
void pwmWriteBeeper(bool onoffBeep);
void pwmToggleBeeper(void);
void beeperPwmInit(const ioTag_t tag, uint16_t frequency);
#endif
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion);
void pwmWriteMotor(uint8_t index, float value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteMotorUpdate(uint8_t motorCount);
bool pwmStartMotorUpdate(uint8_t motorCount);
void pwmWriteServo(uint8_t index, float value);
pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void);
void pwmDisableMotors(void);
void pwmEnableMotors(void);
bool pwmAreMotorsEnabled(void);

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

@ -42,6 +42,9 @@
#endif
#include "pwm_output.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "pwm_output_dshot_shared.h"
@ -143,13 +146,11 @@ FAST_CODE void pwmDshotSetDirectionOutput(
}
void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
void pwmCompleteDshotMotorUpdate(void)
{
UNUSED(motorCount);
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (pwmDshotCommandIsQueued()) {
if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
if (!dshotCommandQueueEmpty()) {
if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
return;
}
}
@ -336,6 +337,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
if (useBurstDshot) {
dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
#if defined(STM32F3)
DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
@ -361,6 +364,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
{
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
#if defined(STM32F3)
DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer;
DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
@ -398,11 +403,11 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
} else
#endif
{
dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
TIM_Cmd(timer, ENABLE);

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

@ -28,8 +28,14 @@
#include "build/debug.h"
#include "common/time.h"
#include "drivers/dma.h"
#include "drivers/dma_reqmap.h"
#include "drivers/motor.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/rcc.h"
@ -122,15 +128,11 @@ void pwmDshotSetDirectionOutput(
}
FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
FAST_CODE void pwmCompleteDshotMotorUpdate(void)
{
UNUSED(motorCount);
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (pwmDshotCommandIsQueued()) {
if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
return;
}
if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
return;
}
for (int i = 0; i < dmaMotorTimerCount; i++) {
@ -317,6 +319,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
if (useBurstDshot) {
dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
DMAINIT.Channel = dmaChannel;
DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
@ -326,6 +330,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
{
dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
DMAINIT.Channel = dmaChannel;
DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_1_4;
@ -359,11 +365,11 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
#endif
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
} else
#endif
{
dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(2, 1), motor->index);
dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
LL_TIM_OC_Init(timer, channel, &OCINIT);

27
src/main/drivers/pwm_output_dshot_hal_hal.c
View File

@ -31,6 +31,9 @@
#include "drivers/io.h"
#include "timer.h"
#include "pwm_output.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "drivers/nvic.h"
#include "dma.h"
#include "rcc.h"
@ -138,7 +141,7 @@ void pwmBurstDMAStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, uint32
__HAL_TIM_ENABLE_DMA(htim, BurstRequestSrc);
}
void pwmWriteDshotInt(uint8_t index, uint16_t value)
FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
{
motorDmaOutput_t *const motor = &dmaMotors[index];
@ -147,10 +150,10 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
/*If there is a command ready to go overwrite the value and send that instead*/
if (pwmDshotCommandIsProcessing()) {
if (dshotCommandIsProcessing()) {
value = pwmGetDshotCommand(index);
if (value) {
motor->requestTelemetry = true;
motor->protocolControl.requestTelemetry = true;
}
}
@ -164,9 +167,9 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
return;
}
motor->value = value;
motor->protocolControl.value = value;
uint16_t packet = prepareDshotPacket(motor);
uint16_t packet = prepareDshotPacket(&motor->protocolControl);
uint8_t bufferSize;
#ifdef USE_DSHOT_DMAR
@ -182,16 +185,12 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
}
void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
void pwmCompleteDshotMotorUpdate(void)
{
UNUSED(motorCount);
// If there is a dshot command loaded up, time it correctly with motor update
if (pwmDshotCommandIsQueued()) {
if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
return;
}
if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
return;
}
#ifdef USE_DSHOT_DMAR
@ -426,12 +425,12 @@ P - High - High -
#ifdef USE_DSHOT_DMAR
if (useBurstDshot) {
dmaInit(identifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), timerIndex);
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
} else
#endif
{
dmaInit(identifier, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
dmaSetHandler(identifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
}
// Start the timer channel now.

27
src/main/drivers/pwm_output_dshot_shared.c
View File

@ -43,6 +43,9 @@
#endif
#include "pwm_output.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "pwm_output_dshot_shared.h"
@ -106,7 +109,7 @@ 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 (pwmDshotCommandIsProcessing()) {
if (dshotCommandIsProcessing()) {
value = pwmGetDshotCommand(index);
#ifdef USE_DSHOT_TELEMETRY
// reset telemetry debug statistics every time telemetry is enabled
@ -116,13 +119,13 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
}
#endif
if (value) {
motor->requestTelemetry = true;
motor->protocolControl.requestTelemetry = true;
}
}
motor->value = value;
motor->protocolControl.value = value;
uint16_t packet = prepareDshotPacket(motor);
uint16_t packet = prepareDshotPacket(&motor->protocolControl);
uint8_t bufferSize;
#ifdef USE_DSHOT_DMAR
@ -238,7 +241,7 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
}
#endif // USE_DSHOT_TELEMETRY_STATS
bool pwmStartDshotMotorUpdate(uint8_t motorCount)
bool pwmStartDshotMotorUpdate(void)
{
if (!useDshotTelemetry) {
return true;
@ -246,7 +249,7 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount)
#ifdef USE_DSHOT_TELEMETRY_STATS
const timeMs_t currentTimeMs = millis();
#endif
for (int i = 0; i < motorCount; i++) {
for (int i = 0; i < dshotPwmDevice.count; i++) {
if (dmaMotors[i].hasTelemetry) {
#ifdef STM32F7
uint32_t edges = LL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
@ -296,7 +299,7 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount)
}
pwmDshotSetDirectionOutput(&dmaMotors[i], true);
}
dshotEnableChannels(motorCount);
dshotEnableChannels(dshotPwmDevice.count);
return true;
}
@ -305,6 +308,16 @@ bool isDshotMotorTelemetryActive(uint8_t motorIndex)
return dmaMotors[motorIndex].dshotTelemetryActive;
}
bool isDshotTelemetryActive(void)
{
for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
if (!isDshotMotorTelemetryActive(i)) {
return false;
}
}
return true;
}
#ifdef USE_DSHOT_TELEMETRY_STATS
int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex)
{

2
src/main/drivers/pwm_output_dshot_shared.h
View File

@ -66,7 +66,7 @@ FAST_CODE void pwmDshotSetDirectionOutput(
#endif
);
bool pwmStartDshotMotorUpdate(uint8_t motorCount);
bool pwmStartDshotMotorUpdate(void);
#endif
#endif

4
src/main/drivers/serial_escserial.c
View File

@ -947,7 +947,9 @@ bool escEnablePassthrough(serialPort_t *escPassthroughPort, const motorDevConfig
LED0_OFF;
LED1_OFF;
//StopPwmAllMotors();
pwmDisableMotors();
// XXX Review effect of motor refactor
//pwmDisableMotors();
motorDisable();
passPort = escPassthroughPort;
uint32_t escBaudrate;

46
src/main/drivers/sound_beeper.c
View File

@ -34,6 +34,52 @@
static IO_t beeperIO = DEFIO_IO(NONE);
static bool beeperInverted = false;
static uint16_t beeperFrequency = 0;
#ifdef USE_PWM_OUTPUT
static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep = 0;
static void pwmWriteBeeper(bool on)
{
if (!beeperPwm.io) {
return;
}
if (on) {
*beeperPwm.channel.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
static void pwmToggleBeeper(void)
{
pwmWriteBeeper(!beeperPwm.enabled);
}
static void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
{
const timerHardware_t *timer = timerGetByTag(tag);
IO_t beeperIO = IOGetByTag(tag);
if (beeperIO && timer) {
beeperPwm.io = beeperIO;
IOInit(beeperPwm.io, OWNER_BEEPER, RESOURCE_INDEX(0));
#if defined(STM32F1)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#else
IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
#endif
freqBeep = frequency;
pwmOutConfig(&beeperPwm.channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
*beeperPwm.channel.ccr = 0;
beeperPwm.enabled = false;
}
}
#endif
#endif
void systemBeep(bool onoff)

2
src/main/fc/config.c
View File

@ -33,6 +33,8 @@
#include "config/config_eeprom.h"
#include "config/feature.h"
#include "drivers/dshot_command.h"
#include "drivers/motor.h"
#include "drivers/system.h"
#include "fc/config.h"

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

@ -40,7 +40,10 @@
#include "config/feature.h"
#include "drivers/dshot.h"
#include "drivers/dshot_command.h"
#include "drivers/light_led.h"
#include "drivers/motor.h"
#include "drivers/sound_beeper.h"
#include "drivers/system.h"
#include "drivers/time.h"
@ -368,7 +371,7 @@ void disarm(void)
BEEP_OFF;
#ifdef USE_DSHOT
if (isMotorProtocolDshot() && flipOverAfterCrashActive && !featureIsEnabled(FEATURE_3D)) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
#endif
flipOverAfterCrashActive = false;
@ -419,7 +422,7 @@ void tryArm(void)
if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
flipOverAfterCrashActive = false;
if (!featureIsEnabled(FEATURE_3D)) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
}
} else {
flipOverAfterCrashActive = true;
@ -427,7 +430,7 @@ void tryArm(void)
runawayTakeoffCheckDisabled = false;
#endif
if (!featureIsEnabled(FEATURE_3D)) {
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
}
}
}

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

@ -526,7 +526,7 @@ void init(void)
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
idlePulse = 0; // brushed motors
}
#ifdef USE_PWM_OUTPUT
#ifdef USE_MOTOR
/* Motors needs to be initialized soon as posible because hardware initialization
* may send spurious pulses to esc's causing their early initialization. Also ppm
* receiver may share timer with motors so motors MUST be initialized here. */
@ -909,8 +909,8 @@ void init(void)
rcdeviceInit();
#endif // USE_RCDEVICE
#ifdef USE_PWM_OUTPUT
pwmEnableMotors();
#ifdef USE_MOTOR
motorEnable();
#endif
#ifdef USE_PERSISTENT_STATS

138
src/main/flight/mixer.c
View File

@ -37,8 +37,7 @@
#include "pg/motor.h"
#include "pg/rx.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_esc_detect.h"
#include "drivers/motor.h"
#include "drivers/time.h"
#include "drivers/io.h"
@ -94,7 +93,6 @@ static motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS];
static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
@ -334,45 +332,7 @@ void initEscEndpoints(void)
motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f;
}
// Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
switch (motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
{
float outputLimitOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) * (1 - motorOutputLimit);
disarmMotorOutput = DSHOT_CMD_MOTOR_STOP;
if (featureIsEnabled(FEATURE_3D)) {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
} else {
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
}
}
break;
#endif
default:
if (featureIsEnabled(FEATURE_3D)) {
float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - motorOutputLimit) / 2;
disarmMotorOutput = flight3DConfig()->neutral3d;
motorOutputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
motorOutputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
} else {
disarmMotorOutput = motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle - ((motorConfig()->maxthrottle - motorConfig()->minthrottle) * (1 - motorOutputLimit));
}
break;
}
motorInitEndpoints(motorOutputLimit, &motorOutputLow, &motorOutputHigh, &disarmMotorOutput, &deadbandMotor3dHigh, &deadbandMotor3dLow);
rcCommandThrottleRange = PWM_RANGE_MAX - PWM_RANGE_MIN;
}
@ -478,19 +438,7 @@ void mixerResetDisarmedMotors(void)
void writeMotors(void)
{
#ifdef USE_PWM_OUTPUT
if (pwmAreMotorsEnabled()) {
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
if (!pwmStartMotorUpdate(motorCount)) {
return;
}
#endif
for (int i = 0; i < motorCount; i++) {
pwmWriteMotor(i, motor[i]);
}
pwmCompleteMotorUpdate(motorCount);
}
#endif
motorWriteAll(motor);
}
static void writeAllMotors(int16_t mc)
@ -508,14 +456,6 @@ void stopMotors(void)
delay(50); // give the timers and ESCs a chance to react.
}
void stopPwmAllMotors(void)
{
#ifdef USE_PWM_OUTPUT
pwmShutdownPulsesForAllMotors(motorCount);
delayMicroseconds(1500);
#endif
}
static FAST_RAM_ZERO_INIT float throttle = 0;
static FAST_RAM_ZERO_INIT float loggingThrottle = 0;
static FAST_RAM_ZERO_INIT float motorOutputMin;
@ -751,7 +691,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
}
}
float applyThrottleLimit(float throttle)
static float applyThrottleLimit(float throttle)
{
if (currentControlRateProfile->throttle_limit_percent < 100) {
const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f;
@ -766,7 +706,7 @@ float applyThrottleLimit(float throttle)
return throttle;
}
void applyMotorStop(void)
static void applyMotorStop(void)
{
for (int i = 0; i < motorCount; i++) {
motor[i] = disarmMotorOutput;
@ -774,7 +714,7 @@ void applyMotorStop(void)
}
#ifdef USE_DYN_LPF
void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
{
static timeUs_t lastDynLpfUpdateUs = 0;
static int dynLpfPreviousQuantizedThrottle = -1; // to allow an initial zero throttle to set the filter cutoff
@ -950,60 +890,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
}
}
float convertExternalToMotor(uint16_t externalValue)
{
uint16_t motorValue;
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
if (featureIsEnabled(FEATURE_3D)) {
if (externalValue == PWM_RANGE_MID) {
motorValue = DSHOT_CMD_MOTOR_STOP;
} else if (externalValue < PWM_RANGE_MID) {
motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
} else {
motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
} else {
motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
}
}
else
#endif
{
motorValue = externalValue;
}
return (float)motorValue;
}
uint16_t convertMotorToExternal(float motorValue)
{
uint16_t externalValue;
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
if (featureIsEnabled(FEATURE_3D)) {
if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
externalValue = PWM_RANGE_MID;
} else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
} else {
externalValue = scaleRange(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
}
} else {
externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
}
}
else
#endif
{
externalValue = motorValue;
}
return externalValue;
}
void mixerSetThrottleAngleCorrection(int correctionValue)
{
throttleAngleCorrection = correctionValue;
@ -1013,15 +899,3 @@ float mixerGetLoggingThrottle(void)
{
return loggingThrottle;
}
#ifdef USE_DSHOT_TELEMETRY
bool isDshotTelemetryActive(void)
{
for (uint8_t i = 0; i < motorCount; i++) {
if (!isDshotMotorTelemetryActive(i)) {
return false;
}
}
return true;
}
#endif

10
src/main/flight/mixer.h
View File

@ -29,9 +29,6 @@
#define QUAD_MOTOR_COUNT 4
// Digital protocol has fixed values
#define DSHOT_3D_FORWARD_MIN_THROTTLE 1048
// Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode
{
@ -108,15 +105,10 @@ void mixerConfigureOutput(void);
void mixerResetDisarmedMotors(void);
void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation);
void syncMotors(bool enabled);
void writeMotors(void);
void stopMotors(void);
void stopPwmAllMotors(void);
void writeMotors(void);
float convertExternalToMotor(uint16_t externalValue);
uint16_t convertMotorToExternal(float motorValue);
bool mixerIsTricopter(void);
void mixerSetThrottleAngleCorrection(int correctionValue);
float mixerGetLoggingThrottle(void);
bool isDshotTelemetryActive(void);

3
src/main/flight/pid.c
View File

@ -34,6 +34,7 @@
#include "config/config_reset.h"
#include "drivers/dshot_command.h"
#include "drivers/pwm_output.h"
#include "drivers/sound_beeper.h"
#include "drivers/time.h"
@ -225,7 +226,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
dT = targetPidLooptime * 1e-6f;
pidFrequency = 1.0f / dT;
#ifdef USE_DSHOT
setDshotPidLoopTime(targetPidLooptime);
dshotSetPidLoopTime(targetPidLooptime);
#endif
}

2
src/main/flight/rpm_filter.c
View File

@ -31,6 +31,8 @@
#include "common/filter.h"
#include "common/maths.h"
#include "drivers/dshot.h"
#include "flight/mixer.h"
#include "flight/pid.h"

3
src/main/io/beeper.c
View File

@ -27,6 +27,7 @@
#include "config/feature.h"
#include "drivers/dshot_command.h"
#include "drivers/io.h"
#include "drivers/pwm_output.h"
#include "drivers/sound_beeper.h"
@ -410,7 +411,7 @@ void beeperUpdate(timeUs_t currentTimeUs)
if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) {
lastDshotBeaconCommandTimeUs = currentTimeUs;
pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false);
dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false);
}
}
#endif

6
src/main/io/serial_4way.c
View File

@ -137,7 +137,9 @@ inline void setEscOutput(uint8_t selEsc)
uint8_t esc4wayInit(void)
{
// StopPwmAllMotors();
pwmDisableMotors();
// XXX Review effect of motor refactor
//pwmDisableMotors();
motorDisable();
escCount = 0;
memset(&escHardware, 0, sizeof(escHardware));
pwmOutputPort_t *pwmMotors = pwmGetMotors();
@ -161,7 +163,7 @@ void esc4wayRelease(void)
IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP);
setEscLo(escCount);
}
pwmEnableMotors();
motorEnable();
}

7
src/main/msp/msp.c
View File

@ -51,6 +51,7 @@
#include "drivers/flash.h"
#include "drivers/io.h"
#include "drivers/max7456.h"
#include "drivers/motor.h"
#include "drivers/pwm_output.h"
#include "drivers/sdcard.h"
#include "drivers/serial.h"
@ -255,7 +256,7 @@ static void mspRebootFn(serialPort_t *serialPort)
{
UNUSED(serialPort);
stopPwmAllMotors();
motorShutdown();
switch (rebootMode) {
case MSP_REBOOT_FIRMWARE:
@ -963,7 +964,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
continue;
}
sbufWriteU16(dst, convertMotorToExternal(motor[i]));
sbufWriteU16(dst, motorConvertToExternal(motor[i]));
#else
sbufWriteU16(dst, 0);
#endif
@ -1950,7 +1951,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MOTOR:
for (int i = 0; i < getMotorCount(); i++) {
motor_disarmed[i] = convertExternalToMotor(sbufReadU16(src));
motor_disarmed[i] = motorConvertFromExternal(sbufReadU16(src));
}
break;

2
src/main/osd/osd_elements.c
View File

@ -66,7 +66,7 @@
#include "drivers/display.h"
#include "drivers/max7456_symbols.h"
#include "drivers/pwm_output.h"
#include "drivers/dshot.h"
#include "drivers/time.h"
#include "drivers/vtx_common.h"

6
src/main/pg/motor.c
View File

@ -50,16 +50,18 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->dev.useUnsyncedPwm = true;
} else
#endif
#endif // USE_BRUSHED_ESC_AUTODETECT
{
motorConfig->minthrottle = 1070;
motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
}
#endif
#endif // BRUSHED_MOTORS
motorConfig->maxthrottle = 2000;
motorConfig->mincommand = 1000;
motorConfig->digitalIdleOffsetValue = 550;
#ifdef USE_DSHOT_DMAR
motorConfig->dev.useBurstDshot = ENABLE_DSHOT_DMAR;
#endif

1
src/main/pg/motor.h
View File

@ -32,6 +32,7 @@ typedef struct motorDevConfig_s {
uint8_t useBurstDshot;
uint8_t useDshotTelemetry;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
uint8_t motorTransportProtocol;
} motorDevConfig_t;
typedef struct motorConfig_s {

13
src/main/sensors/esc_sensor.c
View File

@ -28,6 +28,8 @@
#include "build/debug.h"
#include "common/time.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
@ -36,7 +38,10 @@
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/pwm_output.h"
#include "drivers/timer.h"
#include "drivers/motor.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
@ -291,11 +296,13 @@ static void selectNextMotor(void)
}
}
// XXX Review ESC sensor under refactored motor handling
void escSensorProcess(timeUs_t currentTimeUs)
{
const timeMs_t currentTimeMs = currentTimeUs / 1000;
if (!escSensorPort || !pwmAreMotorsEnabled()) {
if (!escSensorPort || !motorIsEnabled()) {
return;
}
@ -312,7 +319,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true;
motor->protocolControl.requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);

93
src/main/target/SITL/target.c
View File

@ -31,6 +31,7 @@
#include "drivers/io.h"
#include "drivers/dma.h"
#include "drivers/motor.h"
#include "drivers/serial.h"
#include "drivers/serial_tcp.h"
#include "drivers/system.h"
@ -380,8 +381,7 @@ int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y)
// PWM part
static bool pwmMotorsEnabled = false;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
// real value to send
@ -389,18 +389,6 @@ static int16_t motorsPwm[MAX_SUPPORTED_MOTORS];
static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
static int16_t idlePulse;
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
UNUSED(motorConfig);
UNUSED(motorCount);
idlePulse = _idlePulse;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
motors[motorIndex].enabled = true;
}
pwmMotorsEnabled = true;
}
void servoDevInit(const servoDevConfig_t *servoConfig) {
UNUSED(servoConfig);
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
@ -408,33 +396,51 @@ void servoDevInit(const servoDevConfig_t *servoConfig) {
}
}
static motorDevice_t motorPwmDevice; // Forward
pwmOutputPort_t *pwmGetMotors(void) {
return motors;
}
void pwmDisableMotors(void) {
pwmMotorsEnabled = false;
static float pwmConvertFromExternal(uint16_t externalValue)
{
return (float)externalValue;
}
void pwmEnableMotors(void) {
pwmMotorsEnabled = true;
static uint16_t pwmConvertToExternal(float motorValue)
{
return (uint16_t)motorValue;
}
bool pwmAreMotorsEnabled(void) {
return pwmMotorsEnabled;
static void pwmDisableMotors(void)
{
motorPwmDevice.enabled = false;
}
void pwmWriteMotor(uint8_t index, float value) {
static bool pwmEnableMotors(void)
{
motorPwmDevice.enabled = true;
return true;
}
static void pwmWriteMotor(uint8_t index, float value)
{
motorsPwm[index] = value - idlePulse;
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
UNUSED(motorCount);
pwmMotorsEnabled = false;
static void pwmWriteMotorInt(uint8_t index, uint16_t value)
{
pwmWriteMotor(index, (float)value);
}
void pwmCompleteMotorUpdate(uint8_t motorCount) {
UNUSED(motorCount);
static void pwmShutdownPulsesForAllMotors(void)
{
motorPwmDevice.enabled = false;
}
static void pwmCompleteMotorUpdate(void)
{
// send to simulator
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
@ -458,6 +464,41 @@ void pwmWriteServo(uint8_t index, float value) {
servosPwm[index] = value;
}
static motorDevice_t motorPwmDevice = {
.vTable = {
.convertExternalToMotor = pwmConvertFromExternal,
.convertMotorToExternal = pwmConvertToExternal,
.enable = pwmEnableMotors,
.disable = pwmDisableMotors,
.updateStart = motorUpdateStartNull,
.write = pwmWriteMotor,
.writeInt = pwmWriteMotorInt,
.updateComplete = pwmCompleteMotorUpdate,
.shutdown = pwmShutdownPulsesForAllMotors,
}
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
{
UNUSED(motorConfig);
UNUSED(useUnsyncedPwm);
if (motorCount > 4) {
return NULL;
}
idlePulse = _idlePulse;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
motors[motorIndex].enabled = true;
}
motorPwmDevice.count = motorCount; // Never used, but seemingly a right thing to set it anyways.
motorPwmDevice.initialized = true;
motorPwmDevice.enabled = false;
return &motorPwmDevice;
}
// ADC part
uint16_t adcGetChannel(uint8_t channel) {
UNUSED(channel);

4
src/main/telemetry/srxl.c
View File

@ -64,7 +64,7 @@
#include "telemetry/srxl.h"
#include "drivers/vtx_common.h"
#include "drivers/pwm_output.h"
#include "drivers/dshot.h"
#include "io/vtx_tramp.h"
#include "io/vtx_smartaudio.h"
@ -180,7 +180,7 @@ uint16_t getMotorAveragePeriod(void)
}
#endif
#if defined( USE_DSHOT_TELEMETRY)
#if defined(USE_DSHOT_TELEMETRY)
if (useDshotTelemetry) {
uint16_t motors = getMotorCount();

4
src/test/unit/cli_unittest.cc
View File

@ -274,7 +274,8 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh";
void parseRcChannels(const char *, rxConfig_t *){}
void mixerLoadMix(int, motorMixer_t *) {}
bool setModeColor(ledModeIndex_e, int, int) { return false; }
float convertExternalToMotor(uint16_t ){ return 1.0; }
float motorConvertFromExternal(uint16_t) { return 1.0; }
void motorShutdown(void) { }
uint8_t getCurrentPidProfileIndex(void){ return 1; }
uint8_t getCurrentControlRateProfileIndex(void){ return 1; }
void changeControlRateProfile(uint8_t) {}
@ -328,7 +329,6 @@ void schedulerSetCalulateTaskStatistics(bool) {}
void setArmingDisabled(armingDisableFlags_e) {}
void waitForSerialPortToFinishTransmitting(serialPort_t *) {}
void stopPwmAllMotors(void) {}
void systemResetToBootloader(void) {}
void resetConfigs(void) {}
void systemReset(void) {}

2
src/test/unit/cms_unittest.cc
View File

@ -142,7 +142,7 @@ uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }
void saveConfigAndNotify(void) {}
void stopMotors(void) {}
void stopPwmAllMotors(void) {}
void motorShutdown(void) {}
void systemReset(void) {}
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }