2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
28 #ifdef USE_SERIALRX_FPORT
30 #include "build/debug.h"
32 #include "common/maths.h"
33 #include "common/utils.h"
35 #include "drivers/time.h"
37 #include "io/serial.h"
40 #include "telemetry/telemetry.h"
41 #include "telemetry/smartport.h"
46 #include "rx/frsky_crc.h"
48 #include "rx/sbus_channels.h"
52 #define FPORT_TIME_NEEDED_PER_FRAME_US 3000
53 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
54 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
55 #define FPORT_MAX_TELEMETRY_AGE_MS 500
57 #define FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES 2
60 #define FPORT_FRAME_MARKER 0x7E
62 #define FPORT_ESCAPE_CHAR 0x7D
63 #define FPORT_ESCAPE_MASK 0x20
65 #define FPORT_BAUDRATE 115200
67 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
70 DEBUG_FPORT_FRAME_INTERVAL
= 0,
71 DEBUG_FPORT_FRAME_ERRORS
,
72 DEBUG_FPORT_FRAME_LAST_ERROR
,
73 DEBUG_FPORT_TELEMETRY_INTERVAL
,
77 DEBUG_FPORT_NO_ERROR
= 0,
78 DEBUG_FPORT_ERROR_TIMEOUT
,
79 DEBUG_FPORT_ERROR_OVERSIZE
,
80 DEBUG_FPORT_ERROR_SIZE
,
81 DEBUG_FPORT_ERROR_CHECKSUM
,
82 DEBUG_FPORT_ERROR_TYPE
,
83 DEBUG_FPORT_ERROR_TYPE_SIZE
,
87 FPORT_FRAME_TYPE_CONTROL
= 0x00,
88 FPORT_FRAME_TYPE_TELEMETRY_REQUEST
= 0x01,
89 FPORT_FRAME_TYPE_TELEMETRY_RESPONSE
= 0x81,
94 FPORT_FRAME_ID_NULL
= 0x00, // (master/slave)
95 FPORT_FRAME_ID_DATA
= 0x10, // (master/slave)
96 FPORT_FRAME_ID_READ
= 0x30, // (master)
97 FPORT_FRAME_ID_WRITE
= 0x31, // (master)
98 FPORT_FRAME_ID_RESPONSE
= 0x32, // (slave)
101 typedef struct fportControlData_s
{
102 sbusChannels_t channels
;
104 } fportControlData_t
;
106 typedef union fportData_s
{
107 fportControlData_t controlData
;
108 smartPortPayload_t telemetryData
;
111 typedef struct fportFrame_s
{
116 #ifdef USE_TELEMETRY_SMARTPORT
117 static const smartPortPayload_t emptySmartPortFrame
= { .frameId
= 0, .valueId
= 0, .data
= 0 };
120 #define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
121 #define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
123 #define FPORT_FRAME_PAYLOAD_LENGTH_CONTROL (sizeof(uint8_t) + sizeof(fportControlData_t))
124 #define FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST (sizeof(uint8_t) + sizeof(smartPortPayload_t))
126 #define NUM_RX_BUFFERS 3
127 #define BUFFER_SIZE (FPORT_REQUEST_FRAME_LENGTH + 2 * sizeof(uint8_t))
129 typedef struct fportBuffer_s
{
130 uint8_t data
[BUFFER_SIZE
];
132 timeUs_t frameStartTimeUs
;
135 static fportBuffer_t rxBuffer
[NUM_RX_BUFFERS
];
137 static volatile uint8_t rxBufferWriteIndex
= 0;
138 static volatile uint8_t rxBufferReadIndex
= 0;
140 static volatile timeUs_t lastTelemetryFrameReceivedUs
;
141 static volatile bool clearToSend
= false;
143 static volatile uint8_t framePosition
= 0;
145 static smartPortPayload_t
*mspPayload
= NULL
;
146 static timeUs_t lastRcFrameReceivedMs
= 0;
148 static serialPort_t
*fportPort
;
149 #ifdef USE_TELEMETRY_SMARTPORT
150 static bool telemetryEnabled
= false;
153 static void reportFrameError(uint8_t errorReason
)
155 static volatile uint16_t frameErrors
= 0;
159 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_FRAME_ERRORS
, frameErrors
);
160 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_FRAME_LAST_ERROR
, errorReason
);
163 // Receive ISR callback
164 static void fportDataReceive(uint16_t c
, void *data
)
168 static timeUs_t frameStartAt
= 0;
169 static bool escapedCharacter
= false;
170 static timeUs_t lastFrameReceivedUs
= 0;
171 static bool telemetryFrame
= false;
173 const timeUs_t currentTimeUs
= microsISR();
177 if (framePosition
> 1 && cmpTimeUs(currentTimeUs
, frameStartAt
) > FPORT_TIME_NEEDED_PER_FRAME_US
+ 500) {
178 reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT
);
183 uint8_t val
= (uint8_t)c
;
185 if (val
== FPORT_FRAME_MARKER
) {
186 if (framePosition
> 1) {
187 const uint8_t nextWriteIndex
= (rxBufferWriteIndex
+ 1) % NUM_RX_BUFFERS
;
188 if (nextWriteIndex
!= rxBufferReadIndex
) {
189 rxBuffer
[rxBufferWriteIndex
].length
= framePosition
- 1;
190 rxBufferWriteIndex
= nextWriteIndex
;
193 if (telemetryFrame
) {
195 lastTelemetryFrameReceivedUs
= currentTimeUs
;
196 telemetryFrame
= false;
199 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_FRAME_INTERVAL
, currentTimeUs
- lastFrameReceivedUs
);
200 lastFrameReceivedUs
= currentTimeUs
;
202 escapedCharacter
= false;
205 frameStartAt
= currentTimeUs
;
208 rxBuffer
[rxBufferWriteIndex
].frameStartTimeUs
= currentTimeUs
;
209 } else if (framePosition
> 0) {
210 if (framePosition
>= BUFFER_SIZE
+ 1) {
213 reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE
);
215 if (escapedCharacter
) {
216 val
= val
^ FPORT_ESCAPE_MASK
;
217 escapedCharacter
= false;
218 } else if (val
== FPORT_ESCAPE_CHAR
) {
219 escapedCharacter
= true;
224 if (framePosition
== 2 && val
== FPORT_FRAME_TYPE_TELEMETRY_REQUEST
) {
225 telemetryFrame
= true;
228 rxBuffer
[rxBufferWriteIndex
].data
[framePosition
- 1] = val
;
229 framePosition
= framePosition
+ 1;
234 #if defined(USE_TELEMETRY_SMARTPORT)
235 static void smartPortWriteFrameFport(const smartPortPayload_t
*payload
)
239 uint16_t checksum
= 0;
240 smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH
, &checksum
, fportPort
);
241 smartPortSendByte(FPORT_FRAME_TYPE_TELEMETRY_RESPONSE
, &checksum
, fportPort
);
242 smartPortWriteFrameSerial(payload
, fportPort
, checksum
);
246 static uint8_t fportFrameStatus(rxRuntimeState_t
*rxRuntimeState
)
248 static bool hasTelemetryRequest
= false;
250 #ifdef USE_TELEMETRY_SMARTPORT
251 static smartPortPayload_t payloadBuffer
;
252 static bool rxDrivenFrameRate
= false;
253 static uint8_t consecutiveTelemetryFrameCount
= 0;
256 uint8_t result
= RX_FRAME_PENDING
;
259 if (rxBufferReadIndex
!= rxBufferWriteIndex
) {
260 uint8_t bufferLength
= rxBuffer
[rxBufferReadIndex
].length
;
261 uint8_t frameLength
= rxBuffer
[rxBufferReadIndex
].data
[0];
262 if (frameLength
!= bufferLength
- 2) {
263 reportFrameError(DEBUG_FPORT_ERROR_SIZE
);
265 if (!frskyCheckSumIsGood(&rxBuffer
[rxBufferReadIndex
].data
[0], bufferLength
)) {
266 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM
);
268 fportFrame_t
*frame
= (fportFrame_t
*)&rxBuffer
[rxBufferReadIndex
].data
[1];
270 switch (frame
->type
) {
271 case FPORT_FRAME_TYPE_CONTROL
:
272 if (frameLength
!= FPORT_FRAME_PAYLOAD_LENGTH_CONTROL
) {
273 reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE
);
275 result
= sbusChannelsDecode(rxRuntimeState
, &frame
->data
.controlData
.channels
);
277 setRssi(scaleRange(frame
->data
.controlData
.rssi
, 0, 100, 0, RSSI_MAX_VALUE
), RSSI_SOURCE_RX_PROTOCOL
);
279 lastRcFrameReceivedMs
= millis();
281 if (!(result
& (RX_FRAME_FAILSAFE
| RX_FRAME_DROPPED
))) {
282 rxRuntimeState
->lastRcFrameTimeUs
= rxBuffer
[rxBufferReadIndex
].frameStartTimeUs
;
287 case FPORT_FRAME_TYPE_TELEMETRY_REQUEST
:
288 if (frameLength
!= FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST
) {
289 reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE
);
291 #if defined(USE_TELEMETRY_SMARTPORT)
292 if (!telemetryEnabled
) {
296 switch(frame
->data
.telemetryData
.frameId
) {
297 case FPORT_FRAME_ID_DATA
:
298 if (!rxDrivenFrameRate
) {
299 rxDrivenFrameRate
= true;
302 hasTelemetryRequest
= true;
305 case FPORT_FRAME_ID_NULL
:
306 if (!rxDrivenFrameRate
) {
307 if (consecutiveTelemetryFrameCount
>= FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES
&& !(mspPayload
&& smartPortPayloadContainsMSP(mspPayload
))) {
308 consecutiveTelemetryFrameCount
= 0;
310 hasTelemetryRequest
= true;
312 consecutiveTelemetryFrameCount
++;
317 case FPORT_FRAME_ID_READ
:
318 case FPORT_FRAME_ID_WRITE
: // never used
319 memcpy(&payloadBuffer
, &frame
->data
.telemetryData
, sizeof(smartPortPayload_t
));
320 mspPayload
= &payloadBuffer
;
332 reportFrameError(DEBUG_FPORT_ERROR_TYPE
);
340 rxBufferReadIndex
= (rxBufferReadIndex
+ 1) % NUM_RX_BUFFERS
;
343 if ((mspPayload
|| hasTelemetryRequest
) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs
) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US
) {
344 hasTelemetryRequest
= false;
346 result
= (result
& ~RX_FRAME_PENDING
) | RX_FRAME_PROCESSING_REQUIRED
;
349 if (lastRcFrameReceivedMs
&& ((millis() - lastRcFrameReceivedMs
) > FPORT_MAX_TELEMETRY_AGE_MS
)) {
350 setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL
);
351 lastRcFrameReceivedMs
= 0;
357 static bool fportProcessFrame(const rxRuntimeState_t
*rxRuntimeState
)
359 UNUSED(rxRuntimeState
);
361 #if defined(USE_TELEMETRY_SMARTPORT)
362 static timeUs_t lastTelemetryFrameSentUs
;
364 timeUs_t currentTimeUs
= micros();
365 if (cmpTimeUs(currentTimeUs
, lastTelemetryFrameReceivedUs
) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US
) {
370 processSmartPortTelemetry(mspPayload
, &clearToSend
, NULL
);
373 smartPortWriteFrameFport(&emptySmartPortFrame
);
378 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_TELEMETRY_INTERVAL
, currentTimeUs
- lastTelemetryFrameSentUs
);
379 lastTelemetryFrameSentUs
= currentTimeUs
;
388 bool fportRxInit(const rxConfig_t
*rxConfig
, rxRuntimeState_t
*rxRuntimeState
)
390 static uint16_t sbusChannelData
[SBUS_MAX_CHANNEL
];
391 rxRuntimeState
->channelData
= sbusChannelData
;
392 sbusChannelsInit(rxConfig
, rxRuntimeState
);
394 rxRuntimeState
->channelCount
= SBUS_MAX_CHANNEL
;
395 rxRuntimeState
->rcFrameStatusFn
= fportFrameStatus
;
396 rxRuntimeState
->rcProcessFrameFn
= fportProcessFrame
;
397 rxRuntimeState
->rcFrameTimeUsFn
= rxFrameTimeUs
;
399 const serialPortConfig_t
*portConfig
= findSerialPortConfig(FUNCTION_RX_SERIAL
);
404 fportPort
= openSerialPort(portConfig
->identifier
,
410 FPORT_PORT_OPTIONS
| (rxConfig
->serialrx_inverted
? SERIAL_INVERTED
: 0) | (rxConfig
->halfDuplex
? SERIAL_BIDIR
: 0)
414 #if defined(USE_TELEMETRY_SMARTPORT)
415 telemetryEnabled
= initSmartPortTelemetryExternal(smartPortWriteFrameFport
);
418 if (rssiSource
== RSSI_SOURCE_NONE
) {
419 rssiSource
= RSSI_SOURCE_RX_PROTOCOL
;
423 return fportPort
!= NULL
;