mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 03:50:02 +03:00
Added Spektrum SRXL2 serial protocol for new receivers (Currently only SPM4650 released)
This commit is contained in:
Andrey Mironov
Miguel.Alvarez
parent
a01cb0cc7d
commit
8a279e688e
14 changed files with 845 additions and 38 deletions
|
|
@ -110,6 +110,7 @@ COMMON_SRC = \
|
|||
rx/sbus.c \
|
||||
rx/sbus_channels.c \
|
||||
rx/spektrum.c \
|
||||
rx/srxl2.c \
|
||||
io/spektrum_vtx_control.c \
|
||||
io/spektrum_rssi.c \
|
||||
rx/sumd.c \
|
||||
|
|
@ -253,6 +254,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
rx/sbus.c \
|
||||
rx/sbus_channels.c \
|
||||
rx/spektrum.c \
|
||||
rx/srxl2.c \
|
||||
rx/sumd.c \
|
||||
rx/xbus.c \
|
||||
rx/fport.c \
|
||||
|
|
|
|||
|
|
@ -151,6 +151,7 @@ uint8_t cliMode = 0;
|
|||
#include "rx/rx.h"
|
||||
#include "rx/spektrum.h"
|
||||
#include "rx/rx_spi_common.h"
|
||||
#include "rx/srxl2.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
|
|
@ -3251,38 +3252,56 @@ static void cliBeeper(char *cmdline)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RX_SPI
|
||||
void cliRxSpiBind(char *cmdline){
|
||||
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
|
||||
void cliRxBind(char *cmdline){
|
||||
UNUSED(cmdline);
|
||||
switch (rxSpiConfig()->rx_spi_protocol) {
|
||||
default:
|
||||
cliPrint("Not supported.");
|
||||
break;
|
||||
if (featureIsEnabled(FEATURE_RX_SERIAL)) {
|
||||
switch (rxConfig()->serialrx_provider) {
|
||||
default:
|
||||
cliPrint("Not supported.");
|
||||
break;
|
||||
#if defined(USE_SERIALRX_SRXL2)
|
||||
case SERIALRX_SRXL2:
|
||||
srxl2Bind();
|
||||
cliPrint("Binding SRXL2 receiver...");
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#if defined(USE_RX_SPI)
|
||||
else if (featureIsEnabled(FEATURE_RX_SPI)) {
|
||||
switch (rxSpiConfig()->rx_spi_protocol) {
|
||||
default:
|
||||
cliPrint("Not supported.");
|
||||
break;
|
||||
#if defined(USE_RX_FRSKY_SPI)
|
||||
#if defined(USE_RX_FRSKY_SPI_D)
|
||||
case RX_SPI_FRSKY_D:
|
||||
case RX_SPI_FRSKY_D:
|
||||
#endif
|
||||
#if defined(USE_RX_FRSKY_SPI_X)
|
||||
case RX_SPI_FRSKY_X:
|
||||
case RX_SPI_FRSKY_X_LBT:
|
||||
case RX_SPI_FRSKY_X:
|
||||
case RX_SPI_FRSKY_X_LBT:
|
||||
#endif
|
||||
#endif // USE_RX_FRSKY_SPI
|
||||
#ifdef USE_RX_SFHSS_SPI
|
||||
case RX_SPI_SFHSS:
|
||||
case RX_SPI_SFHSS:
|
||||
#endif
|
||||
#ifdef USE_RX_FLYSKY
|
||||
case RX_SPI_A7105_FLYSKY:
|
||||
case RX_SPI_A7105_FLYSKY_2A:
|
||||
case RX_SPI_A7105_FLYSKY:
|
||||
case RX_SPI_A7105_FLYSKY_2A:
|
||||
#endif
|
||||
#ifdef USE_RX_SPEKTRUM
|
||||
case RX_SPI_CYRF6936_DSM:
|
||||
case RX_SPI_CYRF6936_DSM:
|
||||
#endif
|
||||
#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM)
|
||||
rxSpiBind();
|
||||
cliPrint("Binding...");
|
||||
break;
|
||||
rxSpiBind();
|
||||
cliPrint("Binding...");
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -5982,8 +6001,8 @@ const clicmd_t cmdTable[] = {
|
|||
CLI_COMMAND_DEF("beeper", "enable/disable beeper for a condition", "list\r\n"
|
||||
"\t<->[name]", cliBeeper),
|
||||
#endif // USE_BEEPER
|
||||
#ifdef USE_RX_SPI
|
||||
CLI_COMMAND_DEF("bind_rx_spi", "initiate binding for RX SPI", NULL, cliRxSpiBind),
|
||||
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
|
||||
CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind),
|
||||
#endif
|
||||
#if defined(USE_FLASH_BOOT_LOADER)
|
||||
CLI_COMMAND_DEF("bl", "reboot into bootloader", "[flash|rom]", cliBootloader),
|
||||
|
|
|
|||
|
|
@ -224,6 +224,7 @@ static const char * const lookupTableSerialRX[] = {
|
|||
"CUSTOM",
|
||||
"FPORT",
|
||||
"DJI_HDL",
|
||||
"SRXL2",
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
@ -703,6 +704,10 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef USE_SPEKTRUM_BIND
|
||||
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) },
|
||||
{ "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
|
||||
#endif
|
||||
#ifdef USE_SERIALRX_SRXL2
|
||||
{ "srxl2_unit_id", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 0xf }, PG_RX_CONFIG, offsetof(rxConfig_t, srxl2_unit_id) },
|
||||
{ "srxl2_baud_fast", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, srxl2_baud_fast) },
|
||||
#endif
|
||||
{ "airmode_start_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_RX_CONFIG, offsetof(rxConfig_t, airModeActivateThreshold) },
|
||||
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) },
|
||||
|
|
|
|||
|
|
@ -55,24 +55,24 @@ static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, u
|
|||
static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s)
|
||||
{
|
||||
while (*s) {
|
||||
srxlWriteChar(displayPort, col++, row, *(s++));
|
||||
srxlWriteChar(displayPort, col++, row, *(s++));
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int srxlClearScreen(displayPort_t *displayPort)
|
||||
{
|
||||
for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
|
||||
for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
|
||||
for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
|
||||
srxlWriteChar(displayPort, col, row, ' ');
|
||||
}
|
||||
}
|
||||
srxlWriteString(displayPort, 1, 0, "BETAFLIGHT");
|
||||
srxlWriteString(displayPort, 1, 0, "BETAFLIGHT");
|
||||
|
||||
if ( displayPort->grabCount == 0 ) {
|
||||
srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1);
|
||||
srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2);
|
||||
srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3);
|
||||
if (displayPort->grabCount == 0) {
|
||||
srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1);
|
||||
srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2);
|
||||
srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -69,6 +69,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
|
|||
.rc_smoothing_input_type = RC_SMOOTHING_INPUT_BIQUAD,
|
||||
.rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_BIQUAD,
|
||||
.rc_smoothing_auto_factor = 10,
|
||||
.srxl2_unit_id = 1,
|
||||
.srxl2_baud_fast = 1,
|
||||
);
|
||||
|
||||
#ifdef RX_CHANNELS_TAER
|
||||
|
|
|
|||
|
|
@ -61,6 +61,9 @@ typedef struct rxConfig_s {
|
|||
uint8_t rc_smoothing_derivative_type; // Derivative filter type (0 = OFF, 1 = PT1, 2 = BIQUAD)
|
||||
uint8_t rc_smoothing_auto_factor; // Used to adjust the "smoothness" determined by the auto cutoff calculations
|
||||
uint8_t rssi_src_frame_lpf_period; // Period of the cutoff frequency for the source frame RSSI filter (in 0.1 s)
|
||||
|
||||
uint8_t srxl2_unit_id;
|
||||
uint8_t srxl2_baud_fast;
|
||||
} rxConfig_t;
|
||||
|
||||
PG_DECLARE(rxConfig_t, rxConfig);
|
||||
|
|
|
|||
|
|
@ -58,6 +58,7 @@
|
|||
#include "rx/fport.h"
|
||||
#include "rx/sbus.h"
|
||||
#include "rx/spektrum.h"
|
||||
#include "rx/srxl2.h"
|
||||
#include "rx/sumd.h"
|
||||
#include "rx/sumh.h"
|
||||
#include "rx/msp.h"
|
||||
|
|
@ -183,6 +184,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
|
|||
{
|
||||
bool enabled = false;
|
||||
switch (rxConfig->serialrx_provider) {
|
||||
#ifdef USE_SERIALRX_SRXL2
|
||||
case SERIALRX_SRXL2:
|
||||
enabled = srxl2RxInit(rxConfig, rxRuntimeConfig);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_SERIALRX_SPEKTRUM
|
||||
case SERIALRX_SRXL:
|
||||
case SERIALRX_SPEKTRUM1024:
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ typedef enum {
|
|||
SERIALRX_TARGET_CUSTOM = 11,
|
||||
SERIALRX_FPORT = 12,
|
||||
SERIALRX_DJI_HDL_7MS = 13,
|
||||
SERIALRX_SRXL2 = 14,
|
||||
} SerialRXType;
|
||||
|
||||
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12
|
||||
|
|
|
|||
|
|
@ -33,6 +33,8 @@
|
|||
#include "drivers/light_led.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_uart.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
||||
|
|
@ -393,8 +395,10 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
|
|||
NULL,
|
||||
SPEKTRUM_BAUDRATE,
|
||||
portShared || srxlEnabled ? MODE_RXTX : MODE_RX,
|
||||
(rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
|
||||
(rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) |
|
||||
((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
|
||||
);
|
||||
|
||||
#if defined(USE_TELEMETRY_SRXL)
|
||||
if (portShared) {
|
||||
telemetrySharedPort = serialPort;
|
||||
|
|
|
|||
576
src/main/rx/srxl2.c
Normal file
576
src/main/rx/srxl2.c
Normal file
|
|
@ -0,0 +1,576 @@
|
|||
/*
|
||||
* 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 <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_SERIALRX_SRXL2
|
||||
|
||||
#include "common/crc.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/streambuf.h"
|
||||
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_uart.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "rx/srxl2.h"
|
||||
#include "rx/srxl2_types.h"
|
||||
#include "io/spektrum_vtx_control.h"
|
||||
|
||||
#ifndef SRXL2_DEBUG
|
||||
#define SRXL2_DEBUG 0
|
||||
#endif
|
||||
|
||||
#if SRXL2_DEBUG
|
||||
//void cliPrintf(const char *format, ...);
|
||||
//#define DEBUG(format, ...) cliPrintf(format, __VA_ARGS__)
|
||||
#define DEBUG(...) //Temporary until a better debug printf can be included
|
||||
#else
|
||||
#define DEBUG(...)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#define SRXL2_MAX_CHANNELS 32
|
||||
#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
|
||||
#define SRXL2_CHANNEL_SHIFT 5
|
||||
#define SRXL2_CHANNEL_CENTER 0x8000
|
||||
|
||||
#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
|
||||
#define SRXL2_PORT_BAUDRATE_HIGH 400000
|
||||
#define SRXL2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR)
|
||||
#define SRXL2_PORT_MODE MODE_RXTX
|
||||
|
||||
#define SRXL2_REPLY_QUIESCENCE (2 * 10 * 1000000 / SRXL2_PORT_BAUDRATE_DEFAULT) // 2 * (lastIdleTimestamp - lastReceiveTimestamp). Time taken to send 2 bytes
|
||||
|
||||
#define SRXL2_ID 0xA6
|
||||
#define SRXL2_MAX_PACKET_LENGTH 80
|
||||
#define SRXL2_DEVICE_ID_BROADCAST 0xFF
|
||||
|
||||
#define SRXL2_FRAME_TIMEOUT_US 50000
|
||||
|
||||
#define SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US 50000
|
||||
#define SRXL2_SEND_HANDSHAKE_TIMEOUT_US 50000
|
||||
#define SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US 200000
|
||||
|
||||
#define SPEKTRUM_PULSE_OFFSET 988 // Offset value to convert digital data into RC pulse
|
||||
|
||||
typedef union {
|
||||
uint8_t raw[SRXL2_MAX_PACKET_LENGTH];
|
||||
Srxl2Header header;
|
||||
} Srxl2Frame;
|
||||
|
||||
struct rxBuf {
|
||||
volatile unsigned len;
|
||||
Srxl2Frame packet;
|
||||
};
|
||||
|
||||
static uint8_t unitId = 0;
|
||||
static uint8_t baudRate = 0;
|
||||
|
||||
static Srxl2State state = Disabled;
|
||||
static uint32_t timeoutTimestamp = 0;
|
||||
static uint32_t fullTimeoutTimestamp = 0;
|
||||
static uint32_t lastValidPacketTimestamp = 0;
|
||||
static volatile uint32_t lastReceiveTimestamp = 0;
|
||||
static volatile uint32_t lastIdleTimestamp = 0;
|
||||
|
||||
struct rxBuf readBuffer[2];
|
||||
struct rxBuf* readBufferPtr = &readBuffer[0];
|
||||
struct rxBuf* processBufferPtr = &readBuffer[1];
|
||||
static volatile unsigned readBufferIdx = 0;
|
||||
static volatile bool transmittingTelemetry = false;
|
||||
static uint8_t writeBuffer[SRXL2_MAX_PACKET_LENGTH];
|
||||
static unsigned writeBufferIdx = 0;
|
||||
|
||||
static serialPort_t *serialPort;
|
||||
|
||||
static uint8_t busMasterDeviceId = 0xFF;
|
||||
static bool telemetryRequested = false;
|
||||
|
||||
static uint8_t telemetryFrame[22];
|
||||
|
||||
uint8_t globalResult = 0;
|
||||
|
||||
/* handshake protocol
|
||||
1. listen for 50ms for serial activity and go to State::Running if found, autobaud may be necessary
|
||||
2. if srxl2_unitId = 0:
|
||||
send a Handshake with destinationDeviceId = 0 every 50ms for at least 200ms
|
||||
else:
|
||||
listen for Handshake for at least 200ms
|
||||
3. respond to Handshake as currently implemented in process if rePst received
|
||||
4. respond to broadcast Handshake
|
||||
*/
|
||||
|
||||
// if 50ms with not activity, go to default baudrate and to step 1
|
||||
|
||||
bool srxl2ProcessHandshake(const Srxl2Header* header)
|
||||
{
|
||||
const Srxl2HandshakeSubHeader* handshake = (Srxl2HandshakeSubHeader*)(header + 1);
|
||||
if (handshake->destinationDeviceId == Broadcast) {
|
||||
DEBUG("broadcast handshake from %x\r\n", handshake->sourceDeviceId);
|
||||
busMasterDeviceId = handshake->sourceDeviceId;
|
||||
|
||||
if (handshake->baudSupported == 1) {
|
||||
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
|
||||
DEBUG("switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_HIGH);
|
||||
}
|
||||
|
||||
state = Running;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
if (handshake->destinationDeviceId != ((FlightController << 4) | unitId)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
DEBUG("FC handshake from %x\r\n", handshake->sourceDeviceId);
|
||||
|
||||
Srxl2HandshakeFrame response = {
|
||||
.header = *header,
|
||||
.payload = {
|
||||
handshake->destinationDeviceId,
|
||||
handshake->sourceDeviceId,
|
||||
/* priority */ 10,
|
||||
/* baudSupported*/ baudRate,
|
||||
/* info */ 0,
|
||||
// U_ID_2
|
||||
}
|
||||
};
|
||||
|
||||
srxl2RxWriteData(&response, sizeof(response));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void srxl2ProcessChannelData(const Srxl2ChannelDataHeader* channelData, rxRuntimeConfig_t *rxRuntimeConfig) {
|
||||
if (channelData->rssi >= 0) {
|
||||
const int rssiPercent = channelData->rssi;
|
||||
setRssi(scaleRange(rssiPercent, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
|
||||
} else {
|
||||
// If dBm value provided, cant properly convert to % without knowing the receivers sensitivity range. Fix at 50% for now.
|
||||
setRssi(RSSI_MAX_VALUE / 2, RSSI_SOURCE_RX_PROTOCOL);
|
||||
}
|
||||
|
||||
if (channelData->rssi == 0) {
|
||||
globalResult = RX_FRAME_FAILSAFE;
|
||||
} else {
|
||||
globalResult = RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
const uint16_t *frameChannels = (const uint16_t *) (channelData + 1);
|
||||
uint32_t channelMask = channelData->channelMask.u32;
|
||||
while (channelMask) {
|
||||
unsigned idx = __builtin_ctz (channelMask);
|
||||
uint32_t mask = 1 << idx;
|
||||
rxRuntimeConfig->channelData[idx] = *frameChannels++;
|
||||
channelMask &= ~mask;
|
||||
}
|
||||
|
||||
DEBUG("channel data: %d %d %x\r\n", channelData_header->rssi, channelData_header->frameLosses, channelData_header->channelMask.u32);
|
||||
}
|
||||
|
||||
bool srxl2ProcessControlData(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
const Srxl2ControlDataSubHeader* controlData = (Srxl2ControlDataSubHeader*)(header + 1);
|
||||
const uint8_t ownId = (FlightController << 4) | unitId;
|
||||
if (controlData->replyId == ownId) {
|
||||
telemetryRequested = true;
|
||||
DEBUG("command: %x replyId: %x ownId: %x\r\n", controlData->command, controlData->replyId, ownId);
|
||||
}
|
||||
|
||||
switch (controlData->command) {
|
||||
case ChannelData:
|
||||
srxl2ProcessChannelData((const Srxl2ChannelDataHeader *) (controlData + 1), rxRuntimeConfig);
|
||||
break;
|
||||
|
||||
case FailsafeChannelData: {
|
||||
srxl2ProcessChannelData((const Srxl2ChannelDataHeader *) (controlData + 1), rxRuntimeConfig);
|
||||
setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
|
||||
// DEBUG("fs channel data\r\n");
|
||||
} break;
|
||||
|
||||
case VTXData: {
|
||||
#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
|
||||
Srxl2VtxData *vtxData = (Srxl2VtxData*)(controlData + 1);
|
||||
DEBUG("vtx data\r\n");
|
||||
DEBUG("vtx band: %x\r\n", vtxData->band);
|
||||
DEBUG("vtx channel: %x\r\n", vtxData->channel);
|
||||
DEBUG("vtx pit: %x\r\n", vtxData->pit);
|
||||
DEBUG("vtx power: %x\r\n", vtxData->power);
|
||||
DEBUG("vtx powerDec: %x\r\n", vtxData->powerDec);
|
||||
DEBUG("vtx region: %x\r\n", vtxData->region);
|
||||
// Pack data as it was used before srxl2 to use existing functions.
|
||||
// Get the VTX control bytes in a frame
|
||||
uint32_t vtxControl = (0xE0 << 24) | (0xE0 << 8) |
|
||||
((vtxData->band & 0x07) << 21) |
|
||||
((vtxData->channel & 0x0F) << 16) |
|
||||
((vtxData->pit & 0x01) << 4) |
|
||||
((vtxData->region & 0x01) << 3) |
|
||||
((vtxData->power & 0x07));
|
||||
spektrumHandleVtxControl(vtxControl);
|
||||
#endif
|
||||
} break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool srxl2ProcessPacket(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
switch (header->packetType) {
|
||||
case Handshake:
|
||||
return srxl2ProcessHandshake(header);
|
||||
case ControlData:
|
||||
return srxl2ProcessControlData(header, rxRuntimeConfig);
|
||||
default:
|
||||
DEBUG("Other packet type, ID: %x \r\n", header->packetType);
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// @note assumes packet is fully there
|
||||
void srxl2Process(rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
if (processBufferPtr->packet.header.id != SRXL2_ID || processBufferPtr->len != processBufferPtr->packet.header.length) {
|
||||
DEBUG("invalid header id: %x, or length: %x received vs %x expected \r\n", processBufferPtr->packet.header.id, processBufferPtr->len, processBufferPtr->packet.header.length);
|
||||
globalResult = RX_FRAME_DROPPED;
|
||||
return;
|
||||
}
|
||||
|
||||
const uint16_t calculatedCrc = crc16_ccitt_update(0, processBufferPtr->packet.raw, processBufferPtr->packet.header.length);
|
||||
|
||||
//Invalid if crc non-zero
|
||||
if (calculatedCrc) {
|
||||
globalResult = RX_FRAME_DROPPED;
|
||||
DEBUG("crc mismatch %x\r\n", calculatedCrc);
|
||||
return;
|
||||
}
|
||||
|
||||
//Packet is valid only after ID and CRC check out
|
||||
lastValidPacketTimestamp = micros();
|
||||
|
||||
if (srxl2ProcessPacket(&processBufferPtr->packet.header, rxRuntimeConfig)) {
|
||||
return;
|
||||
}
|
||||
|
||||
DEBUG("could not parse packet: %x\r\n", processBufferPtr->packet.header.packetType);
|
||||
globalResult = RX_FRAME_DROPPED;
|
||||
}
|
||||
|
||||
|
||||
static void srxl2DataReceive(uint16_t character, void *data)
|
||||
{
|
||||
UNUSED(data);
|
||||
|
||||
lastReceiveTimestamp = microsISR();
|
||||
|
||||
//If the buffer len is not reset for whatever reason, disable reception
|
||||
if (readBufferPtr->len > 0 || readBufferIdx >= SRXL2_MAX_PACKET_LENGTH) {
|
||||
readBufferIdx = 0;
|
||||
globalResult = RX_FRAME_DROPPED;
|
||||
}
|
||||
else {
|
||||
readBufferPtr->packet.raw[readBufferIdx] = character;
|
||||
readBufferIdx++;
|
||||
}
|
||||
}
|
||||
|
||||
static void srxl2Idle()
|
||||
{
|
||||
if(transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then
|
||||
transmittingTelemetry = false;
|
||||
}
|
||||
else if(readBufferIdx == 0) { // Packet was invalid
|
||||
readBufferPtr->len = 0;
|
||||
}
|
||||
else {
|
||||
lastIdleTimestamp = microsISR();
|
||||
//Swap read and process buffer pointers
|
||||
if(processBufferPtr == &readBuffer[0]) {
|
||||
processBufferPtr = &readBuffer[1];
|
||||
readBufferPtr = &readBuffer[0];
|
||||
} else {
|
||||
processBufferPtr = &readBuffer[0];
|
||||
readBufferPtr = &readBuffer[1];
|
||||
}
|
||||
processBufferPtr->len = readBufferIdx;
|
||||
}
|
||||
|
||||
readBufferIdx = 0;
|
||||
}
|
||||
|
||||
static uint8_t srxl2FrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
UNUSED(rxRuntimeConfig);
|
||||
|
||||
globalResult = RX_FRAME_PENDING;
|
||||
|
||||
// len should only be set after an idle interrupt (packet reception complete)
|
||||
if (processBufferPtr != NULL && processBufferPtr->len) {
|
||||
srxl2Process(rxRuntimeConfig);
|
||||
processBufferPtr->len = 0;
|
||||
}
|
||||
|
||||
uint8_t result = globalResult;
|
||||
|
||||
const uint32_t now = micros();
|
||||
|
||||
switch (state) {
|
||||
case Disabled: break;
|
||||
|
||||
case ListenForActivity: {
|
||||
// activity detected
|
||||
if (lastValidPacketTimestamp != 0) {
|
||||
// as ListenForActivity is done at default baud-rate, we don't need to change anything
|
||||
// @todo if there were non-handshake packets - go to running,
|
||||
// if there were - go to either Send Handshake or Listen For Handshake
|
||||
state = Running;
|
||||
} else if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
|
||||
if (baudRate != 0) {
|
||||
uint32_t currentBaud = serialGetBaudRate(serialPort);
|
||||
|
||||
if(currentBaud == SRXL2_PORT_BAUDRATE_DEFAULT)
|
||||
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
|
||||
else
|
||||
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
|
||||
}
|
||||
} else if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
|
||||
// @todo if there was activity - detect baudrate and ListenForHandshake
|
||||
|
||||
if (unitId == 0) {
|
||||
state = SendHandshake;
|
||||
timeoutTimestamp = now + SRXL2_SEND_HANDSHAKE_TIMEOUT_US;
|
||||
fullTimeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
|
||||
} else {
|
||||
state = ListenForHandshake;
|
||||
timeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
|
||||
}
|
||||
}
|
||||
} break;
|
||||
|
||||
case SendHandshake: {
|
||||
if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
|
||||
// @todo set another timeout for 50ms tries
|
||||
// fill write buffer with handshake frame
|
||||
result |= RX_FRAME_PROCESSING_REQUIRED;
|
||||
}
|
||||
|
||||
if (cmpTimeUs(now, fullTimeoutTimestamp) >= 0) {
|
||||
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
|
||||
DEBUG("case SendHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
|
||||
timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
|
||||
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
|
||||
|
||||
state = ListenForActivity;
|
||||
lastReceiveTimestamp = 0;
|
||||
}
|
||||
} break;
|
||||
|
||||
case ListenForHandshake: {
|
||||
if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
|
||||
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
|
||||
DEBUG("case ListenForHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
|
||||
timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
|
||||
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
|
||||
|
||||
state = ListenForActivity;
|
||||
lastReceiveTimestamp = 0;
|
||||
}
|
||||
} break;
|
||||
|
||||
case Running: {
|
||||
// frame timed out, reset state
|
||||
if (cmpTimeUs(now, lastValidPacketTimestamp) >= SRXL2_FRAME_TIMEOUT_US) {
|
||||
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
|
||||
DEBUG("case Running: switching to %d baud: %d %d\r\n", SRXL2_PORT_BAUDRATE_DEFAULT, now, lastValidPacketTimestamp);
|
||||
timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
|
||||
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
|
||||
|
||||
state = ListenForActivity;
|
||||
lastReceiveTimestamp = 0;
|
||||
lastValidPacketTimestamp = 0;
|
||||
}
|
||||
} break;
|
||||
};
|
||||
|
||||
if (writeBufferIdx) {
|
||||
result |= RX_FRAME_PROCESSING_REQUIRED;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static bool srxl2ProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
UNUSED(rxRuntimeConfig);
|
||||
|
||||
if (writeBufferIdx == 0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const uint32_t now = micros();
|
||||
|
||||
if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
|
||||
// time sufficient for at least 2 characters has passed
|
||||
if (cmpTimeUs(now, lastReceiveTimestamp) > SRXL2_REPLY_QUIESCENCE) {
|
||||
transmittingTelemetry = true;
|
||||
serialWriteBuf(serialPort, writeBuffer, writeBufferIdx);
|
||||
writeBufferIdx = 0;
|
||||
} else {
|
||||
DEBUG("not enough time to send 2 characters passed yet, %d us since last receive, %d required\r\n", now - lastReceiveTimestamp, SRXL2_REPLY_QUIESCENCE);
|
||||
}
|
||||
} else {
|
||||
DEBUG("still receiving a frame, %d %d\r\n", lastIdleTimestamp, lastReceiveTimestamp);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static uint16_t srxl2ReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channelIdx)
|
||||
{
|
||||
if (channelIdx >= rxRuntimeConfig->channelCount) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeConfig->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
|
||||
}
|
||||
|
||||
void srxl2RxWriteData(const void *data, int len)
|
||||
{
|
||||
const uint16_t crc = crc16_ccitt_update(0, (uint8_t*)data, len - 2);
|
||||
((uint8_t*)data)[len-2] = ((uint8_t *) &crc)[1] & 0xFF;
|
||||
((uint8_t*)data)[len-1] = ((uint8_t *) &crc)[0] & 0xFF;
|
||||
|
||||
len = MIN(len, (int)sizeof(writeBuffer));
|
||||
memcpy(writeBuffer, data, len);
|
||||
writeBufferIdx = len;
|
||||
}
|
||||
|
||||
bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
static uint16_t channelData[SRXL2_MAX_CHANNELS];
|
||||
for (size_t i = 0; i < SRXL2_MAX_CHANNELS; ++i) {
|
||||
channelData[i] = SRXL2_CHANNEL_CENTER;
|
||||
}
|
||||
|
||||
unitId = rxConfig->srxl2_unit_id;
|
||||
baudRate = rxConfig->srxl2_baud_fast;
|
||||
|
||||
rxRuntimeConfig->channelData = channelData;
|
||||
rxRuntimeConfig->channelCount = SRXL2_MAX_CHANNELS;
|
||||
rxRuntimeConfig->rxRefreshRate = SRXL2_FRAME_PERIOD_US;
|
||||
|
||||
rxRuntimeConfig->rcReadRawFn = srxl2ReadRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = srxl2FrameStatus;
|
||||
rxRuntimeConfig->rcProcessFrameFn = srxl2ProcessFrame;
|
||||
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
|
||||
if (!portConfig) {
|
||||
return false;
|
||||
}
|
||||
|
||||
portOptions_e options = SRXL2_PORT_OPTIONS;
|
||||
if (rxConfig->serialrx_inverted) {
|
||||
options |= SERIAL_INVERTED;
|
||||
}
|
||||
if (rxConfig->halfDuplex) {
|
||||
options |= SERIAL_BIDIR;
|
||||
}
|
||||
|
||||
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive,
|
||||
NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options);
|
||||
|
||||
if (!serialPort) {
|
||||
return false;
|
||||
}
|
||||
|
||||
serialPort->idleCallback = srxl2Idle;
|
||||
|
||||
state = ListenForActivity;
|
||||
timeoutTimestamp = micros() + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
|
||||
|
||||
if (rssiSource == RSSI_SOURCE_NONE) {
|
||||
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
|
||||
}
|
||||
|
||||
return (bool)serialPort;
|
||||
}
|
||||
|
||||
bool srxl2RxIsActive(void)
|
||||
{
|
||||
return serialPort;
|
||||
}
|
||||
|
||||
bool srxl2TelemetryRequested(void)
|
||||
{
|
||||
return telemetryRequested;
|
||||
}
|
||||
|
||||
void srxl2InitializeFrame(sbuf_t *dst)
|
||||
{
|
||||
dst->ptr = telemetryFrame;
|
||||
dst->end = ARRAYEND(telemetryFrame);
|
||||
|
||||
sbufWriteU8(dst, SRXL2_ID);
|
||||
sbufWriteU8(dst, TelemetrySensorData);
|
||||
sbufWriteU8(dst, ARRAYLEN(telemetryFrame));
|
||||
sbufWriteU8(dst, busMasterDeviceId);
|
||||
}
|
||||
|
||||
void srxl2FinalizeFrame(sbuf_t *dst)
|
||||
{
|
||||
sbufSwitchToReader(dst, telemetryFrame);
|
||||
// Include 2 additional bytes of length since we're letting the srxl2RxWriteData function add the CRC in
|
||||
srxl2RxWriteData(sbufPtr(dst), sbufBytesRemaining(dst) + 2);
|
||||
telemetryRequested = false;
|
||||
}
|
||||
|
||||
void srxl2Bind(void)
|
||||
{
|
||||
const size_t length = sizeof(Srxl2BindInfoFrame);
|
||||
|
||||
Srxl2BindInfoFrame bind = {
|
||||
.header = {
|
||||
.id = SRXL2_ID,
|
||||
.packetType = BindInfo,
|
||||
.length = length
|
||||
},
|
||||
.payload = {
|
||||
.request = EnterBindMode,
|
||||
.deviceId = busMasterDeviceId,
|
||||
.bindType = DMSX_11ms,
|
||||
.options = SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE,
|
||||
}
|
||||
};
|
||||
|
||||
srxl2RxWriteData(&bind, length);
|
||||
}
|
||||
|
||||
#endif
|
||||
17
src/main/rx/srxl2.h
Normal file
17
src/main/rx/srxl2.h
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
#pragma once
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "pg/rx.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
struct sbuf_s;
|
||||
|
||||
bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||
bool srxl2RxIsActive(void);
|
||||
void srxl2RxWriteData(const void *data, int len);
|
||||
bool srxl2TelemetryRequested(void);
|
||||
void srxl2InitializeFrame(struct sbuf_s *dst);
|
||||
void srxl2FinalizeFrame(struct sbuf_s *dst);
|
||||
void srxl2Bind(void);
|
||||
138
src/main/rx/srxl2_types.h
Normal file
138
src/main/rx/srxl2_types.h
Normal file
|
|
@ -0,0 +1,138 @@
|
|||
#pragma once
|
||||
|
||||
#define PACKED __attribute__((packed))
|
||||
|
||||
typedef enum {
|
||||
Disabled,
|
||||
ListenForActivity,
|
||||
SendHandshake,
|
||||
ListenForHandshake,
|
||||
Running
|
||||
} Srxl2State;
|
||||
|
||||
typedef enum {
|
||||
Handshake = 0x21,
|
||||
BindInfo = 0x41,
|
||||
ParameterConfiguration = 0x50,
|
||||
SignalQuality = 0x55,
|
||||
TelemetrySensorData = 0x80,
|
||||
ControlData = 0xCD,
|
||||
} Srxl2PacketType;
|
||||
|
||||
typedef struct {
|
||||
uint8_t id;
|
||||
uint8_t packetType;
|
||||
uint8_t length;
|
||||
} PACKED Srxl2Header;
|
||||
|
||||
typedef struct {
|
||||
uint8_t sourceDeviceId;
|
||||
uint8_t destinationDeviceId;
|
||||
uint8_t priority;
|
||||
uint8_t baudSupported;
|
||||
uint8_t info;
|
||||
uint32_t uniqueId;
|
||||
} PACKED Srxl2HandshakeSubHeader;
|
||||
|
||||
typedef struct {
|
||||
uint8_t command;
|
||||
uint8_t replyId;
|
||||
} PACKED Srxl2ControlDataSubHeader;
|
||||
|
||||
typedef enum {
|
||||
ChannelData = 0x00,
|
||||
FailsafeChannelData = 0x01,
|
||||
VTXData = 0x02,
|
||||
} Srxl2ControlDataCommand;
|
||||
|
||||
typedef struct {
|
||||
int8_t rssi;
|
||||
uint16_t frameLosses;
|
||||
union {
|
||||
//struct {
|
||||
// uint8_t channels_0_7;
|
||||
// uint8_t channels_8_15;
|
||||
// uint8_t channels_16_23;
|
||||
// uint8_t channels_24_31;
|
||||
//} u8;
|
||||
uint8_t u8[4];
|
||||
uint32_t u32;
|
||||
} channelMask;
|
||||
} PACKED Srxl2ChannelDataHeader;
|
||||
|
||||
typedef enum {
|
||||
NoDevice = 0,
|
||||
RemoteReceiver = 1,
|
||||
Receiver = 2,
|
||||
FlightController = 3,
|
||||
ESC = 4,
|
||||
Reserved = 5,
|
||||
SRXLServo = 6,
|
||||
SRXLServo_2 = 7,
|
||||
VTX = 8,
|
||||
} Srxl2DeviceType;
|
||||
|
||||
typedef enum {
|
||||
FlightControllerDefault = 0x30,
|
||||
FlightControllerMax = 0x3F,
|
||||
Broadcast = 0xFF,
|
||||
} Srxl2DeviceId;
|
||||
|
||||
typedef struct {
|
||||
Srxl2Header header;
|
||||
Srxl2HandshakeSubHeader payload;
|
||||
uint8_t crcHigh;
|
||||
uint8_t crcLow;
|
||||
} PACKED Srxl2HandshakeFrame;
|
||||
|
||||
typedef enum {
|
||||
EnterBindMode = 0xEB,
|
||||
RequestBindStatus = 0xB5,
|
||||
BoundDataReport = 0xDB,
|
||||
SetBindInfo = 0x5B,
|
||||
} Srxl2BindRequest;
|
||||
|
||||
typedef enum {
|
||||
NotBound = 0x0,
|
||||
DSM2_1024_22ms = 0x01,
|
||||
DSM2_1024_MC24 = 0x02,
|
||||
DMS2_2048_11ms = 0x12,
|
||||
DMSX_22ms = 0xA2,
|
||||
DMSX_11ms = 0xB2,
|
||||
Surface_DSM2_16_5ms = 0x63,
|
||||
DSMR_11ms_22ms = 0xE2,
|
||||
DSMR_5_5ms = 0xE4,
|
||||
} Srxl2BindType;
|
||||
|
||||
// Bit masks for Options byte
|
||||
#define SRXL_BIND_OPT_NONE (0x00)
|
||||
#define SRXL_BIND_OPT_TELEM_TX_ENABLE (0x01) // Set if this device should be enabled as the current telemetry device to tx over RF
|
||||
#define SRXL_BIND_OPT_BIND_TX_ENABLE (0x02) // Set if this device should reply to a bind request with a Discover packet over RF
|
||||
|
||||
typedef struct {
|
||||
uint8_t request;
|
||||
uint8_t deviceId;
|
||||
uint8_t bindType;
|
||||
uint8_t options;
|
||||
uint64_t guid;
|
||||
uint32_t uid;
|
||||
} PACKED Srxl2BindInfoPayload;
|
||||
|
||||
typedef struct {
|
||||
Srxl2Header header;
|
||||
Srxl2BindInfoPayload payload;
|
||||
uint8_t crcHigh;
|
||||
uint8_t crcLow;
|
||||
} PACKED Srxl2BindInfoFrame;
|
||||
|
||||
// VTX Data
|
||||
typedef struct {
|
||||
uint8_t band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
|
||||
uint8_t channel; // VTX Channel (0-7)
|
||||
uint8_t pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode.
|
||||
uint8_t power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
|
||||
uint16_t powerDec; // VTX Power as a decimal 1mw/unit
|
||||
uint8_t region; // Region (0 = USA, 1 = EU)
|
||||
} PACKED Srxl2VtxData;
|
||||
|
||||
#undef PACKED
|
||||
|
|
@ -333,4 +333,5 @@
|
|||
#define USE_VTX_TABLE
|
||||
#define USE_PERSISTENT_STATS
|
||||
#define USE_PROFILE_NAMES
|
||||
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/spektrum.h"
|
||||
#include "rx/srxl2.h"
|
||||
#include "io/spektrum_vtx_control.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
|
@ -83,24 +84,37 @@
|
|||
#define SRXL_FRAMETYPE_GPS_STAT 0x17
|
||||
|
||||
static bool srxlTelemetryEnabled;
|
||||
static bool srxl2 = false;
|
||||
static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
|
||||
|
||||
static void srxlInitializeFrame(sbuf_t *dst)
|
||||
{
|
||||
dst->ptr = srxlFrame;
|
||||
dst->end = ARRAYEND(srxlFrame);
|
||||
if (srxl2) {
|
||||
#if defined(USE_SERIALRX_SRXL2)
|
||||
srxl2InitializeFrame(dst);
|
||||
#endif
|
||||
} else {
|
||||
dst->ptr = srxlFrame;
|
||||
dst->end = ARRAYEND(srxlFrame);
|
||||
|
||||
sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
|
||||
sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
|
||||
sbufWriteU8(dst, SRXL_PACKET_LENGTH);
|
||||
sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
|
||||
sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
|
||||
sbufWriteU8(dst, SRXL_PACKET_LENGTH);
|
||||
}
|
||||
}
|
||||
|
||||
static void srxlFinalize(sbuf_t *dst)
|
||||
{
|
||||
crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
|
||||
sbufSwitchToReader(dst, srxlFrame);
|
||||
// write the telemetry frame to the receiver.
|
||||
srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
|
||||
if (srxl2) {
|
||||
#if defined(USE_SERIALRX_SRXL2)
|
||||
srxl2FinalizeFrame(dst);
|
||||
#endif
|
||||
} else {
|
||||
crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
|
||||
sbufSwitchToReader(dst, srxlFrame);
|
||||
// write the telemetry frame to the receiver.
|
||||
srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -760,7 +774,18 @@ void initSrxlTelemetry(void)
|
|||
{
|
||||
// check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX)
|
||||
// and feature is enabled, if so, set SRXL telemetry enabled
|
||||
srxlTelemetryEnabled = srxlRxIsActive();
|
||||
if (srxlRxIsActive()) {
|
||||
srxlTelemetryEnabled = true;
|
||||
srxl2 = false;
|
||||
#if defined(USE_SERIALRX_SRXL2)
|
||||
} else if (srxl2RxIsActive()) {
|
||||
srxlTelemetryEnabled = true;
|
||||
srxl2 = true;
|
||||
#endif
|
||||
} else {
|
||||
srxlTelemetryEnabled = false;
|
||||
srxl2 = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool checkSrxlTelemetryState(void)
|
||||
|
|
@ -773,8 +798,16 @@ bool checkSrxlTelemetryState(void)
|
|||
*/
|
||||
void handleSrxlTelemetry(timeUs_t currentTimeUs)
|
||||
{
|
||||
if (srxlTelemetryBufferEmpty()) {
|
||||
processSrxl(currentTimeUs);
|
||||
if (srxl2) {
|
||||
#if defined(USE_SERIALRX_SRXL2)
|
||||
if (srxl2TelemetryRequested()) {
|
||||
processSrxl(currentTimeUs);
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
if (srxlTelemetryBufferEmpty()) {
|
||||
processSrxl(currentTimeUs);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue