mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
PICO: First stage of DSHOT and PWM implementation (#14205)
This commit is contained in:
parent
ea31439824
commit
ac05707f98
4 changed files with 641 additions and 3 deletions
412
src/platform/PICO/dshot_pico.c
Normal file
412
src/platform/PICO/dshot_pico.c
Normal file
|
@ -0,0 +1,412 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Betaflight.
|
||||||
|
*
|
||||||
|
* Betaflight is 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.
|
||||||
|
*
|
||||||
|
* Betaflight is distributed in the hope that it 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 <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT
|
||||||
|
|
||||||
|
#include "build/debug.h"
|
||||||
|
#include "build/debug_pin.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/dshot.h"
|
||||||
|
#include "drivers/dshot_command.h"
|
||||||
|
#include "drivers/dshot_command.h"
|
||||||
|
#include "drivers/motor_types.h"
|
||||||
|
|
||||||
|
#include "drivers/time.h"
|
||||||
|
#include "drivers/timer.h"
|
||||||
|
|
||||||
|
#include "pg/motor.h"
|
||||||
|
|
||||||
|
#include "hardware/pio.h"
|
||||||
|
#include "hardware/clocks.h"
|
||||||
|
|
||||||
|
// Maximum time to wait for telemetry reception to complete
|
||||||
|
#define DSHOT_TELEMETRY_TIMEOUT 2000
|
||||||
|
|
||||||
|
FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs;
|
||||||
|
//static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
|
||||||
|
|
||||||
|
typedef struct motorOutput_s {
|
||||||
|
int offset;
|
||||||
|
dshotProtocolControl_t protocolControl;
|
||||||
|
int pinIndex; // pinIndex of this motor output within a group that bbPort points to
|
||||||
|
IO_t io; // IO_t for this output
|
||||||
|
bool configured;
|
||||||
|
bool enabled;
|
||||||
|
PIO pio;
|
||||||
|
uint16_t pio_sm;
|
||||||
|
} motorOutput_t;
|
||||||
|
|
||||||
|
typedef struct picoDshotTelemetryBuffer_s {
|
||||||
|
union {
|
||||||
|
uint32_t u32[2];
|
||||||
|
uint16_t u16[4];
|
||||||
|
};
|
||||||
|
} picoDshotTelemetryBuffer_t;
|
||||||
|
|
||||||
|
static motorProtocolTypes_e motorProtocol;
|
||||||
|
//uint8_t dshotMotorCount = 0;
|
||||||
|
static motorOutput_t dshotMotors[MAX_SUPPORTED_MOTORS];
|
||||||
|
bool useDshotTelemetry = false;
|
||||||
|
|
||||||
|
#define DSHOT_BIT_PERIOD 40
|
||||||
|
|
||||||
|
static const uint16_t dshot_bidir_PIO_instructions[] = {
|
||||||
|
// .wrap_target
|
||||||
|
0xff81, // 0: set pindirs, 1 [31]
|
||||||
|
0xff01, // 1: set pins, 1 [31]
|
||||||
|
0x80a0, // 2: pull block
|
||||||
|
0x6050, // 3: out y, 16
|
||||||
|
0x00e6, // 4: jmp !osre, 6
|
||||||
|
0x000e, // 5: jmp 14
|
||||||
|
0x6041, // 6: out y, 1
|
||||||
|
0x006b, // 7: jmp !y, 11
|
||||||
|
0xfd00, // 8: set pins, 0 [29]
|
||||||
|
0xe501, // 9: set pins, 1 [5]
|
||||||
|
0x0004, // 10: jmp 4
|
||||||
|
0xee00, // 11: set pins, 0 [14]
|
||||||
|
0xf401, // 12: set pins, 1 [20]
|
||||||
|
0x0004, // 13: jmp 4
|
||||||
|
0xe055, // 14: set y, 21
|
||||||
|
0x1f8f, // 15: jmp y--, 15 [31]
|
||||||
|
0xe080, // 16: set pindirs, 0
|
||||||
|
0xe05f, // 17: set y, 31
|
||||||
|
0xa142, // 18: nop [1]
|
||||||
|
0x0076, // 19: jmp !y, 22
|
||||||
|
0x0095, // 20: jmp y--, 21
|
||||||
|
0x00d2, // 21: jmp pin, 18
|
||||||
|
0xe05e, // 22: set y, 30
|
||||||
|
0x4901, // 23: in pins, 1 [9]
|
||||||
|
0x0097, // 24: jmp y--, 23
|
||||||
|
0x4801, // 25: in pins, 1 [8]
|
||||||
|
0x8020, // 26: push block
|
||||||
|
0xe05f, // 27: set y, 31
|
||||||
|
0x4a01, // 28: in pins, 1 [10]
|
||||||
|
0x009c, // 29: jmp y--, 28
|
||||||
|
0x8020, // 30: push block
|
||||||
|
0x0000, // 31: jmp 0
|
||||||
|
// .wrap
|
||||||
|
};
|
||||||
|
|
||||||
|
static float getPeriodTiming(void)
|
||||||
|
{
|
||||||
|
switch(motorProtocol) {
|
||||||
|
case MOTOR_PROTOCOL_DSHOT600:
|
||||||
|
return 1.67f;
|
||||||
|
case MOTOR_PROTOCOL_DSHOT300:
|
||||||
|
return 3.33f;
|
||||||
|
default:
|
||||||
|
case MOTOR_PROTOCOL_DSHOT150:
|
||||||
|
return 6.66f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static const struct pio_program dshot_bidir_program = {
|
||||||
|
.instructions = dshot_bidir_PIO_instructions,
|
||||||
|
.length = ARRAYLEN(dshot_bidir_PIO_instructions),
|
||||||
|
.origin = -1,
|
||||||
|
};
|
||||||
|
|
||||||
|
static void dshot_bidir_program_init(PIO pio, uint sm, int offset, uint pin)
|
||||||
|
{
|
||||||
|
pio_sm_config config = pio_get_default_sm_config();
|
||||||
|
sm_config_set_wrap(&config, offset + ARRAYLEN(dshot_bidir_PIO_instructions), offset);
|
||||||
|
|
||||||
|
sm_config_set_set_pins(&config, pin, 1);
|
||||||
|
sm_config_set_in_pins(&config, pin);
|
||||||
|
sm_config_set_jmp_pin (&config, pin);
|
||||||
|
pio_gpio_init(pio, pin);
|
||||||
|
pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
|
||||||
|
gpio_set_pulls(pin, true, false);
|
||||||
|
sm_config_set_out_shift(&config, false, false, ARRAYLEN(dshot_bidir_PIO_instructions));
|
||||||
|
sm_config_set_in_shift(&config, false, false, ARRAYLEN(dshot_bidir_PIO_instructions));
|
||||||
|
|
||||||
|
float clocks_per_us = clock_get_hz(clk_sys) / 1000000;
|
||||||
|
sm_config_set_clkdiv(&config, getPeriodTiming() / DSHOT_BIT_PERIOD * clocks_per_us);
|
||||||
|
|
||||||
|
pio_sm_init(pio, sm, offset, &config);
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint32_t decodeTelemetry(const uint32_t first, const uint32_t second)
|
||||||
|
{
|
||||||
|
UNUSED(first);
|
||||||
|
UNUSED(second);
|
||||||
|
return DSHOT_TELEMETRY_INVALID;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool dshotTelemetryWait(void)
|
||||||
|
{
|
||||||
|
// Wait for telemetry reception to complete
|
||||||
|
bool telemetryPending;
|
||||||
|
bool telemetryWait = false;
|
||||||
|
const timeUs_t startTimeUs = micros();
|
||||||
|
|
||||||
|
do {
|
||||||
|
telemetryPending = false;
|
||||||
|
for (unsigned motorIndex = 0; motorIndex < dshotMotorCount && !telemetryPending; motorIndex++) {
|
||||||
|
const motorOutput_t *motor = &dshotMotors[motorIndex];
|
||||||
|
const int fifo_words = pio_sm_get_rx_fifo_level(motor->pio, motor->pio_sm);
|
||||||
|
telemetryPending |= fifo_words >= 2;
|
||||||
|
}
|
||||||
|
|
||||||
|
telemetryWait |= telemetryPending;
|
||||||
|
|
||||||
|
if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} while (telemetryPending);
|
||||||
|
|
||||||
|
if (telemetryWait) {
|
||||||
|
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
return telemetryWait;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotUpdateInit(void)
|
||||||
|
{
|
||||||
|
// NO OP as no buffer needed for PIO
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool dshotDecodeTelemetry(void)
|
||||||
|
{
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
|
if (!useDshotTelemetry) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY_STATS
|
||||||
|
const timeMs_t currentTimeMs = millis();
|
||||||
|
#endif
|
||||||
|
|
||||||
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
|
||||||
|
const motorOutput_t *motor = &dshotMotors[motorIndex];
|
||||||
|
|
||||||
|
if (dshotTelemetryState.rawValueState == DSHOT_RAW_VALUE_STATE_NOT_PROCESSED) {
|
||||||
|
int fifo_words = pio_sm_get_rx_fifo_level(motor->pio, motor->pio_sm);
|
||||||
|
if (fifo_words < 2) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint32_t rawOne = pio_sm_get_blocking(motor->pio, motor->pio_sm);
|
||||||
|
const uint32_t rawTwo = pio_sm_get_blocking(motor->pio, motor->pio_sm);
|
||||||
|
|
||||||
|
uint32_t rawValue = decodeTelemetry(rawOne, rawTwo);
|
||||||
|
|
||||||
|
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
|
||||||
|
dshotTelemetryState.readCount++;
|
||||||
|
|
||||||
|
if (rawValue != DSHOT_TELEMETRY_INVALID) {
|
||||||
|
// Check EDT enable or store raw value
|
||||||
|
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
|
||||||
|
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
|
||||||
|
} else {
|
||||||
|
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
dshotTelemetryState.invalidPacketCount++;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY_STATS
|
||||||
|
updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotWriteInt(uint8_t motorIndex, uint16_t value)
|
||||||
|
{
|
||||||
|
motorOutput_t *const motor = &dshotMotors[motorIndex];
|
||||||
|
|
||||||
|
if (!motor->configured) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*If there is a command ready to go overwrite the value and send that instead*/
|
||||||
|
if (dshotCommandIsProcessing()) {
|
||||||
|
value = dshotCommandGetCurrent(motorIndex);
|
||||||
|
if (value) {
|
||||||
|
motor->protocolControl.requestTelemetry = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
motor->protocolControl.value = value;
|
||||||
|
|
||||||
|
uint16_t packet = prepareDshotPacket(&motor->protocolControl);
|
||||||
|
pio_sm_put(motor->pio, motor->pio_sm, packet);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotWrite(uint8_t motorIndex, float value)
|
||||||
|
{
|
||||||
|
dshotWriteInt(motorIndex, lrintf(value));
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotUpdateComplete(void)
|
||||||
|
{
|
||||||
|
// If there is a dshot command loaded up, time it correctly with motor update
|
||||||
|
if (!dshotCommandQueueEmpty()) {
|
||||||
|
if (!dshotCommandOutputIsEnabled(dshotMotorCount)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool dshotEnableMotors(void)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < dshotMotorCount; i++) {
|
||||||
|
const motorOutput_t *motor = &dshotMotors[i];
|
||||||
|
if (motor->configured) {
|
||||||
|
IOConfigGPIO(motor->io, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotDisableMotors(void)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotShutdown(void)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool dshotIsMotorEnabled(unsigned index)
|
||||||
|
{
|
||||||
|
return dshotMotors[index].enabled;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotPostInit(void)
|
||||||
|
{
|
||||||
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
|
||||||
|
dshotMotors[motorIndex].enabled = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool dshotIsMotorIdle(unsigned motorIndex)
|
||||||
|
{
|
||||||
|
if (motorIndex >= ARRAYLEN(dshotMotors)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return dshotMotors[motorIndex].protocolControl.value == 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void dshotRequestTelemetry(unsigned index)
|
||||||
|
{
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
|
if (index < dshotMotorCount) {
|
||||||
|
dshotMotors[index].protocolControl.requestTelemetry = true;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
static motorVTable_t dshotVTable = {
|
||||||
|
.postInit = dshotPostInit,
|
||||||
|
.enable = dshotEnableMotors,
|
||||||
|
.disable = dshotDisableMotors,
|
||||||
|
.isMotorEnabled = dshotIsMotorEnabled,
|
||||||
|
.telemetryWait = dshotTelemetryWait,
|
||||||
|
.decodeTelemetry = dshotDecodeTelemetry,
|
||||||
|
.updateInit = dshotUpdateInit,
|
||||||
|
.write = dshotWrite,
|
||||||
|
.writeInt = dshotWriteInt,
|
||||||
|
.updateComplete = dshotUpdateComplete,
|
||||||
|
.convertExternalToMotor = dshotConvertFromExternal,
|
||||||
|
.convertMotorToExternal = dshotConvertToExternal,
|
||||||
|
.shutdown = dshotShutdown,
|
||||||
|
.isMotorIdle = dshotIsMotorIdle,
|
||||||
|
.requestTelemetry = dshotRequestTelemetry,
|
||||||
|
};
|
||||||
|
|
||||||
|
bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
|
||||||
|
{
|
||||||
|
dbgPinLo(0);
|
||||||
|
dbgPinLo(1);
|
||||||
|
|
||||||
|
motorProtocol = motorConfig->motorProtocol;
|
||||||
|
device->vTable = &dshotVTable;
|
||||||
|
dshotMotorCount = device->count;
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT_TELEMETRY
|
||||||
|
useDshotTelemetry = motorConfig->useDshotTelemetry;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
|
||||||
|
IO_t io = IOGetByTag(motorConfig->ioTags[motorIndex]);
|
||||||
|
|
||||||
|
const PIO pio = pio0;
|
||||||
|
const int pio_sm = pio_claim_unused_sm(pio, false);
|
||||||
|
|
||||||
|
if (!IOIsFreeOrPreinit(io) || pio_sm < 0) {
|
||||||
|
/* not enough motors initialised for the mixer or a break in the motors or issue with PIO */
|
||||||
|
device->vTable = NULL;
|
||||||
|
dshotMotorCount = 0;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
int pinIndex = DEFIO_TAG_PIN(motorConfig->ioTags[motorIndex]);
|
||||||
|
|
||||||
|
dshotMotors[motorIndex].pinIndex = pinIndex;
|
||||||
|
dshotMotors[motorIndex].io = io;
|
||||||
|
dshotMotors[motorIndex].pio = pio;
|
||||||
|
dshotMotors[motorIndex].pio_sm = pio_sm;
|
||||||
|
|
||||||
|
uint8_t iocfg = 0;
|
||||||
|
IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
||||||
|
IOConfigGPIO(io, iocfg);
|
||||||
|
|
||||||
|
int offset = pio_add_program(pio, &dshot_bidir_program);
|
||||||
|
if (offset == -1) {
|
||||||
|
/* error loading PIO */
|
||||||
|
pio_sm_unclaim(pio, pio_sm);
|
||||||
|
device->vTable = NULL;
|
||||||
|
dshotMotorCount = 0;
|
||||||
|
return false;
|
||||||
|
} else {
|
||||||
|
dshotMotors[motorIndex].offset = offset;
|
||||||
|
}
|
||||||
|
|
||||||
|
dshot_bidir_program_init(pio, pio_sm, dshotMotors[motorIndex].offset, pinIndex);
|
||||||
|
pio_sm_set_enabled(pio, pio_sm, true);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
|
||||||
|
{
|
||||||
|
/* DSHOT BIT BANG not required for PICO */
|
||||||
|
UNUSED(motorDevConfig);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
#endif // USE_DSHOT
|
|
@ -200,6 +200,9 @@ MCU_COMMON_SRC = \
|
||||||
drivers/serial_pinconfig.c \
|
drivers/serial_pinconfig.c \
|
||||||
drivers/serial_uart_pinconfig.c \
|
drivers/serial_uart_pinconfig.c \
|
||||||
drivers/usb_io.c \
|
drivers/usb_io.c \
|
||||||
|
drivers/dshot.c \
|
||||||
|
PICO/dshot_pico.c \
|
||||||
|
PICO/pwm_pico.c \
|
||||||
PICO/stdio_pico_stub.c \
|
PICO/stdio_pico_stub.c \
|
||||||
PICO/debug_pico.c \
|
PICO/debug_pico.c \
|
||||||
PICO/system.c \
|
PICO/system.c \
|
||||||
|
|
226
src/platform/PICO/pwm_pico.c
Normal file
226
src/platform/PICO/pwm_pico.c
Normal file
|
@ -0,0 +1,226 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Betaflight.
|
||||||
|
*
|
||||||
|
* Betaflight is 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.
|
||||||
|
*
|
||||||
|
* Betaflight is distributed in the hope that it 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 <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_PWM_OUTPUT
|
||||||
|
|
||||||
|
#include "build/debug.h"
|
||||||
|
#include "build/debug_pin.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
|
#include "drivers/io_impl.h"
|
||||||
|
#include "drivers/pwm_output.h"
|
||||||
|
#include "drivers/motor_types.h"
|
||||||
|
|
||||||
|
#include "drivers/time.h"
|
||||||
|
#include "drivers/timer.h"
|
||||||
|
|
||||||
|
#include "pg/motor.h"
|
||||||
|
|
||||||
|
#include "hardware/gpio.h"
|
||||||
|
#include "hardware/pwm.h"
|
||||||
|
#include "hardware/clocks.h"
|
||||||
|
|
||||||
|
typedef struct picoPwmMotors_s {
|
||||||
|
uint16_t slice;
|
||||||
|
uint16_t channel;
|
||||||
|
} picoPwmMotors_t;
|
||||||
|
|
||||||
|
static picoPwmMotors_t picoPwmMotors[MAX_SUPPORTED_MOTORS];
|
||||||
|
static bool useContinuousUpdate = false;
|
||||||
|
|
||||||
|
void pwmShutdownPulsesForAllMotors(void)
|
||||||
|
{
|
||||||
|
for (int index = 0; index < pwmMotorCount; index++) {
|
||||||
|
pwm_set_chan_level(picoPwmMotors[index].slice, picoPwmMotors[index].channel, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void pwmDisableMotors(void)
|
||||||
|
{
|
||||||
|
pwmShutdownPulsesForAllMotors();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool pwmEnableMotors(void)
|
||||||
|
{
|
||||||
|
return pwmMotorCount > 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool pwmIsMotorEnabled(unsigned index)
|
||||||
|
{
|
||||||
|
return motors[index].enabled;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmWriteStandard(uint8_t index, float value)
|
||||||
|
{
|
||||||
|
/* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
|
||||||
|
pwm_set_chan_level(picoPwmMotors[index].slice, picoPwmMotors[index].channel, lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset));
|
||||||
|
}
|
||||||
|
|
||||||
|
static void pwmCompleteMotorUpdate(void)
|
||||||
|
{
|
||||||
|
if (useContinuousUpdate) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int index = 0; index < pwmMotorCount; index++) {
|
||||||
|
pwm_set_chan_level(picoPwmMotors[index].slice, picoPwmMotors[index].channel, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static float pwmConvertFromExternal(uint16_t externalValue)
|
||||||
|
{
|
||||||
|
return (float)externalValue;
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint16_t pwmConvertToExternal(float motorValue)
|
||||||
|
{
|
||||||
|
return (uint16_t)motorValue;
|
||||||
|
}
|
||||||
|
|
||||||
|
static motorVTable_t motorPwmVTable = {
|
||||||
|
.postInit = NULL,
|
||||||
|
.enable = pwmEnableMotors,
|
||||||
|
.disable = pwmDisableMotors,
|
||||||
|
.isMotorEnabled = pwmIsMotorEnabled,
|
||||||
|
.shutdown = pwmShutdownPulsesForAllMotors,
|
||||||
|
.convertExternalToMotor = pwmConvertFromExternal,
|
||||||
|
.convertMotorToExternal = pwmConvertToExternal,
|
||||||
|
.write = pwmWriteStandard,
|
||||||
|
.decodeTelemetry = NULL,
|
||||||
|
.updateComplete = pwmCompleteMotorUpdate,
|
||||||
|
.requestTelemetry = NULL,
|
||||||
|
.isMotorIdle = NULL,
|
||||||
|
};
|
||||||
|
|
||||||
|
bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
|
||||||
|
{
|
||||||
|
UNUSED(idlePulse);
|
||||||
|
|
||||||
|
if (!device) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwmMotorCount = device->count;
|
||||||
|
|
||||||
|
memset(motors, 0, sizeof(motors));
|
||||||
|
|
||||||
|
if (!device || !motorConfig) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwmMotorCount = device->count;
|
||||||
|
device->vTable = &motorPwmVTable;
|
||||||
|
|
||||||
|
useContinuousUpdate = motorConfig->useContinuousUpdate;
|
||||||
|
|
||||||
|
float sMin = 0;
|
||||||
|
float sLen = 0;
|
||||||
|
switch (motorConfig->motorProtocol) {
|
||||||
|
default:
|
||||||
|
case MOTOR_PROTOCOL_ONESHOT125:
|
||||||
|
sMin = 125e-6f;
|
||||||
|
sLen = 125e-6f;
|
||||||
|
break;
|
||||||
|
case MOTOR_PROTOCOL_ONESHOT42:
|
||||||
|
sMin = 42e-6f;
|
||||||
|
sLen = 42e-6f;
|
||||||
|
break;
|
||||||
|
case MOTOR_PROTOCOL_MULTISHOT:
|
||||||
|
sMin = 5e-6f;
|
||||||
|
sLen = 20e-6f;
|
||||||
|
break;
|
||||||
|
case MOTOR_PROTOCOL_BRUSHED:
|
||||||
|
sMin = 0;
|
||||||
|
useContinuousUpdate = true;
|
||||||
|
break;
|
||||||
|
case MOTOR_PROTOCOL_PWM :
|
||||||
|
sMin = 1e-3f;
|
||||||
|
sLen = 1e-3f;
|
||||||
|
useContinuousUpdate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (unsigned motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) {
|
||||||
|
|
||||||
|
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
|
||||||
|
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
|
||||||
|
|
||||||
|
motors[motorIndex].io = IOGetByTag(tag);
|
||||||
|
uint8_t pin = IO_PINBYTAG(tag);
|
||||||
|
|
||||||
|
const uint16_t slice = pwm_gpio_to_slice_num(pin);
|
||||||
|
const uint16_t channel = pwm_gpio_to_channel(pin);
|
||||||
|
|
||||||
|
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
|
||||||
|
|
||||||
|
picoPwmMotors[motorIndex].slice = slice;
|
||||||
|
picoPwmMotors[motorIndex].channel = channel;
|
||||||
|
|
||||||
|
/* standard PWM outputs */
|
||||||
|
// margin of safety is 4 periods when not continuous
|
||||||
|
const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
|
||||||
|
|
||||||
|
/*
|
||||||
|
PWM Frequency = clock / (interval * (wrap + 1))
|
||||||
|
|
||||||
|
Wrap is when the counter resets to zero.
|
||||||
|
Interval (divider) will determine the resolution.
|
||||||
|
*/
|
||||||
|
const uint32_t clock = clock_get_hz(clk_sys); // PICO timer clock is the CPU clock.
|
||||||
|
|
||||||
|
/* used to find the desired timer frequency for max resolution */
|
||||||
|
const unsigned prescaler = ceilf(clock / pwmRateHz); /* rounding up */
|
||||||
|
const uint32_t hz = clock / prescaler;
|
||||||
|
const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
|
||||||
|
|
||||||
|
pwm_config config = pwm_get_default_config();
|
||||||
|
|
||||||
|
const uint8_t interval = (uint8_t)(clock / period);
|
||||||
|
const uint8_t fraction = (uint8_t)(((clock / period) - interval) * (0x01 << 4));
|
||||||
|
pwm_config_set_clkdiv_int_frac(&config, interval, fraction);
|
||||||
|
pwm_config_set_wrap(&config, period);
|
||||||
|
|
||||||
|
gpio_set_function(pin, GPIO_FUNC_PWM);
|
||||||
|
|
||||||
|
pwm_set_chan_level(slice, channel, 0);
|
||||||
|
pwm_init(slice, &config, true);
|
||||||
|
|
||||||
|
/*
|
||||||
|
if brushed then it is the entire length of the period.
|
||||||
|
TODO: this can be moved back to periodMin and periodLen
|
||||||
|
once mixer outputs a 0..1 float value.
|
||||||
|
*/
|
||||||
|
motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
|
||||||
|
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
|
||||||
|
motors[motorIndex].enabled = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // USE_PWM_OUTPUT
|
|
@ -40,7 +40,6 @@
|
||||||
#define USE_SPI
|
#define USE_SPI
|
||||||
#define USE_SPI_DEVICE_0
|
#define USE_SPI_DEVICE_0
|
||||||
#define USE_SPI_DEVICE_1
|
#define USE_SPI_DEVICE_1
|
||||||
//#undef USE_SPI
|
|
||||||
|
|
||||||
#undef USE_SOFTSERIAL1
|
#undef USE_SOFTSERIAL1
|
||||||
#undef USE_SOFTSERIAL2
|
#undef USE_SOFTSERIAL2
|
||||||
|
@ -56,10 +55,8 @@
|
||||||
#undef USE_TIMER
|
#undef USE_TIMER
|
||||||
#undef USE_I2C
|
#undef USE_I2C
|
||||||
#undef USE_UART
|
#undef USE_UART
|
||||||
#undef USE_DSHOT
|
|
||||||
#undef USE_RCC
|
#undef USE_RCC
|
||||||
#undef USE_CLI
|
#undef USE_CLI
|
||||||
#undef USE_PWM_OUTPUT
|
|
||||||
#undef USE_RX_PWM
|
#undef USE_RX_PWM
|
||||||
#undef USE_RX_PPM
|
#undef USE_RX_PPM
|
||||||
#undef USE_RX_SPI
|
#undef USE_RX_SPI
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue