Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

[SERIALSHOT] Initial cut on Matek Serialshot implementation

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2019-04-25 21:50:06 +02:00
parent 572bef0057
commit cac528337c
14 changed files with 291 additions and 108 deletions
Download patch file Download diff file Expand all files Collapse all files

1
make/source.mk
View file

@ -93,6 +93,7 @@ COMMON_SRC = \
io/beeper.c \
io/lights.c \
io/pwmdriver_i2c.c \
io/esc_serialshot.c \
io/piniobox.c \
io/serial.c \
io/serial_4way.c \

13
src/main/drivers/pwm_mapping.c
View file

@ -205,15 +205,8 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
continue;
}
if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->pwmProtocolType, init->enablePWMOutput)) {
if (init->useFastPwm) {
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM;
} else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) {
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM;
} else {
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ;
}
if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, motorConfig()->motorPwmRate, init->enablePWMOutput)) {
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR;
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.motorCount;
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr;
@ -232,7 +225,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
}
if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) {
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM;
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO;
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount;
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr;

6
src/main/drivers/pwm_mapping.h
View file

@ -57,7 +57,6 @@ typedef struct drv_pwm_config_s {
bool useUART3;
bool useUART6;
bool useVbat;
bool useFastPwm;
bool useSoftSerial;
bool useLEDStrip;
#ifdef USE_RANGEFINDER
@ -66,8 +65,6 @@ typedef struct drv_pwm_config_s {
bool useServoOutputs;
uint16_t servoPwmRate;
uint16_t servoCenterPulse;
uint8_t pwmProtocolType;
uint16_t motorPwmRate;
rangefinderIOConfig_t rangefinderIOConfig;
} drv_pwm_config_t;
@ -75,9 +72,6 @@ typedef enum {
PWM_PF_NONE = 0,
PWM_PF_MOTOR = (1 << 0),
PWM_PF_SERVO = (1 << 1),
PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2),
PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3),
PWM_PF_OUTPUT_PROTOCOL_FASTPWM = (1 << 4),
PWM_PF_PPM = (1 << 5),
PWM_PF_PWM = (1 << 6)
} pwmPortFlags_e;

196
src/main/drivers/pwm_output.c
View file

@ -32,6 +32,7 @@
#include "drivers/io_pca9685.h"
#include "io/pwmdriver_i2c.h"
#include "io/esc_serialshot.h"
#include "config/feature.h"
@ -59,9 +60,8 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function poi
typedef struct {
TCH_t * tch;
pwmWriteFuncPtr pwmWritePtr;
bool configured;
uint16_t value; // Used to keep track of last motor value
uint16_t value;
// PWM parameters
volatile timCCR_t *ccr; // Shortcut for timer CCR register
@ -74,16 +74,22 @@ typedef struct {
#endif
} pwmOutputPort_t;
typedef struct {
pwmOutputPort_t * pwmPort; // May be NULL if motor doesn't use the PWM port
pwmWriteFuncPtr pwmWritePtr; // Function to update the "value" and/or pwmPort
uint16_t value; // Used to keep track of last motor value
} pwmOutputMotor_t;
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
static pwmOutputMotor_t motors[MAX_PWM_MOTORS];
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#ifdef USE_DSHOT
static motorPwmProtocolTypes_e initMotorProtocol;
static bool isProtocolDshot = false;
static timeUs_t dshotMotorUpdateIntervalUs = 0;
static timeUs_t dshotMotorLastUpdateUs;
#if defined(USE_DSHOT) || defined(USE_SERIALSHOT)
static timeUs_t digitalMotorUpdateIntervalUs = 0;
static timeUs_t digitalMotorLastUpdateUs;
#endif
#ifdef BEEPER_PWM
@ -110,20 +116,34 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin
*p->ccr = 0;
}
static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
static pwmOutputPort_t *pwmOutAllocatePort(void)
{
if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) {
LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
return NULL;
}
pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
p->tch = NULL;
p->configured = false;
return p;
}
static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
{
// Attempt to allocate TCH
TCH_t * tch = timerGetTCH(timHw);
if (tch == NULL) {
return NULL;
}
pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
// Allocate motor output port
pwmOutputPort_t *p = pwmOutAllocatePort();
if (p == NULL) {
return NULL;
}
const IO_t io = IOGetByTag(timHw->tag);
IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount);
@ -143,13 +163,15 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t
static void pwmWriteStandard(uint8_t index, uint16_t value)
{
*motors[index]->ccr = lrintf((value * motors[index]->pulseScale) + motors[index]->pulseOffset);
if (motors[index].pwmPort) {
*(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset);
}
}
void pwmWriteMotor(uint8_t index, uint16_t value)
{
if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) {
motors[index]->pwmWritePtr(index, value);
if (motors[index].pwmWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) {
motors[index].pwmWritePtr(index, value);
}
}
@ -157,7 +179,9 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
for (int index = 0; index < motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
*motors[index]->ccr = 0;
if (motors[index].pwmPort) {
*(motors[index].pwmPort->ccr) = 0;
}
}
}
@ -221,7 +245,7 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
// Keep track of motor update interval
const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz;
dshotMotorUpdateIntervalUs = MAX(dshotMotorUpdateIntervalUs, motorIntervalUs);
digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs);
// Configure timer DMA
if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
@ -233,12 +257,6 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
return port;
}
static void pwmWriteDshot(uint8_t index, uint16_t value)
{
// DMA operation might still be running. Cache value for future use
motors[index]->value = value;
}
static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
{
for (int i = 0; i < 16; i++) {
@ -265,71 +283,125 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
return packet;
}
#endif
void pwmCompleteDshotUpdate(uint8_t motorCount)
#ifdef USE_SERIALSHOT
static void motorConfigSerialShot(uint16_t motorPwmRateHz)
{
// Keep track of motor update interval
const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz;
digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs);
// Kick off SerialShot driver initalization
serialshotInitialize();
}
#endif
#if defined(USE_DSHOT) || defined(USE_SERIALSHOT)
static void pwmWriteDigital(uint8_t index, uint16_t value)
{
// Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate()
// DSHOT and some other digital protocols use 11-bit throttle range [0;2047]
motors[index].value = constrain(value, 0, 2047);
}
bool FAST_CODE NOINLINE isMotorProtocolDshot(void)
{
// We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config
// motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used
return (initMotorProtocol == PWM_TYPE_DSHOT150) ||
(initMotorProtocol == PWM_TYPE_DSHOT300) ||
(initMotorProtocol == PWM_TYPE_DSHOT600) ||
(initMotorProtocol == PWM_TYPE_DSHOT1200);
}
bool FAST_CODE NOINLINE isMotorProtocolSerialShot(void)
{
return (initMotorProtocol == PWM_TYPE_SERIALSHOT);
}
bool FAST_CODE NOINLINE isMotorProtocolDigital(void)
{
return isMotorProtocolDshot() || isMotorProtocolSerialShot();
}
void pwmCompleteMotorUpdate(void)
{
// Get motor count from mixer
int motorCount = getMotorCount();
// Get latest REAL time
timeUs_t currentTimeUs = micros();
// Enforce motor update rate
if (!isProtocolDshot || (dshotMotorUpdateIntervalUs == 0) || ((currentTimeUs - dshotMotorLastUpdateUs) <= dshotMotorUpdateIntervalUs)) {
if (!isMotorProtocolDigital() || (digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) {
return;
}
dshotMotorLastUpdateUs = currentTimeUs;
digitalMotorLastUpdateUs = currentTimeUs;
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
// Generate DMA buffers
for (int index = 0; index < motorCount; index++) {
if (motors[index] && motors[index]->configured) {
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
// TODO: ESC telemetry
uint16_t packet = prepareDshotPacket(motors[index]->value, false);
uint16_t packet = prepareDshotPacket(motors[index].value, false);
loadDmaBufferDshot(motors[index]->dmaBuffer, packet);
timerPWMPrepareDMA(motors[index]->tch, DSHOT_DMA_BUFFER_SIZE);
loadDmaBufferDshot(motors[index].pwmPort->dmaBuffer, packet);
timerPWMPrepareDMA(motors[index].pwmPort->tch, DSHOT_DMA_BUFFER_SIZE);
}
}
// Start DMA on all timers
for (int index = 0; index < motorCount; index++) {
if (motors[index] && motors[index]->configured) {
timerPWMStartDMA(motors[index]->tch);
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
timerPWMStartDMA(motors[index].pwmPort->tch);
}
}
}
}
#endif
bool FAST_CODE NOINLINE isMotorProtocolDshot(void)
{
return isProtocolDshot;
#ifdef USE_SERIALSHOT
if (isMotorProtocolSerialShot()) {
for (int index = 0; index < motorCount; index++) {
serialshotUpdateMotor(index, motors[index].value);
}
serialshotSendUpdate();
}
#endif
}
#endif
bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, motorPwmProtocolTypes_e proto, bool enableOutput)
bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, bool enableOutput)
{
pwmOutputPort_t * port = NULL;
pwmWriteFuncPtr pwmWritePtr;
// Keep track of initial motor protocol
initMotorProtocol = motorConfig()->motorPwmProtocol;
#ifdef BRUSHED_MOTORS
proto = PWM_TYPE_BRUSHED; // Override proto
initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto
#endif
switch (proto) {
switch (initMotorProtocol) {
case PWM_TYPE_BRUSHED:
port = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput);
pwmWritePtr = pwmWriteStandard;
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput);
motors[motorIndex].pwmWritePtr = pwmWriteStandard;
break;
case PWM_TYPE_ONESHOT125:
port = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput);
pwmWritePtr = pwmWriteStandard;
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput);
motors[motorIndex].pwmWritePtr = pwmWriteStandard;
break;
case PWM_TYPE_ONESHOT42:
port = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput);
pwmWritePtr = pwmWriteStandard;
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput);
motors[motorIndex].pwmWritePtr = pwmWriteStandard;
break;
case PWM_TYPE_MULTISHOT:
port = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput);
pwmWritePtr = pwmWriteStandard;
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput);
motors[motorIndex].pwmWritePtr = pwmWriteStandard;
break;
#ifdef USE_DSHOT
@ -337,28 +409,32 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
port = motorConfigDshot(timerHardware, proto, motorPwmRateHz, enableOutput);
if (port) {
isProtocolDshot = true;
pwmWritePtr = pwmWriteDshot;
}
motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, initMotorProtocol, motorPwmRateHz, enableOutput);
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
break;
#endif
#ifdef USE_SERIALSHOT
case PWM_TYPE_SERIALSHOT:
// This is hacky. Our motor output flow is: init() -> pwmInit() -> pwmMotorConfig()
// This is decoupled from mixer, so if the board doesn't define any PWM motor output the pwmMotorConfig() won't get called
// We rely on the fact that all FCs define hardware PWM motor outputs. To make this bullet-proof we need to change the
// init sequence to originate from the mixer and allocate timers only if necessary
motorConfigSerialShot(motorPwmRateHz);
motors[motorIndex].pwmPort = NULL;
// Serialshot uses the same throttle interpretation as DSHOT, so we use the same write function here
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
break;
#endif
default:
case PWM_TYPE_STANDARD:
port = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput);
pwmWritePtr = pwmWriteStandard;
motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput);
motors[motorIndex].pwmWritePtr = pwmWriteStandard;
break;
}
if (port) {
port->pwmWritePtr = pwmWritePtr;
motors[motorIndex] = port;
return true;
}
return false;
return (motors[motorIndex].pwmPort != NULL);
}
bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)

7
src/main/drivers/pwm_output.h
View file

@ -30,19 +30,20 @@ typedef enum {
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
PWM_TYPE_DSHOT1200,
PWM_TYPE_SERIALSHOT,
} motorPwmProtocolTypes_e;
void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteDshotUpdate(uint8_t motorCount);
bool isMotorProtocolDshot(void);
void pwmCompleteMotorUpdate(void);
bool isMotorProtocolDigital(void);
void pwmWriteServo(uint8_t index, uint16_t value);
void pwmDisableMotors(void);
void pwmEnableMotors(void);
struct timerHardware_s;
bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, motorPwmProtocolTypes_e proto, bool enableOutput);
bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, bool enableOutput);
bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
void pwmWriteBeeper(bool onoffBeep);
void beeperPwmInit(ioTag_t tag, uint16_t frequency);

2
src/main/fc/fc_core.c
View file

@ -722,7 +722,7 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
#endif
#ifdef USE_DSHOT
pwmCompleteDshotUpdate(getMotorCount());
pwmCompleteMotorUpdate();
#endif
}

12
src/main/fc/fc_init.c
View file

@ -317,16 +317,7 @@ void init(void)
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servoPwmRate;
pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol;
#ifndef BRUSHED_MOTORS
pwm_params.useFastPwm = (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT125) ||
(motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT42) ||
(motorConfig()->motorPwmProtocol == PWM_TYPE_MULTISHOT);
#endif
pwm_params.motorPwmRate = motorConfig()->motorPwmRate;
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
pwm_params.useFastPwm = false;
featureClear(FEATURE_3D);
}
@ -351,9 +342,6 @@ void init(void)
mixerPrepare();
if (!pwm_params.useFastPwm)
motorControlEnable = true;
addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE);
systemState |= SYSTEM_STATE_MOTORS_READY;

2
src/main/fc/settings.yaml
View file

@ -32,7 +32,7 @@ tables:
- name: blackbox_device
values: ["SERIAL", "SPIFLASH", "SDCARD"]
- name: motor_pwm_protocol
values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"]
values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"]
- name: failsafe_procedure
values: ["SET-THR", "DROP", "RTH", "NONE"]
- name: current_sensor

2
src/main/flight/mixer.c
View file

@ -175,7 +175,7 @@ void FAST_CODE NOINLINE writeMotors(void)
#ifdef USE_DSHOT
// If we use DSHOT we need to convert motorValue to DSHOT ranges
if (isMotorProtocolDshot()) {
if (isMotorProtocolDigital()) {
const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue;
if (feature(FEATURE_3D)) {

98
src/main/io/esc_serialshot.c Normal file
View file

@ -0,0 +1,98 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it 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.
*
* This file 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 program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <math.h>
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/maths.h"
#include "io/serial.h"
#include "io/esc_serialshot.h"
#if defined(USE_SERIALSHOT)
#define SERIALSHOT_UART_BAUD 921600
#define THROTTLE_DATA_FRAME_SIZE 9
static uint8_t txBuffer[THROTTLE_DATA_FRAME_SIZE];
static serialPort_t * escPort = NULL;
static serialPortConfig_t * portConfig;
bool serialshotInitialize(void)
{
// Avoid double initialization
if (escPort) {
return true;
}
portConfig = findSerialPortConfig(FUNCTION_SERIALSHOT);
if (!portConfig) {
return false;
}
escPort = openSerialPort(portConfig->identifier, FUNCTION_SERIALSHOT, NULL, NULL, SERIALSHOT_UART_BAUD, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_UNIDIR);
if (!escPort) {
return false;
}
return true;
}
void serialshotUpdateMotor(int index, uint16_t value)
{
if (index < 0 && index > 3) {
return;
}
txBuffer[index * 2 + 0] = 0xFF & (value >> 8);
txBuffer[index * 2 + 1] = 0xFF & (value >> 0);
}
void serialshotSendUpdate(void)
{
// Skip update if previous one is not yet fully sent
// This helps to avoid buffer overflow and evenyually the data corruption
if (!isSerialTransmitBufferEmpty(escPort)) {
return;
}
// Calculate checksum
txBuffer[8] =
txBuffer[0] + txBuffer[1] +
txBuffer[2] + txBuffer[3] +
txBuffer[4] + txBuffer[5] +
txBuffer[6] + txBuffer[7];
// Send data
serialWriteBuf(escPort, txBuffer, THROTTLE_DATA_FRAME_SIZE);
}
#endif

29
src/main/io/esc_serialshot.h Normal file
View file

@ -0,0 +1,29 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it 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.
*
* This file 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 program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
bool serialshotInitialize(void);
void serialshotUpdateMotor(int index, uint16_t value);
void serialshotSendUpdate(void);

1
src/main/io/serial.h
View file

@ -49,6 +49,7 @@ typedef enum {
FUNCTION_LOG = (1 << 15), // 32768
FUNCTION_RANGEFINDER = (1 << 16), // 65536
FUNCTION_VTX_FFPV = (1 << 17), // 131072
FUNCTION_SERIALSHOT = (1 << 18), // 262144
} serialPortFunction_e;
typedef enum {

1
src/main/target/MATEKF405/target.h
View file

@ -207,6 +207,7 @@
#define TARGET_IO_PORTD (BIT(2))
#define USE_DSHOT
#define USE_SERIALSHOT
#define MAX_PWM_OUTPUT_PORTS 6

1
src/main/target/OMNIBUSF7NXT/target.h
View file

@ -174,6 +174,7 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define USE_DSHOT
#define USE_SERIALSHOT
// Number of available PWM outputs
#define MAX_PWM_OUTPUT_PORTS 6