rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Spektrum protocol corrections, VTX ctrl and srxl tm in correct phases.

This commit is contained in:
Anders Hoglund 2018-01-30 22:32:02 +01:00
parent ab1812455f
commit 7e54f8cdc9
2 changed files with 21 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/io/spektrum_vtx_control.h
View File

@ -25,6 +25,7 @@
#define SPEKTRUM_VTX_CONTROL_2 (SPEK_FRAME_SIZE - 3)
#define SPEKTRUM_VTX_CONTROL_3 (SPEK_FRAME_SIZE - 2)
#define SPEKTRUM_VTX_CONTROL_4 (SPEK_FRAME_SIZE - 1)
#define SPEKTRUM_VTX_CONTROL_SIZE 4
#define SPEKTRUM_VTX_BAND_MASK 0x00e00000
#define SPEKTRUM_VTX_CHANNEL_MASK 0x000f0000

37
src/main/rx/spektrum.c
View File

@ -111,8 +111,26 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
spektrumHandleRSSI(spekFrame);
#endif
// Get the VTX control bytes in a frame
uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) |
(spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) |
(spekFrame[SPEKTRUM_VTX_CONTROL_3] << 8) |
(spekFrame[SPEKTRUM_VTX_CONTROL_4] << 0) );
int8_t spektrumRcDataSize;
// Handle VTX control frame.
if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME &&
(spekFrame[2] & 0x80) == 0 ) {
#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
spektrumHandleVtxControl(vtxControl);
#endif
spektrumRcDataSize = SPEK_FRAME_SIZE - SPEKTRUM_VTX_CONTROL_SIZE;
} else {
spektrumRcDataSize = SPEK_FRAME_SIZE;
}
// Get the RC control channel inputs
for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
for (int b = 3; b < spektrumRcDataSize; 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) {
@ -121,23 +139,8 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
}
}
#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
// Get the VTX control bytes in a frame
uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) |
(spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) |
(spekFrame[SPEKTRUM_VTX_CONTROL_3] << 8) |
(spekFrame[SPEKTRUM_VTX_CONTROL_4] << 0) );
// Handle VTX control frame,
if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME) {
spektrumHandleVtxControl(vtxControl);
}
#endif // USE_SPEKTRUM_VTX_CONTROL && VTX_COMMON
/* only process if srxl enabled, some data in buffer AND servos in phase 0 */
if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80)) {
if (srxlEnabled && telemetryBufLen && (spekFrame[2] & 0x80) == 0) {
dispatchAdd(&srxlTelemetryDispatch, SPEKTRUM_TELEMETRY_FRAME_DELAY);
}
return RX_FRAME_COMPLETE;