11#include " crsf.h"
22
33crsf::crsf (Stream *rxPort, int rxPin, int txPin, bool inverted)
4- : SerialIO(rxPort, rxPin, txPin, inverted){};
4+ : SerialIO(rxPort, rxPin, txPin, inverted) {};
55
6- void crsf::begin () {
6+ void
7+ crsf::begin ()
8+ {
79
810// Initialize the serial port
911#if defined(ARDUINO_ARCH_ESP32)
@@ -20,45 +22,51 @@ void crsf::begin() {
2022#endif
2123}
2224
23- void crsf::processIncoming () {
25+ void
26+ crsf::processIncoming ()
27+ {
2428 uint8_t size = CRSF_MAX_PACKET_SIZE ;
25- while (_rxPort->available ()) {
29+ while (_rxPort->available ()) {
2630 _rxData[CRSF_MAX_PACKET_SIZE - 1 ] = _rxPort->read ();
27- if (crc8 (&_rxData[CRSF_MAX_PACKET_SIZE - size],
28- _rxData[CRSF_MAX_PACKET_SIZE - size - 1 ]) == 0 ) {
29- if ((_rxData[CRSF_MAX_PACKET_SIZE - size - 2 ] ==
30- CRSF_ADDRESS_FLIGHT_CONTROLLER ) ||
31- (_rxData[CRSF_MAX_PACKET_SIZE - size - 2 ] ==
32- CRSF_ADDRESS_CRSF_TRANSMITTER )) {
33- if (_rxData[CRSF_MAX_PACKET_SIZE - size] ==
34- CRSF_FRAMETYPE_RC_CHANNELS_PACKED ) {
35- memcpy (&_channelData, &_rxData[CRSF_MAX_PACKET_SIZE - size + 1 ],
31+ if (crc8 (&_rxData[CRSF_MAX_PACKET_SIZE - size],
32+ _rxData[CRSF_MAX_PACKET_SIZE - size - 1 ]) == 0 ) {
33+ if ((_rxData[CRSF_MAX_PACKET_SIZE - size - 2 ] ==
34+ CRSF_ADDRESS_FLIGHT_CONTROLLER ) ||
35+ (_rxData[CRSF_MAX_PACKET_SIZE - size - 2 ] ==
36+ CRSF_ADDRESS_CRSF_TRANSMITTER )) {
37+ if (_rxData[CRSF_MAX_PACKET_SIZE - size] ==
38+ CRSF_FRAMETYPE_RC_CHANNELS_PACKED ) {
39+ memcpy (&_channelData,
40+ &_rxData[CRSF_MAX_PACKET_SIZE - size + 1 ],
3641 sizeof (_channelData));
3742 }
3843 }
3944 }
40- if (_rxData[CRSF_MAX_PACKET_SIZE - 2 ] ==
41- CRSF_ADDRESS_CRSF_TRANSMITTER ||
42- _rxData[CRSF_MAX_PACKET_SIZE - 2 ] ==
43- CRSF_ADDRESS_FLIGHT_CONTROLLER ) {
45+ if (_rxData[CRSF_MAX_PACKET_SIZE - 2 ] == CRSF_ADDRESS_CRSF_TRANSMITTER ||
46+ _rxData[CRSF_MAX_PACKET_SIZE - 2 ] == CRSF_ADDRESS_FLIGHT_CONTROLLER ) {
4447 size = _rxData[CRSF_MAX_PACKET_SIZE - 1 ];
45- }
48+ }
4649 leftShift (_rxData, sizeof (_rxData));
4750 }
4851}
4952
50- void crsf::getChannel (rc_channels_t *channelData) {
53+ void
54+ crsf::getChannel (rc_channels_t *channelData)
55+ {
5156 memcpy (channelData, &_channelData, sizeof (rc_channels_t ));
5257}
5358
54- uint8_t crsf::crc8 (uint8_t *data, uint8_t len) {
59+ uint8_t
60+ crsf::crc8 (uint8_t *data, uint8_t len)
61+ {
5562 uint8_t crc = 0 ;
56- for (uint8_t i = 0 ; i < len; i++) {
63+ for (uint8_t i = 0 ; i < len; i++) {
5764 crc ^= data[i];
58- for (uint8_t j = 0 ; j < 8 ; j++) {
59- if (crc & 0x80 ) {
65+ for (uint8_t j = 0 ; j < 8 ; j++) {
66+ if (crc & 0x80 ) {
6067 crc = (crc << 1 ) ^ CRC8_POLY_D5 ;
61- } else {
68+ }
69+ else {
6270 crc <<= 1 ;
6371 }
6472 }
0 commit comments