Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Fixed Spektrum implementation inconsistency.

Browse source
This commit is contained in:
mikeller 2018-08-30 20:18:29 +12:00
parent 8c47db882f
commit ac5bc7bc55
2 changed files with 71 additions and 63 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -36,7 +36,6 @@
#include "io/serial.h"
#include "fc/config.h"
#include "fc/dispatch.h"
#include "io/spektrum_rssi.h"
#include "io/spektrum_vtx_control.h"
@ -53,6 +52,8 @@
// driver for spektrum satellite receiver / sbus
#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
bool srxlEnabled = false;
int32_t resolution;
uint8_t rssi_channel;
@ -70,9 +71,6 @@ static serialPort_t *serialPort;
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0;
void srxlRxSendTelemetryDataDispatch(dispatchEntry_t *self);
static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch};
#endif
// Receive ISR callback
@ -109,10 +107,14 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!rcFrameComplete) {
return RX_FRAME_PENDING;
}
static timeUs_t telemetryFrameRequestedUs = 0;
uint8_t result = RX_FRAME_PENDING;
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
timeUs_t currentTimeUs = micros();
#endif
if (rcFrameComplete) {
rcFrameComplete = false;
#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
@ -148,21 +150,23 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
}
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
if (srxlEnabled) {
/* Only dispatch for transmission if there are some data in buffer AND servos in phase 0 */
if (telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
}
/* Trigger tm data collection if buffer has been sent and is empty,
so data will be ready to transmit in the next phase 0 */
if (telemetryBufLen == 0) {
srxlCollectTelemetryNow();
}
if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
telemetryFrameRequestedUs = currentTimeUs;
}
#endif
return RX_FRAME_COMPLETE;
result = RX_FRAME_COMPLETE;
}
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
if (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)
@ -303,6 +307,30 @@ void spektrumBind(rxConfig_t *rxConfig)
}
#endif // USE_SPEKTRUM_BIND
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
static bool spektrumProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
// if there is telemetry data to write
if (telemetryBufLen > 0) {
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
telemetryBufLen = 0; // reset telemetry buffer
srxlCollectTelemetryNow();
}
return true;
}
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;
@ -347,6 +375,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
rxRuntimeConfig->rcReadRawFn = spektrumReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = spektrumFrameStatus;
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
rxRuntimeConfig->rcProcessFrameFn = spektrumProcessFrame;
#endif
serialPort = openSerialPort(portConfig->identifier,
FUNCTION_RX_SERIAL,
@ -367,31 +398,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
rssi_channel = 0;
}
if (serialPort && srxlEnabled) {
dispatchEnable();
}
return serialPort != NULL;
}
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
void srxlRxWriteTelemetryData(const void *data, int len)
{
len = MIN(len, (int)sizeof(telemetryBuf));
memcpy(telemetryBuf, data, len);
telemetryBufLen = len;
}
void srxlRxSendTelemetryDataDispatch(dispatchEntry_t* self)
{
UNUSED(self);
// if there is telemetry data to write
if (telemetryBufLen > 0) {
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
telemetryBufLen = 0; // reset telemetry buffer
}
}
#endif
bool srxlRxIsActive(void)
{
return serialPort != NULL;

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

@ -32,7 +32,6 @@
#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
#define SPEKTRUM_TELEMETRY_FRAME_DELAY 1000 // Gap between received Rc frame and transmited TM frame, uS
#define SPEKTRUM_BAUDRATE 115200