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_CRSF
30 #include "build/build_config.h"
31 #include "build/debug.h"
33 #include "common/crc.h"
34 #include "common/maths.h"
35 #include "common/utils.h"
39 #include "drivers/serial.h"
40 #include "drivers/serial_uart.h"
41 #include "drivers/system.h"
42 #include "drivers/time.h"
44 #include "io/serial.h"
49 #include "telemetry/crsf.h"
51 #define CRSF_TIME_NEEDED_PER_FRAME_US 1100 // 700 ms + 400 ms for potential ad-hoc request
52 #define CRSF_TIME_BETWEEN_FRAMES_US 6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
54 #define CRSF_DIGITAL_CHANNEL_MIN 172
55 #define CRSF_DIGITAL_CHANNEL_MAX 1811
57 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
59 #define CRSF_LINK_STATUS_UPDATE_TIMEOUT_US 250000 // 250ms, 4 Hz mode 1 telemetry
61 STATIC_UNIT_TESTED
bool crsfFrameDone
= false;
62 STATIC_UNIT_TESTED crsfFrame_t crsfFrame
;
63 STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame
;
64 STATIC_UNIT_TESTED
uint32_t crsfChannelData
[CRSF_MAX_CHANNEL
];
66 static serialPort_t
*serialPort
;
67 static timeUs_t crsfFrameStartAtUs
= 0;
68 static uint8_t telemetryBuf
[CRSF_FRAME_SIZE_MAX
];
69 static uint8_t telemetryBufLen
= 0;
71 static timeUs_t lastRcFrameTimeUs
= 0;
76 * CRSF protocol uses a single wire half duplex uart connection.
77 * The master sends one frame every 4ms and the slave replies between two frames from the master.
84 * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
85 * Max frame size is 64 bytes
86 * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
88 * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
90 * Every frame has the structure:
91 * <Device address><Frame length><Type><Payload><CRC>
93 * Device address: (uint8_t)
94 * Frame length: length in bytes including Type (uint8_t)
100 struct crsfPayloadRcChannelsPacked_s
{
101 // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
102 unsigned int chan0
: 11;
103 unsigned int chan1
: 11;
104 unsigned int chan2
: 11;
105 unsigned int chan3
: 11;
106 unsigned int chan4
: 11;
107 unsigned int chan5
: 11;
108 unsigned int chan6
: 11;
109 unsigned int chan7
: 11;
110 unsigned int chan8
: 11;
111 unsigned int chan9
: 11;
112 unsigned int chan10
: 11;
113 unsigned int chan11
: 11;
114 unsigned int chan12
: 11;
115 unsigned int chan13
: 11;
116 unsigned int chan14
: 11;
117 unsigned int chan15
: 11;
118 } __attribute__ ((__packed__
));
120 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t
;
122 #if defined(USE_CRSF_LINK_STATISTICS)
124 * 0x14 Link statistics
127 * uint8_t Uplink RSSI Ant. 1 ( dBm * -1 )
128 * uint8_t Uplink RSSI Ant. 2 ( dBm * -1 )
129 * uint8_t Uplink Package success rate / Link quality ( % )
130 * int8_t Uplink SNR ( db )
131 * uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
132 * uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
133 * uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
134 * uint8_t Downlink RSSI ( dBm * -1 )
135 * uint8_t Downlink package success rate / Link quality ( % )
136 * int8_t Downlink SNR ( db )
137 * Uplink is the connection from the ground to the UAV and downlink the opposite direction.
140 typedef struct crsfPayloadLinkstatistics_s
{
141 uint8_t uplink_RSSI_1
;
142 uint8_t uplink_RSSI_2
;
143 uint8_t uplink_Link_quality
;
145 uint8_t active_antenna
;
147 uint8_t uplink_TX_Power
;
148 uint8_t downlink_RSSI
;
149 uint8_t downlink_Link_quality
;
151 } crsfLinkStatistics_t
;
153 static timeUs_t lastLinkStatisticsFrameUs
;
155 static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t
* statsPtr
, timeUs_t currentTimeUs
)
157 const crsfLinkStatistics_t stats
= *statsPtr
;
158 lastLinkStatisticsFrameUs
= currentTimeUs
;
159 int16_t rssiDbm
= -1 * (stats
.active_antenna
? stats
.uplink_RSSI_2
: stats
.uplink_RSSI_1
);
160 if (rssiSource
== RSSI_SOURCE_RX_PROTOCOL_CRSF
) {
161 const uint16_t rssiPercentScaled
= scaleRange(rssiDbm
, CRSF_RSSI_MIN
, 0, 0, RSSI_MAX_VALUE
);
162 setRssi(rssiPercentScaled
, RSSI_SOURCE_RX_PROTOCOL_CRSF
);
164 #ifdef USE_RX_RSSI_DBM
165 if (rxConfig()->crsf_use_rx_snr
) {
166 rssiDbm
= stats
.uplink_SNR
;
168 setRssiDbm(rssiDbm
, RSSI_SOURCE_RX_PROTOCOL_CRSF
);
171 #ifdef USE_RX_LINK_QUALITY_INFO
172 if (linkQualitySource
== LQ_SOURCE_RX_PROTOCOL_CRSF
) {
173 setLinkQualityDirect(stats
.uplink_Link_quality
);
174 rxSetRfMode(stats
.rf_Mode
);
179 case DEBUG_CRSF_LINK_STATISTICS_UPLINK
:
180 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK
, 0, stats
.uplink_RSSI_1
);
181 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK
, 1, stats
.uplink_RSSI_2
);
182 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK
, 2, stats
.uplink_Link_quality
);
183 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK
, 3, stats
.rf_Mode
);
185 case DEBUG_CRSF_LINK_STATISTICS_PWR
:
186 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR
, 0, stats
.active_antenna
);
187 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR
, 1, stats
.uplink_SNR
);
188 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR
, 2, stats
.uplink_TX_Power
);
190 case DEBUG_CRSF_LINK_STATISTICS_DOWN
:
191 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN
, 0, stats
.downlink_RSSI
);
192 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN
, 1, stats
.downlink_Link_quality
);
193 DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN
, 2, stats
.downlink_SNR
);
200 #if defined(USE_CRSF_LINK_STATISTICS)
201 static void crsfCheckRssi(uint32_t currentTimeUs
) {
203 if (cmpTimeUs(currentTimeUs
, lastLinkStatisticsFrameUs
) > CRSF_LINK_STATUS_UPDATE_TIMEOUT_US
) {
204 if (rssiSource
== RSSI_SOURCE_RX_PROTOCOL_CRSF
) {
205 setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL_CRSF
);
206 #ifdef USE_RX_RSSI_DBM
207 if (rxConfig()->crsf_use_rx_snr
) {
208 setRssiDbmDirect(CRSF_SNR_MIN
, RSSI_SOURCE_RX_PROTOCOL_CRSF
);
210 setRssiDbmDirect(CRSF_RSSI_MIN
, RSSI_SOURCE_RX_PROTOCOL_CRSF
);
214 #ifdef USE_RX_LINK_QUALITY_INFO
215 if (linkQualitySource
== LQ_SOURCE_RX_PROTOCOL_CRSF
) {
216 setLinkQualityDirect(0);
223 STATIC_UNIT_TESTED
uint8_t crsfFrameCRC(void)
225 // CRC includes type and payload
226 uint8_t crc
= crc8_dvb_s2(0, crsfFrame
.frame
.type
);
227 for (int ii
= 0; ii
< crsfFrame
.frame
.frameLength
- CRSF_FRAME_LENGTH_TYPE_CRC
; ++ii
) {
228 crc
= crc8_dvb_s2(crc
, crsfFrame
.frame
.payload
[ii
]);
233 // Receive ISR callback, called back from serial port
234 STATIC_UNIT_TESTED
void crsfDataReceive(uint16_t c
, void *data
)
238 static uint8_t crsfFramePosition
= 0;
239 const timeUs_t currentTimeUs
= microsISR();
241 #ifdef DEBUG_CRSF_PACKETS
242 debug
[2] = currentTimeUs
- crsfFrameStartAtUs
;
245 if (cmpTimeUs(currentTimeUs
, crsfFrameStartAtUs
) > CRSF_TIME_NEEDED_PER_FRAME_US
) {
246 // We've received a character after max time needed to complete a frame,
247 // so this must be the start of a new frame.
248 crsfFramePosition
= 0;
251 if (crsfFramePosition
== 0) {
252 crsfFrameStartAtUs
= currentTimeUs
;
254 // assume frame is 5 bytes long until we have received the frame length
255 // full frame length includes the length of the address and framelength fields
256 const int fullFrameLength
= crsfFramePosition
< 3 ? 5 : crsfFrame
.frame
.frameLength
+ CRSF_FRAME_LENGTH_ADDRESS
+ CRSF_FRAME_LENGTH_FRAMELENGTH
;
258 if (crsfFramePosition
< fullFrameLength
) {
259 crsfFrame
.bytes
[crsfFramePosition
++] = (uint8_t)c
;
260 if (crsfFramePosition
>= fullFrameLength
) {
261 crsfFramePosition
= 0;
262 const uint8_t crc
= crsfFrameCRC();
263 if (crc
== crsfFrame
.bytes
[fullFrameLength
- 1]) {
264 switch (crsfFrame
.frame
.type
)
266 case CRSF_FRAMETYPE_RC_CHANNELS_PACKED
:
267 if (crsfFrame
.frame
.deviceAddress
== CRSF_ADDRESS_FLIGHT_CONTROLLER
) {
268 lastRcFrameTimeUs
= currentTimeUs
;
269 crsfFrameDone
= true;
270 memcpy(&crsfChannelDataFrame
, &crsfFrame
, sizeof(crsfFrame
));
274 #if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY)
275 case CRSF_FRAMETYPE_MSP_REQ
:
276 case CRSF_FRAMETYPE_MSP_WRITE
: {
277 uint8_t *frameStart
= (uint8_t *)&crsfFrame
.frame
.payload
+ CRSF_FRAME_ORIGIN_DEST_SIZE
;
278 if (bufferCrsfMspFrame(frameStart
, CRSF_FRAME_RX_MSP_FRAME_SIZE
)) {
279 crsfScheduleMspResponse();
284 #if defined(USE_CRSF_CMS_TELEMETRY)
285 case CRSF_FRAMETYPE_DEVICE_PING
:
286 crsfScheduleDeviceInfoResponse();
288 case CRSF_FRAMETYPE_DISPLAYPORT_CMD
: {
289 uint8_t *frameStart
= (uint8_t *)&crsfFrame
.frame
.payload
+ CRSF_FRAME_ORIGIN_DEST_SIZE
;
290 crsfProcessDisplayPortCmd(frameStart
);
294 #if defined(USE_CRSF_LINK_STATISTICS)
296 case CRSF_FRAMETYPE_LINK_STATISTICS
: {
297 // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
298 if ((rssiSource
== RSSI_SOURCE_RX_PROTOCOL_CRSF
) &&
299 (crsfFrame
.frame
.deviceAddress
== CRSF_ADDRESS_FLIGHT_CONTROLLER
) &&
300 (crsfFrame
.frame
.frameLength
== CRSF_FRAME_ORIGIN_DEST_SIZE
+ CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE
)) {
301 const crsfLinkStatistics_t
* statsFrame
= (const crsfLinkStatistics_t
*)&crsfFrame
.frame
.payload
;
302 handleCrsfLinkStatisticsFrame(statsFrame
, currentTimeUs
);
315 STATIC_UNIT_TESTED
uint8_t crsfFrameStatus(rxRuntimeState_t
*rxRuntimeState
)
317 UNUSED(rxRuntimeState
);
319 #if defined(USE_CRSF_LINK_STATISTICS)
320 crsfCheckRssi(micros());
323 crsfFrameDone
= false;
325 // unpack the RC channels
326 const crsfPayloadRcChannelsPacked_t
* const rcChannels
= (crsfPayloadRcChannelsPacked_t
*)&crsfChannelDataFrame
.frame
.payload
;
327 crsfChannelData
[0] = rcChannels
->chan0
;
328 crsfChannelData
[1] = rcChannels
->chan1
;
329 crsfChannelData
[2] = rcChannels
->chan2
;
330 crsfChannelData
[3] = rcChannels
->chan3
;
331 crsfChannelData
[4] = rcChannels
->chan4
;
332 crsfChannelData
[5] = rcChannels
->chan5
;
333 crsfChannelData
[6] = rcChannels
->chan6
;
334 crsfChannelData
[7] = rcChannels
->chan7
;
335 crsfChannelData
[8] = rcChannels
->chan8
;
336 crsfChannelData
[9] = rcChannels
->chan9
;
337 crsfChannelData
[10] = rcChannels
->chan10
;
338 crsfChannelData
[11] = rcChannels
->chan11
;
339 crsfChannelData
[12] = rcChannels
->chan12
;
340 crsfChannelData
[13] = rcChannels
->chan13
;
341 crsfChannelData
[14] = rcChannels
->chan14
;
342 crsfChannelData
[15] = rcChannels
->chan15
;
343 return RX_FRAME_COMPLETE
;
345 return RX_FRAME_PENDING
;
348 STATIC_UNIT_TESTED
float crsfReadRawRC(const rxRuntimeState_t
*rxRuntimeState
, uint8_t chan
)
350 UNUSED(rxRuntimeState
);
351 /* conversion from RC value to PWM
356 * scale factor = (2012-988) / (1811-172) = 0.62477120195241
357 * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
359 return (0.62477120195241f
* (float)crsfChannelData
[chan
]) + 881;
362 void crsfRxWriteTelemetryData(const void *data
, int len
)
364 len
= MIN(len
, (int)sizeof(telemetryBuf
));
365 memcpy(telemetryBuf
, data
, len
);
366 telemetryBufLen
= len
;
369 void crsfRxSendTelemetryData(void)
371 // if there is telemetry data to write
372 if (telemetryBufLen
> 0) {
373 serialWriteBuf(serialPort
, telemetryBuf
, telemetryBufLen
);
374 telemetryBufLen
= 0; // reset telemetry buffer
378 static timeUs_t
crsfFrameTimeUs(void)
380 return lastRcFrameTimeUs
;
383 bool crsfRxInit(const rxConfig_t
*rxConfig
, rxRuntimeState_t
*rxRuntimeState
)
385 for (int ii
= 0; ii
< CRSF_MAX_CHANNEL
; ++ii
) {
386 crsfChannelData
[ii
] = (16 * rxConfig
->midrc
) / 10 - 1408;
389 rxRuntimeState
->channelCount
= CRSF_MAX_CHANNEL
;
390 rxRuntimeState
->rxRefreshRate
= CRSF_TIME_BETWEEN_FRAMES_US
; //!!TODO this needs checking
392 rxRuntimeState
->rcReadRawFn
= crsfReadRawRC
;
393 rxRuntimeState
->rcFrameStatusFn
= crsfFrameStatus
;
394 rxRuntimeState
->rcFrameTimeUsFn
= crsfFrameTimeUs
;
396 const serialPortConfig_t
*portConfig
= findSerialPortConfig(FUNCTION_RX_SERIAL
);
401 serialPort
= openSerialPort(portConfig
->identifier
,
407 CRSF_PORT_OPTIONS
| (rxConfig
->serialrx_inverted
? SERIAL_INVERTED
: 0)
410 if (rssiSource
== RSSI_SOURCE_NONE
) {
411 rssiSource
= RSSI_SOURCE_RX_PROTOCOL_CRSF
;
413 #ifdef USE_RX_LINK_QUALITY_INFO
414 if (linkQualitySource
== LQ_SOURCE_NONE
) {
415 linkQualitySource
= LQ_SOURCE_RX_PROTOCOL_CRSF
;
419 return serialPort
!= NULL
;
422 bool crsfRxIsActive(void)
424 return serialPort
!= NULL
;