Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Added Spektrum SRXL2 support. (#5791)

Browse source 
This consists of various files/changes brought over from betaflight with modifications to operate in the current state of inav.
It also includes files/changes that were not a part of the betaflight SRXL2 merge, as the previous bidi srxl implementation was not yet implemented either, and SRXL2 has some dependencies on the Spektrum telemetry structuring from those files.
This commit is contained in:
MiguelFAlvarez 2020-10-23 01:13:18 -05:00 • committed by GitHub
parent ca97a8e0e4
commit a88bab5f91
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
17 changed files with 1958 additions and 47 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/CMakeLists.txt
View file

@ -401,6 +401,8 @@ main_sources(COMMON_SRC
rx/sbus_channels.h
rx/spektrum.c
rx/spektrum.h
rx/srxl2.c
rx/srxl2.h
rx/sumd.c
rx/sumd.h
rx/sumh.c
@ -486,6 +488,8 @@ main_sources(COMMON_SRC
io/displayport_msp.h
io/displayport_oled.c
io/displayport_oled.h
io/displayport_srxl.c
io/displayport_srxl.h
io/displayport_hott.c
io/displayport_hott.h
io/flashfs.c
@ -547,6 +551,8 @@ main_sources(COMMON_SRC
telemetry/crsf.c
telemetry/crsf.h
telemetry/srxl.c
telemetry/srxl.h
telemetry/frsky.c
telemetry/frsky.h
telemetry/frsky_d.c

33
src/main/fc/cli.c
View file

@ -103,6 +103,7 @@ extern uint8_t __config_end;
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/eleres.h"
#include "rx/srxl2.h"
#include "scheduler/scheduler.h"
@ -2578,6 +2579,35 @@ static void cliEleresBind(char *cmdline)
}
#endif // USE_RX_ELERES
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
void cliRxBind(char *cmdline){
UNUSED(cmdline);
if (rxConfig()->receiverType == RX_TYPE_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 (rxConfig()->receiverType == RX_TYPE_SPI) {
switch (rxConfig()->rx_spi_protocol) {
default:
cliPrint("Not supported.");
break;
}
}
#endif
}
#endif
static void cliExit(char *cmdline)
{
UNUSED(cmdline);
@ -3465,6 +3495,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
#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_BOOTLOG)
CLI_COMMAND_DEF("bootlog", "show boot events", NULL, cliBootlog),
#endif

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

@ -107,6 +107,7 @@
#include "io/displayport_frsky_osd.h"
#include "io/displayport_msp.h"
#include "io/displayport_max7456.h"
#include "io/displayport_srxl.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -573,6 +574,11 @@ void init(void)
uavInterconnectBusInit();
#endif
#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
// Register the srxl Textgen telemetry sensor as a displayport device
cmsDisplayPortRegister(displayPortSrxlInit());
#endif
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
gpsInit();

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

@ -25,7 +25,7 @@ tables:
values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2"]
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2"]
- name: rx_spi_protocol
values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
enum: rx_spi_protocol_e
@ -144,6 +144,8 @@ tables:
- name: tristate
enum: tristate_e
values: ["AUTO", "ON", "OFF"]
- name: off_on
values: ["OFF", "ON"]
groups:
- name: PG_GYRO_CONFIG
@ -567,6 +569,13 @@ groups:
condition: USE_SPEKTRUM_BIND
min: SPEKTRUM_SAT_BIND_DISABLED
max: SPEKTRUM_SAT_BIND_MAX
- name: srxl2_unit_id
condition: USE_SERIALRX_SRXL2
min: 0
max: 15
- name: srxl2_baud_fast
condition: USE_SERIALRX_SRXL2
table: off_on
- name: rx_min_usec
description: "Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc."
default_value: "885"

152
src/main/io/displayport_srxl.c Normal file
View file

@ -0,0 +1,152 @@
/*
* 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 <string.h>
#include "platform.h"
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
#include "common/utils.h"
#include "drivers/display.h"
#include "cms/cms.h"
#include "telemetry/srxl.h"
displayPort_t srxlDisplayPort;
static int srxlDrawScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static int srxlScreenSize(const displayPort_t *displayPort)
{
return displayPort->rows * displayPort->cols;
}
static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
{
return (spektrumTmTextGenPutChar(col, row, c));
UNUSED(displayPort);
UNUSED(attr);
}
static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s, textAttributes_t attr)
{
while (*s) {
srxlWriteChar(displayPort, col++, row, *(s++), attr);
}
return 0;
}
static int srxlClearScreen(displayPort_t *displayPort)
{
textAttributes_t attr = 0;
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, ' ', attr);
}
}
srxlWriteString(displayPort, 1, 0, "INAV", attr);
if (displayPort->grabCount == 0) {
srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1, attr);
srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2, attr);
srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3, attr);
}
return 0;
}
static bool srxlIsTransferInProgress(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return false;
}
/*
static bool srxlIsSynced(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return true;
}
*/
static int srxlHeartbeat(displayPort_t *displayPort)
{
UNUSED(displayPort);
return 0;
}
static void srxlResync(displayPort_t *displayPort)
{
UNUSED(displayPort);
}
static uint32_t srxlTxBytesFree(const displayPort_t *displayPort)
{
UNUSED(displayPort);
return UINT32_MAX;
}
static int srxlGrab(displayPort_t *displayPort)
{
return displayPort->grabCount = 1;
}
static int srxlRelease(displayPort_t *displayPort)
{
int cnt = displayPort->grabCount = 0;
srxlClearScreen(displayPort);
return cnt;
}
static const displayPortVTable_t srxlVTable = {
.grab = srxlGrab,
.release = srxlRelease,
.clearScreen = srxlClearScreen,
.drawScreen = srxlDrawScreen,
.screenSize = srxlScreenSize,
.writeString = srxlWriteString,
.writeChar = srxlWriteChar,
.isTransferInProgress = srxlIsTransferInProgress,
.heartbeat = srxlHeartbeat,
.resync = srxlResync,
//.isSynced = srxlIsSynced,
.txBytesFree = srxlTxBytesFree,
//.layerSupported = NULL,
//.layerSelect = NULL,
//.layerCopy = NULL,
};
displayPort_t *displayPortSrxlInit(void)
{
srxlDisplayPort.device = NULL;
displayInit(&srxlDisplayPort, &srxlVTable);
srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
return &srxlDisplayPort;
}
#endif

25
src/main/io/displayport_srxl.h Normal file
View file

@ -0,0 +1,25 @@
/*
* 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
displayPort_t *displayPortSrxlInit(void);
extern displayPort_t srxlDisplayPort;

17
src/main/rx/rx.c
View file

@ -61,6 +61,7 @@
#include "rx/rx_spi.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
#include "rx/srxl2.h"
#include "rx/sumd.h"
#include "rx/sumh.h"
#include "rx/uib_rx.h"
@ -91,6 +92,7 @@ static bool mspOverrideDataProcessingRequired = false;
static bool rxSignalReceived = false;
static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
static uint8_t rxChannelCount;
static timeUs_t rxNextUpdateAtUs = 0;
static timeUs_t needRxSignalBefore = 0;
@ -141,6 +143,8 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.mspOverrideChannels = 15,
#endif
.rssi_source = RSSI_SOURCE_AUTO,
.srxl2_unit_id = 1,
.srxl2_baud_fast = 1,
);
void resetAllRxChannelRangeConfigurations(void)
@ -187,6 +191,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_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
@ -347,6 +356,8 @@ void rxInit(void)
mspOverrideInit();
}
#endif
rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
}
void rxUpdateRSSISource(void)
@ -516,7 +527,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
rxFlightChannelsValid = true;
// Read and process channel data
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
for (int channel = 0; channel < rxChannelCount; channel++) {
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
// sample the channel
@ -549,11 +560,11 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
// If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
if (rxFlightChannelsValid && rxSignalReceived) {
if (rxRuntimeConfig.requireFiltering) {
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
for (int channel = 0; channel < rxChannelCount; channel++) {
rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]);
}
} else {
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
for (int channel = 0; channel < rxChannelCount; channel++) {
rcChannels[channel].data = rcStaging[channel];
}
}

3
src/main/rx/rx.h
View file

@ -82,6 +82,7 @@ typedef enum {
SERIALRX_FPORT = 10,
SERIALRX_SBUS_FAST = 11,
SERIALRX_FPORT2 = 12,
SERIALRX_SRXL2 = 13,
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
@ -128,6 +129,8 @@ typedef struct rxConfig_s {
uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness)
uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active
uint8_t rssi_source;
uint8_t srxl2_unit_id;
uint8_t srxl2_baud_fast;
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);

129
src/main/rx/spektrum.c
View file

@ -15,10 +15,13 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "common/maths.h"
#include "platform.h"
#ifdef USE_SERIALRX_SPEKTRUM
@ -44,19 +47,13 @@
#include "rx/spektrum.h"
// driver for spektrum satellite receiver / sbus
#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
#define SPEKTRUM_2048_CHANNEL_COUNT 12
#define SPEKTRUM_1024_CHANNEL_COUNT 7
#define SPEK_FRAME_SIZE 16
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_BAUDRATE 115200
#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
#define SPEKTRUM_MAX_FADE_PER_SEC 40
#define SPEKTRUM_FADE_REPORTS_PER_SEC 2
bool srxlEnabled = false;
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = false;
@ -81,6 +78,10 @@ static IO_t BindPin = DEFIO_IO(NONE);
static IO_t BindPlug = DEFIO_IO(NONE);
#endif
#if defined(USE_TELEMETRY_SRXL)
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0;
#endif
// Receive ISR callback
static void spektrumDataReceive(uint16_t c, void *rxCallbackData)
@ -116,46 +117,67 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!rcFrameComplete) {
return RX_FRAME_PENDING;
}
#if defined(USE_TELEMETRY_SRXL)
static timeUs_t telemetryFrameRequestedUs = 0;
rcFrameComplete = false;
timeUs_t currentTimeUs = micros();
#endif
// Fetch the fade count
const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1];
const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
uint8_t result = RX_FRAME_PENDING;
if (spek_fade_last_sec == 0) {
// This is the first frame status received.
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
} else if (spek_fade_last_sec != current_secs) {
// If the difference is > 1, then we missed several seconds worth of frames and
// should just throw out the fade calc (as it's likely a full signal loss).
if ((current_secs - spek_fade_last_sec) == 1) {
if (rssi_channel != 0) {
if (spekHiRes)
spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
else
spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
if (rcFrameComplete) {
rcFrameComplete = false;
// Fetch the fade count
const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1];
const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
if (spek_fade_last_sec == 0) {
// This is the first frame status received.
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
} else if (spek_fade_last_sec != current_secs) {
// If the difference is > 1, then we missed several seconds worth of frames and
// should just throw out the fade calc (as it's likely a full signal loss).
if ((current_secs - spek_fade_last_sec) == 1) {
if (rssi_channel != 0) {
if (spekHiRes)
spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
else
spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
}
}
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
}
for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
if (rssi_channel == 0 || spekChannel != rssi_channel) {
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
}
}
}
spek_fade_last_sec_count = fade;
spek_fade_last_sec = current_secs;
}
for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
if (rssi_channel == 0 || spekChannel != rssi_channel) {
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
}
#if defined(USE_TELEMETRY_SRXL)
if (srxlEnabled && (spekFrame[2] & 0x80) == 0) {
telemetryFrameRequestedUs = currentTimeUs;
}
}
#endif
return RX_FRAME_COMPLETE;
result = RX_FRAME_COMPLETE;
}
#if defined(USE_TELEMETRY_SRXL)
if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
telemetryFrameRequestedUs = 0;
result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
}
#endif
return result;
}
static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
@ -251,6 +273,25 @@ void spektrumBind(rxConfig_t *rxConfig)
}
#endif // SPEKTRUM_BIND
#if defined(USE_TELEMETRY_SRXL)
bool srxlTelemetryBufferEmpty(void)
{
if (telemetryBufLen == 0) {
return true;
} else {
return false;
}
}
void srxlRxWriteTelemetryData(const void *data, int len)
{
len = MIN(len, (int)sizeof(telemetryBuf));
memcpy(telemetryBuf, data, len);
telemetryBufLen = len;
}
#endif
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfigPtr = rxRuntimeConfig;
@ -310,4 +351,10 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
return serialPort != NULL;
}
bool srxlRxIsActive(void)
{
return serialPort != NULL;
}
#endif // USE_SERIALRX_SPEKTRUM

22
src/main/rx/spektrum.h
View file

@ -17,8 +17,26 @@
#pragma once
#define SPEKTRUM_SAT_BIND_DISABLED 0
#define SPEKTRUM_SAT_BIND_MAX 10
#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
#define SPEKTRUM_2048_CHANNEL_COUNT 12
#define SPEKTRUM_1024_CHANNEL_COUNT 7
#define SPEKTRUM_SAT_BIND_DISABLED 0
#define SPEKTRUM_SAT_BIND_MAX 10
#define SPEK_FRAME_SIZE 16
#define SRXL_FRAME_OVERHEAD 5
#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_BAUDRATE 115200
extern bool srxlEnabled;
void spektrumBind(rxConfig_t *rxConfig);
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
bool srxlTelemetryBufferEmpty(void);
void srxlRxWriteTelemetryData(const void *data, int len);
bool srxlRxIsActive(void);

580
src/main/rx/srxl2.c Normal file
View file

@ -0,0 +1,580 @@
/*
* 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"
#ifndef SRXL2_DEBUG
#define SRXL2_DEBUG 0
#endif
#if SRXL2_DEBUG
//void cliPrintf(const char *format, ...);
//#define DEBUG_PRINTF(format, ...) cliPrintf(format, __VA_ARGS__)
#define DEBUG_PRINTF(...) //Temporary until a better debug printf can be included
#else
#define DEBUG_PRINTF(...)
#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_PRINTF("broadcast handshake from %x\r\n", handshake->sourceDeviceId);
busMasterDeviceId = handshake->sourceDeviceId;
if (handshake->baudSupported == 1) {
serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
DEBUG_PRINTF("switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_HIGH);
}
state = Running;
return true;
}
if (handshake->destinationDeviceId != ((FlightController << 4) | unitId)) {
return true;
}
DEBUG_PRINTF("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) {
globalResult = RX_FRAME_COMPLETE;
if (channelData->rssi >= 0) {
const int rssiPercent = channelData->rssi;
lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiPercent, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
//setRssi(scaleRange(rssiPercent, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
}
//If receiver is in a connected state, and a packet is missed, the channel mask will be 0.
if (!channelData->channelMask.u32) {
globalResult |= RX_FRAME_FAILSAFE;
return;
}
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_PRINTF("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_PRINTF("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: {
globalResult |= RX_FRAME_FAILSAFE;
//setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
// DEBUG_PRINTF("fs channel data\r\n");
} break;
case VTXData: {
#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
Srxl2VtxData *vtxData = (Srxl2VtxData*)(controlData + 1);
DEBUG_PRINTF("vtx data\r\n");
DEBUG_PRINTF("vtx band: %x\r\n", vtxData->band);
DEBUG_PRINTF("vtx channel: %x\r\n", vtxData->channel);
DEBUG_PRINTF("vtx pit: %x\r\n", vtxData->pit);
DEBUG_PRINTF("vtx power: %x\r\n", vtxData->power);
DEBUG_PRINTF("vtx powerDec: %x\r\n", vtxData->powerDec);
DEBUG_PRINTF("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_PRINTF("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_PRINTF("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_PRINTF("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_PRINTF("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(void)
{
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);
if(serialIsIdle(serialPort))
{
srxl2Idle();
}
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_PRINTF("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_PRINTF("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_PRINTF("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_PRINTF("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_PRINTF("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_t 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;
}
state = ListenForActivity;
timeoutTimestamp = micros() + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
//Looks like this needs to be set in cli config
//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

15
src/main/rx/srxl2.h Normal file
View file

@ -0,0 +1,15 @@
#pragma once
#include <stdint.h>
#include <stdbool.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
View 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

8
src/main/target/common.h
View file

@ -124,6 +124,14 @@
#define USE_I2C_IO_EXPANDER
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
#define USE_TELEMETRY_SRXL
#define USE_SPEKTRUM_CMS_TELEMETRY
//#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented
#define USE_SPEKTRUM_VTX_TELEMETRY
#define USE_VTX_COMMON
#else // FLASH_SIZE < 256
#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
#endif

812
src/main/telemetry/srxl.c Normal file
View file

@ -0,0 +1,812 @@
/*
* 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 <string.h>
#include "platform.h"
#if defined(USE_TELEMETRY_SRXL)
#include "build/version.h"
#include "cms/cms.h"
#include "io/displayport_srxl.h"
#include "common/crc.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "config/feature.h"
#include "io/gps.h"
#include "io/serial.h"
//#include "config/config.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "io/gps.h"
//#include "pg/motor.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/srxl2.h"
#include "sensors/battery.h"
//#include "sensors/adcinternal.h"
#include "sensors/esc_sensor.h"
#include "telemetry/telemetry.h"
#include "telemetry/srxl.h"
#include "drivers/vtx_common.h"
//#include "drivers/dshot.h"
#include "io/vtx_tramp.h"
#include "io/vtx_smartaudio.h"
#define SRXL_ADDRESS_FIRST 0xA5
#define SRXL_ADDRESS_SECOND 0x80
#define SRXL_PACKET_LENGTH 0x15
#define SRXL_FRAMETYPE_TELE_QOS 0x7F
#define SRXL_FRAMETYPE_TELE_RPM 0x7E
#define SRXL_FRAMETYPE_POWERBOX 0x0A
#define SRXL_FRAMETYPE_TELE_FP_MAH 0x34
#define TELE_DEVICE_VTX 0x0D // Video Transmitter Status
#define SRXL_FRAMETYPE_SID 0x00
#define SRXL_FRAMETYPE_GPS_LOC 0x16 // GPS Location Data (Eagle Tree)
#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)
{
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);
}
}
static void srxlFinalize(sbuf_t *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));
}
}
/*
SRXL frame has the structure:
<0xA5><0x80><Length><16-byte telemetry packet><2 Byte CRC of payload>
The <Length> shall be 0x15 (length of the 16-byte telemetry packet + overhead).
*/
/*
typedef struct
{
UINT8 identifier; // Source device = 0x7F
UINT8 sID; // Secondary ID
UINT16 A;
UINT16 B;
UINT16 L;
UINT16 R;
UINT16 F;
UINT16 H;
UINT16 rxVoltage; // Volts, 0.01V increments
} STRU_TELE_QOS;
*/
#define STRU_TELE_QOS_EMPTY_FIELDS_COUNT 14
#define STRU_TELE_QOS_EMPTY_FIELDS_VALUE 0xff
bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder
// Mandatory frame, send it unconditionally.
return true;
}
/*
typedef struct
{
UINT8 identifier; // Source device = 0x7E
UINT8 sID; // Secondary ID
UINT16 microseconds; // microseconds between pulse leading edges
UINT16 volts; // 0.01V increments
INT16 temperature; // degrees F
INT8 dBm_A, // Average signal for A antenna in dBm
INT8 dBm_B; // Average signal for B antenna in dBm.
// If only 1 antenna, set B = A
} STRU_TELE_RPM;
*/
#define STRU_TELE_RPM_EMPTY_FIELDS_COUNT 8
#define STRU_TELE_RPM_EMPTY_FIELDS_VALUE 0xff
#define SPEKTRUM_RPM_UNUSED 0xffff
#define SPEKTRUM_TEMP_UNUSED 0x7fff
#define MICROSEC_PER_MINUTE 60000000
//Original range of 1 - 65534 uSec gives an RPM range of 915 - 60000000rpm, 60MegaRPM
#define SPEKTRUM_MIN_RPM 999 // Min RPM to show the user, indicating RPM is really below 999
#define SPEKTRUM_MAX_RPM 60000000
uint16_t getMotorAveragePeriod(void)
{
#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY)
uint32_t rpm = 0;
uint16_t period_us = SPEKTRUM_RPM_UNUSED;
#if defined( USE_ESC_SENSOR_TELEMETRY)
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData != NULL) {
rpm = escData->rpm;
}
#endif
#if defined(USE_DSHOT_TELEMETRY)
if (useDshotTelemetry) {
uint16_t motors = getMotorCount();
if (motors > 0) {
for (int motor = 0; motor < motors; motor++) {
rpm += getDshotTelemetry(motor);
}
rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm; // convert erpm freq to RPM.
rpm /= motors; // Average combined rpm
}
}
#endif
if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) {
period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds
} else {
period_us = MICROSEC_PER_MINUTE / SPEKTRUM_MIN_RPM;
}
return period_us;
#else
return SPEKTRUM_RPM_UNUSED;
#endif
}
bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
{
int16_t coreTemp = SPEKTRUM_TEMP_UNUSED;
#if defined(USE_ADC_INTERNAL)
coreTemp = getCoreTemperatureCelsius();
coreTemp = coreTemp * 9 / 5 + 32; // C -> F
#endif
UNUSED(currentTimeUs);
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16BigEndian(dst, getMotorAveragePeriod()); // pulse leading edges
if (telemetryConfig()->report_cell_voltage) {
sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V
} else {
sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V
}
sbufWriteU16BigEndian(dst, coreTemp); // temperature
sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);
// Mandatory frame, send it unconditionally.
return true;
}
#if defined(USE_GPS)
// From Frsky implementation
static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
{
int32_t absgps, deg, min;
absgps = ABS(mwiigps);
deg = absgps / GPS_DEGREES_DIVIDER;
absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
min = absgps / GPS_DEGREES_DIVIDER; // minutes left
result->dddmm = deg * 100 + min;
result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
}
// BCD conversion
static uint32_t dec2bcd(uint16_t dec)
{
uint32_t result = 0;
uint8_t counter = 0;
while (dec) {
result |= (dec % 10) << counter * 4;
counter++;
dec /= 10;
}
return result;
}
/*
typedef struct
{
UINT8 identifier; // Source device = 0x16
UINT8 sID; // Secondary ID
UINT16 altitudeLow; // BCD, meters, format 3.1 (Low order of altitude)
UINT32 latitude; // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
UINT32 longitude; // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
UINT16 course; // BCD, 3.1
UINT8 HDOP; // BCD, format 1.1
UINT8 GPSflags; // see definitions below
} STRU_TELE_GPS_LOC;
*/
// GPS flags definitions
#define GPS_FLAGS_IS_NORTH_BIT 0x01
#define GPS_FLAGS_IS_EAST_BIT 0x02
#define GPS_FLAGS_LONGITUDE_GREATER_99_BIT 0x04
#define GPS_FLAGS_GPS_FIX_VALID_BIT 0x08
#define GPS_FLAGS_GPS_DATA_RECEIVED_BIT 0x10
#define GPS_FLAGS_3D_FIX_BIT 0x20
#define GPS_FLAGS_NEGATIVE_ALT_BIT 0x80
bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
gpsCoordinateDDDMMmmmm_t coordinate;
uint32_t latitudeBcd, longitudeBcd, altitudeLo;
uint16_t altitudeLoBcd, groundCourseBcd, hdop;
uint8_t hdopBcd, gpsFlags;
if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
return false;
}
// lattitude
GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate);
latitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
// longitude
GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate);
longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
// altitude (low order)
altitudeLo = ABS(gpsSol.llh.alt) / 10;
altitudeLoBcd = dec2bcd(altitudeLo % 100000);
// Ground course
groundCourseBcd = dec2bcd(gpsSol.groundCourse);
// HDOP
hdop = gpsSol.hdop / 10;
hdop = (hdop > 99) ? 99 : hdop;
hdopBcd = dec2bcd(hdop);
// flags
gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT;
gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0;
gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0;
gpsFlags |= (gpsSol.llh.alt < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0;
gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0;
// SRXL frame
sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16(dst, altitudeLoBcd);
sbufWriteU32(dst, latitudeBcd);
sbufWriteU32(dst, longitudeBcd);
sbufWriteU16(dst, groundCourseBcd);
sbufWriteU8(dst, hdopBcd);
sbufWriteU8(dst, gpsFlags);
return true;
}
/*
typedef struct
{
UINT8 identifier; // Source device = 0x17
UINT8 sID; // Secondary ID
UINT16 speed; // BCD, knots, format 3.1
UINT32 UTC; // BCD, format HH:MM:SS.S, format 6.1
UINT8 numSats; // BCD, 0-99
UINT8 altitudeHigh; // BCD, meters, format 2.0 (High bits alt)
} STRU_TELE_GPS_STAT;
*/
#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE 0xff
#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT 6
#define SPEKTRUM_TIME_UNKNOWN 0xFFFFFFFF
bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
uint32_t timeBcd;
uint16_t speedKnotsBcd, speedTmp;
uint8_t numSatBcd, altitudeHighBcd;
bool timeProvided = false;
if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
return false;
}
// Number of sats and altitude (high bits)
numSatBcd = (gpsSol.numSat > 99) ? dec2bcd(99) : dec2bcd(gpsSol.numSat);
altitudeHighBcd = dec2bcd(gpsSol.llh.alt / 100000);
// Speed (knots)
speedTmp = gpsSol.groundSpeed * 1944 / 1000;
speedKnotsBcd = (speedTmp > 9999) ? dec2bcd(9999) : dec2bcd(speedTmp);
#ifdef USE_RTC_TIME
dateTime_t dt;
// RTC
if (rtcHasTime()) {
rtcGetDateTime(&dt);
timeBcd = dec2bcd(dt.hours);
timeBcd = timeBcd << 8;
timeBcd = timeBcd | dec2bcd(dt.minutes);
timeBcd = timeBcd << 8;
timeBcd = timeBcd | dec2bcd(dt.seconds);
timeBcd = timeBcd << 4;
timeBcd = timeBcd | dec2bcd(dt.millis / 100);
timeProvided = true;
}
#endif
timeBcd = (timeProvided) ? timeBcd : SPEKTRUM_TIME_UNKNOWN;
// SRXL frame
sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_STAT);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16(dst, speedKnotsBcd);
sbufWriteU32(dst, timeBcd);
sbufWriteU8(dst, numSatBcd);
sbufWriteU8(dst, altitudeHighBcd);
sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT);
return true;
}
#endif
/*
typedef struct
{
UINT8 identifier; // Source device = 0x34
UINT8 sID; // Secondary ID
INT16 current_A; // Instantaneous current, 0.1A (0-3276.8A)
INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah)
UINT16 temp_A; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
INT16 current_B; // Instantaneous current, 0.1A (0-3276.8A)
INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah)
UINT16 temp_B; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
UINT16 spare; // Not used
} STRU_TELE_FP_MAH;
*/
#define STRU_TELE_FP_EMPTY_FIELDS_COUNT 2
#define STRU_TELE_FP_EMPTY_FIELDS_VALUE 0xff
#define SPEKTRUM_AMPS_UNUSED 0x7fff
#define SPEKTRUM_AMPH_UNUSED 0x7fff
#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s
bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
{
uint16_t amps = getAmperage() / 10;
uint16_t mah = getMAhDrawn();
static uint16_t sentAmps;
static uint16_t sentMah;
static timeUs_t lastTimeSentFPmAh = 0;
timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
if ( amps != sentAmps ||
mah != sentMah ||
keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) {
sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU16(dst, amps);
sbufWriteU16(dst, mah);
sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp A
sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED); // Amps B
sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED); // mAH B
sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp B
sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT);
sentAmps = amps;
sentMah = mah;
lastTimeSentFPmAh = currentTimeUs;
return true;
}
return false;
}
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C) // Text Generator
#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9) // Text Generator ROWS
#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13) // Text Generator COLS
/*
typedef struct
{
UINT8 identifier;
UINT8 sID; // Secondary ID
UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
char text[13]; // 0-terminated text when < 13 chars
} STRU_SPEKTRUM_SRXL_TEXTGEN;
*/
#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS )
static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS];
static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS];
#else
static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS];
static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS];
#endif
//**************************************************************************
// API Running in external client task context. E.g. in the CMS task
int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c)
{
if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) {
// Only update and force a tm transmision if something has actually changed.
if (srxlTextBuff[row][col] != c) {
srxlTextBuff[row][col] = c;
lineSent[row] = false;
}
}
return 0;
}
//**************************************************************************
bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
static uint8_t lineNo = 0;
int lineCount = 0;
// Skip already sent lines...
while (lineSent[lineNo] &&
lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
lineCount++;
}
sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU8(dst, lineNo);
sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS);
lineSent[lineNo] = true;
lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
// Always send something, Always one user frame after the two mandatory frames
// I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
// too keep the "Waltz" sequence intact.
return true;
}
#endif
#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
static uint8_t vtxDeviceType;
static void collectVtxTmData(spektrumVtx_t * vtx)
{
const vtxDevice_t *vtxDevice = vtxCommonDevice();
vtxDeviceType = vtxCommonGetDeviceType(vtxDevice);
// Collect all data from VTX, if VTX is ready
unsigned vtxStatus;
if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) &&
vtxCommonGetStatus(vtxDevice, &vtxStatus) &&
vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) )
{
vtx->band = 0;
vtx->channel = 0;
vtx->power = 0;
vtx->pitMode = 0;
} else {
vtx->pitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0;
}
vtx->powerValue = 0;
#ifdef USE_SPEKTRUM_REGION_CODES
vtx->region = SpektrumRegion;
#else
vtx->region = SPEKTRUM_VTX_REGION_NONE;
#endif
}
// Reverse lookup, device power index to Spektrum power range index.
static void convertVtxPower(spektrumVtx_t * vtx)
{
uint8_t const * powerIndexTable = NULL;
vtxCommonLookupPowerValue(vtxCommonDevice(), vtx->power, &vtx->powerValue);
switch (vtxDeviceType) {
#if defined(USE_VTX_TRAMP)
case VTXDEV_TRAMP:
powerIndexTable = vtxTrampPi;
break;
#endif
#if defined(USE_VTX_SMARTAUDIO)
case VTXDEV_SMARTAUDIO:
powerIndexTable = vtxSaPi;
break;
#endif
#if defined(USE_VTX_RTC6705)
case VTXDEV_RTC6705:
powerIndexTable = vtxRTC6705Pi;
break;
#endif
case VTXDEV_UNKNOWN:
case VTXDEV_UNSUPPORTED:
default:
break;
}
if (powerIndexTable != NULL) {
for (int i = 0; i < SPEKTRUM_VTX_POWER_COUNT; i++)
if (powerIndexTable[i] >= vtx->power) {
vtx->power = i; // Translate device power index to Spektrum power index.
break;
}
}
}
static void convertVtxTmData(spektrumVtx_t * vtx)
{
// Convert from internal band indexes to Spektrum indexes
for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) {
if (spek2commonBand[i] == vtx->band) {
vtx->band = i;
break;
}
}
// De-bump channel no 1 based interally, 0-based in Spektrum.
vtx->channel--;
// Convert Power index to Spektrum ranges, different per brand.
convertVtxPower(vtx);
}
/*
typedef struct
{
UINT8 identifier;
UINT8 sID; // Secondary ID
UINT8 band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A, 5-7 = Reserved)
UINT8 channel; // VTX Channel (0-7)
UINT8 pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. When PIT is set, it overrides all other power settings
UINT8 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 powerDec; // VTX Power as a decimal 1mw/unit
UINT8 region; // Region (0 = USA, 1 = EU, 0xFF = N/A)
UINT8 rfu[7]; // reserved
} STRU_TELE_VTX;
*/
#define STRU_TELE_VTX_EMPTY_COUNT 7
#define STRU_TELE_VTX_EMPTY_VALUE 0xff
#define VTX_KEEPALIVE_TIME_OUT 2000000 // uS
static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs)
{
static timeUs_t lastTimeSentVtx = 0;
static spektrumVtx_t vtxSent;
spektrumVtx_t vtx;
collectVtxTmData(&vtx);
if ((vtxDeviceType != VTXDEV_UNKNOWN) && vtxDeviceType != VTXDEV_UNSUPPORTED) {
convertVtxTmData(&vtx);
if ((memcmp(&vtxSent, &vtx, sizeof(spektrumVtx_t)) != 0) ||
((currentTimeUs - lastTimeSentVtx) > VTX_KEEPALIVE_TIME_OUT) ) {
// Fill in the VTX tm structure
sbufWriteU8(dst, TELE_DEVICE_VTX);
sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
sbufWriteU8(dst, vtx.band);
sbufWriteU8(dst, vtx.channel);
sbufWriteU8(dst, vtx.pitMode);
sbufWriteU8(dst, vtx.power);
sbufWriteU16(dst, vtx.powerValue);
sbufWriteU8(dst, vtx.region);
sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT);
memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t));
lastTimeSentVtx = currentTimeUs;
return true;
}
}
return false;
}
#endif // USE_SPEKTRUM_VTX_TELEMETRY && USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON
// Schedule array to decide how often each type of frame is sent
// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame.
// The user frame type is cycled for each set.
// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc
#define SRXL_SCHEDULE_MANDATORY_COUNT 2 // Mandatory QOS and RPM sensors
#define SRXL_FP_MAH_COUNT 1
#if defined(USE_GPS)
#define SRXL_GPS_LOC_COUNT 1
#define SRXL_GPS_STAT_COUNT 1
#else
#define SRXL_GPS_LOC_COUNT 0
#define SRXL_GPS_STAT_COUNT 0
#endif
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
#define SRXL_SCHEDULE_CMS_COUNT 1
#else
#define SRXL_SCHEDULE_CMS_COUNT 0
#endif
#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
#define SRXL_VTX_TM_COUNT 1
#else
#define SRXL_VTX_TM_COUNT 0
#endif
#define SRXL_SCHEDULE_USER_COUNT (SRXL_FP_MAH_COUNT + SRXL_SCHEDULE_CMS_COUNT + SRXL_VTX_TM_COUNT + SRXL_GPS_LOC_COUNT + SRXL_GPS_STAT_COUNT)
#define SRXL_SCHEDULE_COUNT_MAX (SRXL_SCHEDULE_MANDATORY_COUNT + 1)
#define SRXL_TOTAL_COUNT (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT)
typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs);
const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = {
/* must send srxlFrameQos, Rpm and then alternating items of our own */
srxlFrameQos,
srxlFrameRpm,
srxlFrameFlightPackCurrent,
#if defined(USE_GPS)
srxlFrameGpsStat,
srxlFrameGpsLoc,
#endif
#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
srxlFrameVTX,
#endif
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
srxlFrameText,
#endif
};
static void processSrxl(timeUs_t currentTimeUs)
{
static uint8_t srxlScheduleIndex = 0;
static uint8_t srxlScheduleUserIndex = 0;
sbuf_t srxlPayloadBuf;
sbuf_t *dst = &srxlPayloadBuf;
srxlScheduleFnPtr srxlFnPtr;
if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) {
srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
} else {
srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex];
srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT;
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
// Boost CMS performance by sending nothing else but CMS Text frames when in a CMS menu.
// Sideeffect, all other reports are still not sent if user leaves CMS without a proper EXIT.
if (cmsInMenu &&
(cmsDisplayPortGetCurrent() == &srxlDisplayPort)) {
srxlFnPtr = srxlFrameText;
}
#endif
}
if (srxlFnPtr) {
srxlInitializeFrame(dst);
if (srxlFnPtr(dst, currentTimeUs)) {
srxlFinalize(dst);
}
}
srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
}
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
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)
{
return srxlTelemetryEnabled;
}
/*
* Called periodically by the scheduler
*/
void handleSrxlTelemetry(timeUs_t currentTimeUs)
{
if (srxl2) {
#if defined(USE_SERIALRX_SRXL2)
if (srxl2TelemetryRequested()) {
processSrxl(currentTimeUs);
}
#endif
} else {
if (srxlTelemetryBufferEmpty()) {
processSrxl(currentTimeUs);
}
}
}
#endif

35
src/main/telemetry/srxl.h Normal file
View file

@ -0,0 +1,35 @@
/*
* 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
#include "common/time.h"
void srxlCollectTelemetryNow(void);
void initSrxlTelemetry(void);
bool checkSrxlTelemetryState(void);
void handleSrxlTelemetry(timeUs_t currentTimeUs);
#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9
#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20
//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21
#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255
int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c);

13
src/main/telemetry/telemetry.c
View file

@ -48,6 +48,7 @@
#include "telemetry/jetiexbus.h"
#include "telemetry/ibus.h"
#include "telemetry/crsf.h"
#include "telemetry/srxl.h"
#include "telemetry/sim.h"
@ -124,6 +125,10 @@ void telemetryInit(void)
initCrsfTelemetry();
#endif
#ifdef USE_TELEMETRY_SRXL
initSrxlTelemetry();
#endif
telemetryCheckState();
}
@ -185,6 +190,10 @@ void telemetryCheckState(void)
#if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
checkCrsfTelemetryState();
#endif
#ifdef USE_TELEMETRY_SRXL
checkSrxlTelemetryState();
#endif
}
void telemetryProcess(timeUs_t currentTimeUs)
@ -226,6 +235,10 @@ void telemetryProcess(timeUs_t currentTimeUs)
#if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
handleCrsfTelemetry(currentTimeUs);
#endif
#ifdef USE_TELEMETRY_SRXL
handleSrxlTelemetry(currentTimeUs);
#endif
}
#endif