Skip to content

Commit 607144e

Browse files
mmoscammosca
authored
Merge pull request #10270 from iNavFlight/mmosca-sbus2-26chan
Refactor sbus to support futaba's FASSTest26 sbus2 mode (Potentially 36 channels)
2 parents 3cc11ce + 124a0d3 commit 607144e

File tree

10 files changed

+186
-22
lines changed

10 files changed

+186
-22
lines changed

‎src/main/blackbox/blackbox.c‎

Lines changed: 38 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -353,6 +353,25 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
353353
{"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_16)},
354354
{"servo", 16, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_17)},
355355
{"servo", 17, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)},
356+
{"servo", 18, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)},
357+
{"servo", 19, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_19)},
358+
{"servo", 20, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_20)},
359+
{"servo", 21, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_21)},
360+
{"servo", 22, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_22)},
361+
{"servo", 23, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_23)},
362+
{"servo", 24, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_24)},
363+
{"servo", 25, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_25)},
364+
/*
365+
{"servo", 26, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_26)},
366+
{"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_27)},
367+
{"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_28)},
368+
{"servo", 28, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_29)},
369+
{"servo", 29, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_30)},
370+
{"servo", 30, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_31)},
371+
{"servo", 31, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_32)},
372+
{"servo", 32, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_33)},
373+
{"servo", 33, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_34)},
374+
*/
356375

357376
{"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
358377
{"navFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@@ -675,7 +694,25 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
675694
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16:
676695
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17:
677696
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18:
678-
return ((FlightLogFieldCondition)getServoCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS);
697+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19:
698+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20:
699+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21:
700+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22:
701+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23:
702+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24:
703+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25:
704+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26:
705+
/*
706+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_27:
707+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_28:
708+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_29:
709+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_30:
710+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_31:
711+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_32:
712+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33:
713+
case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34:
714+
*/
715+
return ((FlightLogFieldCondition)MIN(getServoCount(), 26) >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS);
679716

680717
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
681718
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:

‎src/main/blackbox/blackbox_fielddefs.h‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,24 @@ typedef enum FlightLogFieldCondition {
5353
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_16,
5454
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_17,
5555
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_18,
56+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_19,
57+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_20,
58+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_21,
59+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_22,
60+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_23,
61+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_24,
62+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_25,
63+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_26,
64+
/*
65+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_27,
66+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_28,
67+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_29,
68+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_30,
69+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_31,
70+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_32,
71+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33,
72+
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34,
73+
*/
5674

5775
FLIGHT_LOG_FIELD_CONDITION_MAG,
5876
FLIGHT_LOG_FIELD_CONDITION_BARO,

‎src/main/drivers/pwm_mapping.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@
2828
#define MAX_MOTORS 12
2929
#endif
3030

31-
#define MAX_SERVOS 18
31+
#define MAX_SERVOS 34
3232

3333
#define PWM_TIMER_HZ 1000000
3434

‎src/main/flight/servos.h‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
#include "config/parameter_group.h"
2121
#include "programming/logic_condition.h"
2222

23-
#define MAX_SUPPORTED_SERVOS 18
23+
#define MAX_SUPPORTED_SERVOS 34
2424

2525
// These must be consecutive
2626
typedef enum {
@@ -75,6 +75,16 @@ typedef enum {
7575
INPUT_RC_CH22 = 47,
7676
INPUT_RC_CH23 = 48,
7777
INPUT_RC_CH24 = 49,
78+
INPUT_RC_CH25 = 50,
79+
INPUT_RC_CH26 = 51,
80+
INPUT_RC_CH27 = 52,
81+
INPUT_RC_CH28 = 53,
82+
INPUT_RC_CH29 = 54,
83+
INPUT_RC_CH30 = 55,
84+
INPUT_RC_CH31 = 56,
85+
INPUT_RC_CH32 = 57,
86+
INPUT_RC_CH33 = 58,
87+
INPUT_RC_CH34 = 59,
7888
#endif
7989
INPUT_SOURCE_COUNT
8090
} inputSource_e;

‎src/main/io/servo_sbus.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -109,8 +109,8 @@ void sbusServoUpdate(uint8_t index, uint16_t value)
109109
case 13: sbusFrame.channels.chan13 = sbusEncodeChannelValue(value); break;
110110
case 14: sbusFrame.channels.chan14 = sbusEncodeChannelValue(value); break;
111111
case 15: sbusFrame.channels.chan15 = sbusEncodeChannelValue(value); break;
112-
case 16: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_17) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_17) ; break;
113-
case 17: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_18) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_18) ; break;
112+
case 16: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_DG1) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_DG1) ; break;
113+
case 17: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_DG2) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_DG2) ; break;
114114
default:
115115
break;
116116
}

‎src/main/rx/rx.h‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -84,11 +84,7 @@ typedef enum {
8484
SERIALRX_SBUS2,
8585
} rxSerialReceiverType_e;
8686

87-
#ifdef USE_24CHANNELS
88-
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 26
89-
#else
90-
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18
91-
#endif
87+
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 34
9288

9389
#define NON_AUX_CHANNEL_COUNT 4
9490
#define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT)

‎src/main/rx/sbus.c‎

Lines changed: 55 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -54,13 +54,16 @@
5454
typedef enum {
5555
STATE_SBUS_SYNC = 0,
5656
STATE_SBUS_PAYLOAD,
57+
STATE_SBUS26_PAYLOAD,
5758
STATE_SBUS_WAIT_SYNC
5859
} sbusDecoderState_e;
5960

6061
typedef struct sbusFrameData_s {
6162
sbusDecoderState_e state;
6263
volatile sbusFrame_t frame;
64+
volatile sbusFrame_t frameHigh;
6365
volatile bool frameDone;
66+
volatile bool is26channels;
6467
uint8_t buffer[SBUS_FRAME_SIZE];
6568
uint8_t position;
6669
timeUs_t lastActivityTimeUs;
@@ -79,7 +82,10 @@ static void sbusDataReceive(uint16_t c, void *data)
7982
sbusFrameData->lastActivityTimeUs = currentTimeUs;
8083

8184
// Handle inter-frame gap. We dwell in STATE_SBUS_WAIT_SYNC state ignoring all incoming bytes until we get long enough quite period on the wire
82-
if (sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= rxConfig()->sbusSyncInterval) {
85+
if (timeSinceLastByteUs >= rxConfig()->sbusSyncInterval) {
86+
sbusFrameData->state = STATE_SBUS_SYNC;
87+
} else if ((sbusFrameData->state == STATE_SBUS_PAYLOAD || sbusFrameData->state == STATE_SBUS26_PAYLOAD) && timeSinceLastByteUs >= 300) {
88+
// payload is pausing too long, possible if some telemetry have been sent between frames, or false positves mid frame
8389
sbusFrameData->state = STATE_SBUS_SYNC;
8490
}
8591

@@ -89,6 +95,10 @@ static void sbusDataReceive(uint16_t c, void *data)
8995
sbusFrameData->position = 0;
9096
sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
9197
sbusFrameData->state = STATE_SBUS_PAYLOAD;
98+
} else if ((uint8_t)c == SBUS2_HIGHFRAME_BEGIN_BYTE) {
99+
sbusFrameData->position = 0;
100+
sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
101+
sbusFrameData->state = STATE_SBUS26_PAYLOAD;
92102
}
93103
break;
94104

@@ -115,7 +125,6 @@ static void sbusDataReceive(uint16_t c, void *data)
115125
frameTime = -1;
116126
}
117127

118-
119128
frameValid = true;
120129
sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
121130
break;
@@ -134,6 +143,40 @@ static void sbusDataReceive(uint16_t c, void *data)
134143
}
135144
break;
136145

146+
case STATE_SBUS26_PAYLOAD:
147+
sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
148+
149+
if (sbusFrameData->position == SBUS_FRAME_SIZE) {
150+
const sbusFrame_t * frame = (sbusFrame_t *)&sbusFrameData->buffer[0];
151+
bool frameValid = false;
152+
153+
// Do some sanity check
154+
switch (frame->endByte) {
155+
case 0x00:
156+
case 0x04: // S.BUS 2 telemetry page 1
157+
case 0x14: // S.BUS 2 telemetry page 2
158+
case 0x24: // S.BUS 2 telemetry page 3
159+
case 0x34: // S.BUS 2 telemetry page 4
160+
frameTime = -1; // ignore this one, as you can't fit telemetry between this and the next frame.
161+
frameValid = true;
162+
sbusFrameData->state = STATE_SBUS_SYNC; // Next piece of data should be a sync byte
163+
break;
164+
165+
default: // Failed end marker
166+
frameValid = false;
167+
sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
168+
break;
169+
}
170+
171+
// Frame seems sane, pass data to decoder
172+
if (!sbusFrameData->frameDone && frameValid) {
173+
memcpy((void *)&sbusFrameData->frameHigh, (void *)&sbusFrameData->buffer[0], SBUS_FRAME_SIZE);
174+
sbusFrameData->frameDone = true;
175+
sbusFrameData->is26channels = true;
176+
}
177+
}
178+
break;
179+
137180
case STATE_SBUS_WAIT_SYNC:
138181
// Stay at this state and do nothing. Exit will be handled before byte is processed if the
139182
// inter-frame gap is long enough
@@ -149,14 +192,20 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
149192
return RX_FRAME_PENDING;
150193
}
151194

195+
uint8_t retValue = 0;
152196
// Decode channel data and store return value
153-
const uint8_t retValue = sbusChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frame.channels);
197+
if (sbusFrameData->is26channels)
198+
{
199+
retValue = sbus26ChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frame.channels, false);
200+
retValue |= sbus26ChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frameHigh.channels, true);
201+
202+
} else {
203+
retValue = sbusChannelsDecode(rxRuntimeConfig, (void *)&sbusFrameData->frame.channels);
204+
}
154205

155206
// Reset the frameDone flag - tell ISR that we're ready to receive next frame
156207
sbusFrameData->frameDone = false;
157208

158-
//taskSendSbus2Telemetry(micros());
159-
160209
// Calculate "virtual link quality based on packet loss metric"
161210
if (retValue & RX_FRAME_COMPLETE) {
162211
lqTrackerAccumulate(rxRuntimeConfig->lqTracker, ((retValue & RX_FRAME_DROPPED) || (retValue & RX_FRAME_FAILSAFE)) ? 0 : RSSI_MAX_VALUE);
@@ -168,7 +217,7 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
168217
static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint32_t sbusBaudRate)
169218
{
170219
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
171-
static sbusFrameData_t sbusFrameData;
220+
static sbusFrameData_t sbusFrameData = { .is26channels = false};
172221

173222
rxRuntimeConfig->channelData = sbusChannelData;
174223
rxRuntimeConfig->frameData = &sbusFrameData;

‎src/main/rx/sbus.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#pragma once
1919

2020
#define SBUS_DEFAULT_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case
21+
#define SBUS_MIN_SYNC_DELAY_US MS2US(2) // 2ms
2122

2223
#include "rx/rx.h"
2324

‎src/main/rx/sbus_channels.c‎

Lines changed: 53 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@
2626
#include "common/utils.h"
2727
#include "common/maths.h"
2828

29+
#include "build/debug.h"
30+
2931
#include "rx/sbus_channels.h"
3032

3133

@@ -34,6 +36,55 @@
3436

3537
STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
3638

39+
uint8_t sbus26ChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels, bool highChannels)
40+
{
41+
uint8_t offset = highChannels ? 16 : 0;
42+
uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
43+
sbusChannelData[0 + offset] = channels->chan0;
44+
sbusChannelData[1 + offset] = channels->chan1;
45+
sbusChannelData[2 + offset] = channels->chan2;
46+
sbusChannelData[3 + offset] = channels->chan3;
47+
sbusChannelData[4 + offset] = channels->chan4;
48+
sbusChannelData[5 + offset] = channels->chan5;
49+
sbusChannelData[6 + offset] = channels->chan6;
50+
sbusChannelData[7 + offset] = channels->chan7;
51+
sbusChannelData[8 + offset] = channels->chan8;
52+
sbusChannelData[9 + offset] = channels->chan9;
53+
sbusChannelData[10 + offset] = channels->chan10;
54+
sbusChannelData[11 + offset] = channels->chan11;
55+
sbusChannelData[12 + offset] = channels->chan12;
56+
sbusChannelData[13 + offset] = channels->chan13;
57+
sbusChannelData[14 + offset] = channels->chan14;
58+
sbusChannelData[15 + offset] = channels->chan15;
59+
60+
if (!highChannels) {
61+
if (channels->flags & SBUS_FLAG_CHANNEL_DG1) {
62+
sbusChannelData[32] = SBUS_DIGITAL_CHANNEL_MAX;
63+
} else {
64+
sbusChannelData[32] = SBUS_DIGITAL_CHANNEL_MIN;
65+
}
66+
67+
if (channels->flags & SBUS_FLAG_CHANNEL_DG2) {
68+
sbusChannelData[33] = SBUS_DIGITAL_CHANNEL_MAX;
69+
} else {
70+
sbusChannelData[33] = SBUS_DIGITAL_CHANNEL_MIN;
71+
}
72+
}
73+
74+
if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
75+
// internal failsafe enabled and rx failsafe flag set
76+
// RX *should* still be sending valid channel data, so use it.
77+
return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
78+
}
79+
80+
if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
81+
// The received data is a repeat of the last valid data so can be considered complete.
82+
return RX_FRAME_COMPLETE | RX_FRAME_DROPPED;
83+
}
84+
85+
return RX_FRAME_COMPLETE;
86+
}
87+
3788
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
3889
{
3990
uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
@@ -54,13 +105,13 @@ uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannel
54105
sbusChannelData[14] = channels->chan14;
55106
sbusChannelData[15] = channels->chan15;
56107

57-
if (channels->flags & SBUS_FLAG_CHANNEL_17) {
108+
if (channels->flags & SBUS_FLAG_CHANNEL_DG1) {
58109
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
59110
} else {
60111
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
61112
}
62113

63-
if (channels->flags & SBUS_FLAG_CHANNEL_18) {
114+
if (channels->flags & SBUS_FLAG_CHANNEL_DG2) {
64115
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
65116
} else {
66117
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;

‎src/main/rx/sbus_channels.h‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -20,17 +20,18 @@
2020
#include <stdint.h>
2121
#include "rx/rx.h"
2222

23-
#define SBUS_MAX_CHANNEL 18
23+
#define SBUS_MAX_CHANNEL 34
2424

25-
#define SBUS_FLAG_CHANNEL_17 (1 << 0)
26-
#define SBUS_FLAG_CHANNEL_18 (1 << 1)
25+
#define SBUS_FLAG_CHANNEL_DG1 (1 << 0)
26+
#define SBUS_FLAG_CHANNEL_DG2 (1 << 1)
2727
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
2828
#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
2929

3030
#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
3131
#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
3232

33-
#define SBUS_FRAME_BEGIN_BYTE 0x0F
33+
#define SBUS_FRAME_BEGIN_BYTE ((uint8_t)0x0F)
34+
#define SBUS2_HIGHFRAME_BEGIN_BYTE ((uint8_t)0x2F)
3435

3536
#define SBUS_BAUDRATE 100000
3637
#define SBUS_BAUDRATE_FAST 200000
@@ -78,5 +79,6 @@ uint16_t sbusDecodeChannelValue(uint16_t sbusValue, bool safeValuesOnly);
7879
uint16_t sbusEncodeChannelValue(uint16_t rcValue);
7980

8081
uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
82+
uint8_t sbus26ChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels, bool highChannels);
8183

8284
void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig);

0 commit comments

Comments
 (0)