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 #if defined(USE_SERIAL_RX)
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"
45 #include "rx/sbus_channels.h"
49 #define FPORT_TIME_NEEDED_PER_FRAME_US 3000
50 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
51 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
52 #define FPORT_MAX_TELEMETRY_AGE_MS 500
55 #define FPORT_FRAME_MARKER 0x7E
57 #define FPORT_ESCAPE_CHAR 0x7D
58 #define FPORT_ESCAPE_MASK 0x20
60 #define FPORT_CRC_VALUE 0xFF
62 #define FPORT_BAUDRATE 115200
64 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
67 DEBUG_FPORT_FRAME_INTERVAL
= 0,
68 DEBUG_FPORT_FRAME_ERRORS
,
69 DEBUG_FPORT_FRAME_LAST_ERROR
,
70 DEBUG_FPORT_TELEMETRY_DELAY
,
74 DEBUG_FPORT_NO_ERROR
= 0,
75 DEBUG_FPORT_ERROR_TIMEOUT
,
76 DEBUG_FPORT_ERROR_OVERSIZE
,
77 DEBUG_FPORT_ERROR_SIZE
,
78 DEBUG_FPORT_ERROR_CHECKSUM
,
79 DEBUG_FPORT_ERROR_TYPE
,
80 DEBUG_FPORT_ERROR_TYPE_SIZE
,
84 FPORT_FRAME_TYPE_CONTROL
= 0x00,
85 FPORT_FRAME_TYPE_TELEMETRY_REQUEST
= 0x01,
86 FPORT_FRAME_TYPE_TELEMETRY_RESPONSE
= 0x81,
91 FPORT_FRAME_ID_NULL
= 0x00, // (master/slave)
92 FPORT_FRAME_ID_DATA
= 0x10, // (master/slave)
93 FPORT_FRAME_ID_READ
= 0x30, // (master)
94 FPORT_FRAME_ID_WRITE
= 0x31, // (master)
95 FPORT_FRAME_ID_RESPONSE
= 0x32, // (slave)
98 typedef struct fportControlData_s
{
99 sbusChannels_t channels
;
101 } fportControlData_t
;
103 typedef union fportData_s
{
104 fportControlData_t controlData
;
105 smartPortPayload_t telemetryData
;
108 typedef struct fportFrame_s
{
113 static const smartPortPayload_t emptySmartPortFrame
= { .frameId
= 0, .valueId
= 0, .data
= 0 };
115 #define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
116 #define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
118 #define FPORT_FRAME_PAYLOAD_LENGTH_CONTROL (sizeof(uint8_t) + sizeof(fportControlData_t))
119 #define FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST (sizeof(uint8_t) + sizeof(smartPortPayload_t))
121 #define NUM_RX_BUFFERS 3
122 #define BUFFER_SIZE (FPORT_REQUEST_FRAME_LENGTH + 2 * sizeof(uint8_t))
124 typedef struct fportBuffer_s
{
125 uint8_t data
[BUFFER_SIZE
];
129 static fportBuffer_t rxBuffer
[NUM_RX_BUFFERS
];
131 static volatile uint8_t rxBufferWriteIndex
= 0;
132 static volatile uint8_t rxBufferReadIndex
= 0;
134 static volatile timeUs_t lastTelemetryFrameReceivedUs
;
135 static volatile bool clearToSend
= false;
137 static volatile uint8_t framePosition
= 0;
139 static smartPortPayload_t
*mspPayload
= NULL
;
140 static timeUs_t lastRcFrameReceivedMs
= 0;
142 static serialPort_t
*fportPort
;
143 static bool telemetryEnabled
= false;
145 static void reportFrameError(uint8_t errorReason
) {
146 static volatile uint16_t frameErrors
= 0;
148 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_FRAME_ERRORS
, ++frameErrors
);
149 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_FRAME_LAST_ERROR
, errorReason
);
152 // Receive ISR callback
153 static void fportDataReceive(uint16_t c
, void *data
)
157 static timeUs_t frameStartAt
= 0;
158 static bool escapedCharacter
= false;
159 static timeUs_t lastFrameReceivedUs
= 0;
160 static bool telemetryFrame
= false;
162 const timeUs_t currentTimeUs
= micros();
166 if (framePosition
> 1 && cmpTimeUs(currentTimeUs
, frameStartAt
) > FPORT_TIME_NEEDED_PER_FRAME_US
+ 500) {
167 reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT
);
172 uint8_t val
= (uint8_t)c
;
174 if (val
== FPORT_FRAME_MARKER
) {
175 if (framePosition
> 1) {
176 const uint8_t nextWriteIndex
= (rxBufferWriteIndex
+ 1) % NUM_RX_BUFFERS
;
177 if (nextWriteIndex
!= rxBufferReadIndex
) {
178 rxBuffer
[rxBufferWriteIndex
].length
= framePosition
- 1;
179 rxBufferWriteIndex
= nextWriteIndex
;
182 if (telemetryFrame
) {
184 lastTelemetryFrameReceivedUs
= currentTimeUs
;
185 telemetryFrame
= false;
188 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_FRAME_INTERVAL
, currentTimeUs
- lastFrameReceivedUs
);
189 lastFrameReceivedUs
= currentTimeUs
;
191 escapedCharacter
= false;
194 frameStartAt
= currentTimeUs
;
196 } else if (framePosition
> 0) {
197 if (framePosition
>= BUFFER_SIZE
+ 1) {
200 reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE
);
202 if (escapedCharacter
) {
203 val
= val
^ FPORT_ESCAPE_MASK
;
204 escapedCharacter
= false;
205 } else if (val
== FPORT_ESCAPE_CHAR
) {
206 escapedCharacter
= true;
211 if (framePosition
== 2 && val
== FPORT_FRAME_TYPE_TELEMETRY_REQUEST
) {
212 telemetryFrame
= true;
215 rxBuffer
[rxBufferWriteIndex
].data
[framePosition
- 1] = val
;
216 framePosition
= framePosition
+ 1;
221 #if defined(USE_TELEMETRY_SMARTPORT)
222 static void smartPortWriteFrameFport(const smartPortPayload_t
*payload
)
226 uint16_t checksum
= 0;
227 smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH
, &checksum
, fportPort
);
228 smartPortSendByte(FPORT_FRAME_TYPE_TELEMETRY_RESPONSE
, &checksum
, fportPort
);
229 smartPortWriteFrameSerial(payload
, fportPort
, checksum
);
233 static bool checkChecksum(uint8_t *data
, uint8_t length
)
235 uint16_t checksum
= 0;
236 for (unsigned i
= 0; i
< length
; i
++) {
237 checksum
= checksum
+ *(uint8_t *)(data
+ i
);
240 checksum
= (checksum
& 0xff) + (checksum
>> 8);
242 return checksum
== FPORT_CRC_VALUE
;
245 static uint8_t fportFrameStatus(rxRuntimeConfig_t
*rxRuntimeConfig
)
247 static smartPortPayload_t payloadBuffer
;
248 static bool hasTelemetryRequest
= false;
250 uint8_t result
= RX_FRAME_PENDING
;
252 if (rxBufferReadIndex
!= rxBufferWriteIndex
) {
253 uint8_t bufferLength
= rxBuffer
[rxBufferReadIndex
].length
;
254 uint8_t frameLength
= rxBuffer
[rxBufferReadIndex
].data
[0];
255 if (frameLength
!= bufferLength
- 2) {
256 reportFrameError(DEBUG_FPORT_ERROR_SIZE
);
258 if (!checkChecksum(&rxBuffer
[rxBufferReadIndex
].data
[0], bufferLength
)) {
259 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM
);
261 fportFrame_t
*frame
= (fportFrame_t
*)&rxBuffer
[rxBufferReadIndex
].data
[1];
263 switch (frame
->type
) {
264 case FPORT_FRAME_TYPE_CONTROL
:
265 if (frameLength
!= FPORT_FRAME_PAYLOAD_LENGTH_CONTROL
) {
266 reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE
);
268 result
= sbusChannelsDecode(rxRuntimeConfig
, &frame
->data
.controlData
.channels
);
270 setRssiUnfiltered(scaleRange(constrain(frame
->data
.controlData
.rssi
, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL
);
272 lastRcFrameReceivedMs
= millis();
276 case FPORT_FRAME_TYPE_TELEMETRY_REQUEST
:
277 if (frameLength
!= FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST
) {
278 reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE
);
280 #if defined(USE_TELEMETRY_SMARTPORT)
281 if (!telemetryEnabled
) {
285 switch(frame
->data
.telemetryData
.frameId
) {
286 case FPORT_FRAME_ID_NULL
:
287 case FPORT_FRAME_ID_DATA
: // never used
288 hasTelemetryRequest
= true;
291 case FPORT_FRAME_ID_READ
:
292 case FPORT_FRAME_ID_WRITE
: // never used
293 memcpy(&payloadBuffer
, &frame
->data
.telemetryData
, sizeof(smartPortPayload_t
));
294 mspPayload
= &payloadBuffer
;
306 reportFrameError(DEBUG_FPORT_ERROR_TYPE
);
314 rxBufferReadIndex
= (rxBufferReadIndex
+ 1) % NUM_RX_BUFFERS
;
317 if ((mspPayload
|| hasTelemetryRequest
) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs
) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US
) {
318 hasTelemetryRequest
= false;
320 result
= (result
& ~RX_FRAME_PENDING
) | RX_FRAME_PROCESSING_REQUIRED
;
323 if (lastRcFrameReceivedMs
&& ((millis() - lastRcFrameReceivedMs
) > FPORT_MAX_TELEMETRY_AGE_MS
)) {
324 setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL
);
325 lastRcFrameReceivedMs
= 0;
331 static bool fportProcessFrame(const rxRuntimeConfig_t
*rxRuntimeConfig
)
333 UNUSED(rxRuntimeConfig
);
335 #if defined(USE_TELEMETRY_SMARTPORT)
336 timeUs_t currentTimeUs
= micros();
337 if (cmpTimeUs(currentTimeUs
, lastTelemetryFrameReceivedUs
) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US
) {
342 DEBUG_SET(DEBUG_FPORT
, DEBUG_FPORT_TELEMETRY_DELAY
, currentTimeUs
- lastTelemetryFrameReceivedUs
);
344 processSmartPortTelemetry(mspPayload
, &clearToSend
, NULL
);
347 smartPortWriteFrameFport(&emptySmartPortFrame
);
359 bool fportRxInit(const rxConfig_t
*rxConfig
, rxRuntimeConfig_t
*rxRuntimeConfig
)
361 static uint16_t sbusChannelData
[SBUS_MAX_CHANNEL
];
362 rxRuntimeConfig
->channelData
= sbusChannelData
;
363 sbusChannelsInit(rxConfig
, rxRuntimeConfig
);
365 rxRuntimeConfig
->channelCount
= SBUS_MAX_CHANNEL
;
366 rxRuntimeConfig
->rxRefreshRate
= 11000;
368 rxRuntimeConfig
->rcFrameStatusFn
= fportFrameStatus
;
369 rxRuntimeConfig
->rcProcessFrameFn
= fportProcessFrame
;
371 const serialPortConfig_t
*portConfig
= findSerialPortConfig(FUNCTION_RX_SERIAL
);
376 fportPort
= openSerialPort(portConfig
->identifier
,
382 FPORT_PORT_OPTIONS
| (rxConfig
->serialrx_inverted
? SERIAL_INVERTED
: 0) | (rxConfig
->halfDuplex
? SERIAL_BIDIR
: 0)
386 #if defined(USE_TELEMETRY_SMARTPORT)
387 telemetryEnabled
= initSmartPortTelemetryExternal(smartPortWriteFrameFport
);
390 if (rssiSource
== RSSI_SOURCE_NONE
) {
391 rssiSource
= RSSI_SOURCE_RX_PROTOCOL
;
395 return fportPort
!= NULL
;