src/main/target/SITL/serial_proxy.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include "serial_proxy.h"
  26
  27 #if defined(SITL_BUILD)
  28
  29 #include <stdbool.h>
  30 #include <stdint.h>
  31 #include <stdio.h>
  32 #include <stdlib.h>
  33 #include <string.h>
  34 #include <sys/select.h>
  35 #include <time.h>
  36 #include <unistd.h>
  37
  38 #include "platform.h"
  39
  40 #include <sys/types.h>
  41 #include <fcntl.h>
  42 #include <errno.h>
  43 #include <unistd.h>
  44 #include <unistd.h>
  45
  46 #include "drivers/time.h"
  47 #include "msp/msp_serial.h"
  48 #include "msp/msp_protocol.h"
  49 #include "common/crc.h"
  50 #include "rx/sim.h"
  51
  52 #include <sys/ioctl.h>
  53
  54 #ifdef  __FreeBSD__
  55 # define __BSD_VISIBLE 1
  56 #endif
  57
  58 #ifdef __linux__
  59 #include <asm/termbits.h>
  60 #ifndef TCGETS2
  61 #include <asm-generic/ioctls.h>
  62 #endif
  63 #else
  64 #include <termios.h>
  65 #endif
  66
  67 #ifdef __APPLE__
  68 #include <IOKit/serial/ioss.h>
  69 #endif
  70
  71 #include "drivers/serial_tcp.h"
  72
  73 #define SYM_BEGIN       '$'
  74 #define SYM_PROTO_V1    'M'
  75 #define SYM_PROTO_V2    'X'
  76 #define SYM_FROM_MWC    '>'
  77 #define SYM_TO_MWC      '<'
  78 #define SYM_UNSUPPORTED '!'
  79
  80 #define JUMBO_FRAME_MIN_SIZE  255
  81 #define MAX_MSP_MESSAGE 1024
  82 #define RX_CONFIG_SIZE 24
  83
  84 typedef enum {
  85     DS_IDLE,
  86     DS_PROTO_IDENTIFIER,
  87     DS_DIRECTION_V1,
  88     DS_DIRECTION_V2,
  89     DS_FLAG_V2,
  90     DS_PAYLOAD_LENGTH_V1,
  91     DS_PAYLOAD_LENGTH_JUMBO_LOW,
  92     DS_PAYLOAD_LENGTH_JUMBO_HIGH,
  93     DS_PAYLOAD_LENGTH_V2_LOW,
  94     DS_PAYLOAD_LENGTH_V2_HIGH,
  95     DS_CODE_V1,
  96     DS_CODE_JUMBO_V1,
  97     DS_CODE_V2_LOW,
  98     DS_CODE_V2_HIGH,
  99     DS_PAYLOAD_V1,
 100     DS_PAYLOAD_V2,
 101     DS_CHECKSUM_V1,
 102     DS_CHECKSUM_V2,
 103 } TDecoderState;
 104
 105 static TDecoderState decoderState = DS_IDLE;
 106
 107 typedef enum {
 108     RXC_IDLE = 0,
 109     RXC_RQ   = 1,
 110     RXC_DONE = 2
 111 } TRXConfigState;
 112
 113 static TRXConfigState rxConfigState = RXC_IDLE;
 114
 115 static int message_length_expected;
 116 static unsigned char message_buffer[MAX_MSP_MESSAGE];
 117 static int message_length_received;
 118 static int unsupported;
 119 static int code;
 120 static int message_direction;
 121 static uint8_t message_checksum;
 122 static int reqCount = 0;
 123 static uint16_t rssi = 0;
 124 static uint8_t rxConfigBuffer[RX_CONFIG_SIZE];
 125
 126 static timeMs_t lastWarning = 0;
 127
 128 int serialUartIndex = -1;
 129 char serialPort[64] = "";
 130 int serialBaudRate = 115200;
 131 OptSerialStopBits_e serialStopBits = OPT_SERIAL_STOP_BITS_ONE;  //0:None|1:One|2:OnePointFive|3:Two
 132 OptSerialParity_e serialParity = OPT_SERIAL_PARITY_NONE;
 133 bool serialFCProxy = false;
 134
 135 #define FC_PROXY_REQUEST_PERIOD_MIN_MS 20  //inav can handle 100 msp messages per second max
 136 #define FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS 200
 137 #define FC_PROXY_REQUEST_PERIOD_RSSI_MS 300
 138 #define SERIAL_BUFFER_SIZE 256
 139
 140 #if defined(__CYGWIN__)
 141 #include <windows.h>
 142 static HANDLE hSerial;
 143 #else
 144 static int fd;
 145 #endif
 146
 147 static bool connected = false;
 148 static bool started = false;
 149
 150 static timeMs_t nextProxyRequestTimeout = 0;
 151 static timeMs_t nextProxyRequestMin = 0;
 152 static timeMs_t nextProxyRequestRssi = 0;
 153
 154 static timeMs_t lastVisit = 0;
 155
 156 #if !defined(__CYGWIN__)
 157 #if !defined( __linux__) && !defined(__APPLE__)
 158 static int rate_to_constant(int baudrate)
 159 {
 160 #ifdef __FreeBSD__
 161     return baudrate;
 162 #else
 163 #define B(x) case x: return B##x
 164     switch (baudrate) {
 165             B(50);
 166             B(75);
 167             B(110);
 168             B(134);
 169             B(150);
 170             B(200);
 171             B(300);
 172             B(600);
 173             B(1200);
 174             B(1800);
 175             B(2400);
 176             B(4800);
 177             B(9600);
 178             B(19200);
 179             B(38400);
 180             B(57600);
 181             B(115200);
 182             B(230400);
 183         default:
 184             return 0;
 185     }
 186 #undef B
 187 #endif
 188 }
 189 #endif
 190
 191 static void close_serial(void)
 192 {
 193 #ifdef __linux__
 194     ioctl(fd, TCFLSH, TCIOFLUSH);
 195 #else
 196     tcflush(fd, TCIOFLUSH);
 197 #endif
 198     close(fd);
 199 }
 200
 201 static int set_fd_speed(void)
 202 {
 203     int res = -1;
 204 #ifdef __linux__
 205     // Just user BOTHER for everything (allows non-standard speeds)
 206     struct termios2 t;
 207     if ((res = ioctl(fd, TCGETS2, &t)) != -1) {
 208         t.c_cflag &= ~CBAUD;
 209         t.c_cflag |= BOTHER;
 210         t.c_ospeed = t.c_ispeed = serialBaudRate;
 211         res = ioctl(fd, TCSETS2, &t);
 212     }
 213 #elif __APPLE__
 214     speed_t speed = serialBaudRate;
 215     res = ioctl(fd, IOSSIOSPEED, &speed);
 216 #else
 217     int speed = rate_to_constant(serialBaudRate);
 218     struct termios term;
 219     if (tcgetattr(fd, &term)) return -1;
 220     if (speed == 0) {
 221         res = -1;
 222     } else {
 223         if (cfsetispeed(&term, speed) != -1) {
 224             cfsetospeed(&term, speed);
 225             res = tcsetattr(fd, TCSANOW, &term);
 226         }
 227         if (res != -1) {
 228             memset(&term, 0, sizeof(term));
 229             res = (tcgetattr(fd, &term));
 230         }
 231     }
 232 #endif
 233     return res;
 234 }
 235
 236 static int set_attributes(void)
 237 {
 238     struct termios tio;
 239     memset (&tio, 0, sizeof(tio));
 240     int res = -1;
 241 #ifdef __linux__
 242     res = ioctl(fd, TCGETS, &tio);
 243 #else
 244     res = tcgetattr(fd, &tio);
 245 #endif
 246     if (res != -1) {
 247         // cfmakeraw ...
 248         tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
 249         tio.c_oflag &= ~OPOST;
 250         tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
 251         tio.c_cflag &= ~(CSIZE | PARENB);
 252         tio.c_cflag |=  CS8 | CREAD | CLOCAL;
 253         tio.c_cc[VTIME] = 0;
 254         tio.c_cc[VMIN] = 0;
 255
 256         switch (serialStopBits) {
 257             case OPT_SERIAL_STOP_BITS_ONE:
 258                 tio.c_cflag &= ~CSTOPB;
 259                 break;
 260             case OPT_SERIAL_STOP_BITS_TWO:
 261                 tio.c_cflag |= CSTOPB;
 262                 break;
 263             case OPT_SERIAL_STOP_BITS_INVALID:
 264                 break;
 265         }
 266
 267         switch (serialParity) {
 268             case OPT_SERIAL_PARITY_EVEN:
 269                 tio.c_cflag |= PARENB;
 270                 tio.c_cflag &= ~PARODD;
 271                 break;
 272             case OPT_SERIAL_PARITY_NONE:
 273                 tio.c_cflag &= ~PARENB;
 274                 tio.c_cflag &= ~PARODD;
 275                 break;
 276             case OPT_SERIAL_PARITY_ODD:
 277                 tio.c_cflag |= PARENB;
 278                 tio.c_cflag |= PARODD;
 279                 break;
 280             case OPT_SERIAL_PARITY_INVALID:
 281                 break;
 282         }
 283 #ifdef __linux__
 284         res = ioctl(fd, TCSETS, &tio);
 285 #else
 286         res = tcsetattr(fd, TCSANOW, &tio);
 287 #endif
 288     }
 289     if (res != -1) {
 290         res = set_fd_speed();
 291     }
 292     return res;
 293 }
 294 #endif
 295
 296 void serialProxyInit(void)
 297 {
 298     char portName[64 + 20];
 299     if ( strlen(serialPort) < 1) {
 300         return;
 301     }
 302     connected = false;
 303
 304 #if defined(__CYGWIN__)
 305     sprintf(portName, "\\\\.\\%s", serialPort);
 306
 307     hSerial = CreateFile(portName,
 308                          GENERIC_READ | GENERIC_WRITE,
 309                          0,
 310                          NULL,
 311                          OPEN_EXISTING,
 312                          FILE_ATTRIBUTE_NORMAL,
 313                          NULL);
 314
 315     if (hSerial == INVALID_HANDLE_VALUE) {
 316         if (GetLastError() == ERROR_FILE_NOT_FOUND) {
 317             fprintf(stderr, "[SERIALPROXY] ERROR: Sserial port was not attached. Reason: %s not available.\n", portName);
 318         } else {
 319             fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port, unknown error.\n");
 320         }
 321         return;
 322     } else {
 323         DCB dcbSerialParams = { 0 };
 324         if (!GetCommState(hSerial, &dcbSerialParams)) {
 325             fprintf(stderr, "[SERIALPROXY] ALERT: failed to get current serial parameters!\n");
 326         } else {
 327             dcbSerialParams.BaudRate = serialBaudRate;
 328             dcbSerialParams.ByteSize = 8;
 329
 330             // Disable software flow control (XON/XOFF)
 331             dcbSerialParams.fOutX = FALSE;
 332             dcbSerialParams.fInX = FALSE;
 333
 334             // Disable hardware flow control (RTS/CTS)
 335             dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
 336             dcbSerialParams.fDtrControl = DTR_CONTROL_DISABLE;
 337
 338             // Disable any special processing of bytes
 339             dcbSerialParams.fBinary = TRUE;
 340
 341             switch (serialStopBits) {
 342                 case OPT_SERIAL_STOP_BITS_ONE:
 343                     dcbSerialParams.StopBits = ONESTOPBIT;
 344                     break;
 345                 case OPT_SERIAL_STOP_BITS_TWO:
 346                     dcbSerialParams.StopBits = TWOSTOPBITS;
 347                     break;
 348                 case OPT_SERIAL_STOP_BITS_INVALID:
 349                     break;
 350             }
 351
 352             switch (serialParity) {
 353                 case OPT_SERIAL_PARITY_EVEN:
 354                     dcbSerialParams.Parity = EVENPARITY;
 355                     break;
 356                 case OPT_SERIAL_PARITY_NONE:
 357                     dcbSerialParams.Parity = NOPARITY;
 358                     break;
 359                 case OPT_SERIAL_PARITY_ODD:
 360                     dcbSerialParams.Parity = ODDPARITY;
 361                     break;
 362                 case OPT_SERIAL_PARITY_INVALID:
 363                     break;
 364             }
 365
 366             if (!SetCommState(hSerial, &dcbSerialParams)) {
 367                 fprintf(stderr, "[SERIALPROXY] ALERT: Could not set Serial Port parameters\n");
 368             } else {
 369                 COMMTIMEOUTS comTimeOut;
 370                 comTimeOut.ReadIntervalTimeout = MAXDWORD;
 371                 comTimeOut.ReadTotalTimeoutMultiplier = 0;
 372                 comTimeOut.ReadTotalTimeoutConstant = 0;
 373                 comTimeOut.WriteTotalTimeoutMultiplier = 0;
 374                 comTimeOut.WriteTotalTimeoutConstant = 0;
 375                 SetCommTimeouts(hSerial, &comTimeOut);
 376             }
 377         }
 378     }
 379 #else
 380     strcpy(portName, serialPort); // because, windows ...
 381     fd = open(serialPort, O_RDWR | O_NOCTTY);
 382     if (fd != -1) {
 383         int res = 1;
 384         res = set_attributes();
 385         if (res == -1) {
 386             fprintf(stderr, "[SERIALPROXY] ALERT: Failed to configure device: %s %s\n", portName, strerror(errno));
 387             close(fd);
 388             fd = -1;
 389         }
 390     } else {
 391         fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port %s\n", portName);
 392         return;
 393     }
 394 #endif
 395     connected = true;
 396
 397     if ( !serialFCProxy ) {
 398         fprintf(stderr, "[SERIALPROXY] Connected %s to UART%d\n", portName, serialUartIndex);
 399     } else {
 400         fprintf(stderr, "[SERIALPROXY] Using proxy flight controller on %s\n", portName);
 401     }
 402 }
 403
 404 void serialProxyStart(void)
 405 {
 406     started = true;
 407 }
 408
 409 void serialProxyClose(void)
 410 {
 411     if (connected) {
 412         connected = false;
 413 #if defined(__CYGWIN__)
 414         CloseHandle(hSerial);
 415 #else
 416         close_serial();
 417 #endif
 418         started = false;
 419         nextProxyRequestTimeout = 0;
 420         nextProxyRequestMin = 0;
 421         nextProxyRequestRssi = 0;
 422         lastWarning = 0;
 423         lastVisit = 0;
 424     }
 425 }
 426
 427 static bool canOutputWarning(void)
 428 {
 429     if ( millis() > lastWarning ) {
 430         lastWarning = millis() + 5000;
 431         return true;
 432     }
 433     return false;
 434 }
 435
 436 int serialProxyReadData(unsigned char *buffer, unsigned int nbChar)
 437 {
 438     if (!connected) return 0;
 439
 440 #if defined(__CYGWIN__)
 441     COMSTAT status;
 442     DWORD errors;
 443     DWORD bytesRead;
 444
 445     ClearCommError(hSerial, &errors, &status);
 446     if (status.cbInQue > 0) {
 447         unsigned int toRead = (status.cbInQue > nbChar) ? nbChar : status.cbInQue;
 448         if (ReadFile(hSerial, buffer, toRead, &bytesRead, NULL) && (bytesRead != 0)) {
 449             return bytesRead;
 450         }
 451     }
 452     return 0;
 453 #else
 454     if (nbChar == 0) return 0;
 455     int bytesRead = read(fd, buffer, nbChar);
 456     return bytesRead;
 457 #endif
 458 }
 459
 460 bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar)
 461 {
 462     if (!connected) return false;
 463
 464 #if defined(__CYGWIN__)
 465     COMSTAT status;
 466     DWORD errors;
 467     DWORD bytesSent;
 468     if (!WriteFile(hSerial, (void *)buffer, nbChar, &bytesSent, 0)) {
 469         ClearCommError(hSerial, &errors, &status);
 470         if ( canOutputWarning() ) {
 471             fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
 472         }
 473         return false;
 474     }
 475     if ( bytesSent != nbChar ) {
 476         if ( canOutputWarning() ) {
 477             fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
 478         }
 479     }
 480 #else
 481     ssize_t l = write(fd, buffer, nbChar);
 482     if ( l != (ssize_t)nbChar ) {
 483         if ( canOutputWarning() ) {
 484             fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
 485         }
 486         return false;
 487     }
 488 #endif
 489     return true;
 490 }
 491
 492 bool serialProxyIsConnected(void)
 493 {
 494     return connected;
 495 }
 496
 497 static void mspSendCommand(int cmd, unsigned char *data, int dataLen)
 498 {
 499     uint8_t msgBuf[MAX_MSP_MESSAGE] = { '$', 'X', '<' };
 500     int msgLen = 3;
 501
 502     mspHeaderV2_t *hdrV2 = (mspHeaderV2_t *)&msgBuf[msgLen];
 503     hdrV2->flags = 0;
 504     hdrV2->cmd = cmd;
 505     hdrV2->size = dataLen;
 506     msgLen += sizeof(mspHeaderV2_t);
 507
 508     for ( int i = 0; i < dataLen; i++ ) {
 509         msgBuf[msgLen++] = data[i];
 510     }
 511
 512     uint8_t crc = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
 513     crc = crc8_dvb_s2_update(crc, data, dataLen);
 514     msgBuf[msgLen] = crc;
 515     msgLen++;
 516
 517     serialProxyWriteData((unsigned char *)&msgBuf, msgLen);
 518 }
 519
 520 static void mspRequestChannels(void)
 521 {
 522     mspSendCommand(MSP_RC, NULL, 0);
 523 }
 524
 525 static void mspRequestRssi(void)
 526 {
 527     mspSendCommand(MSP_ANALOG, NULL, 0);
 528 }
 529
 530 static void requestRXConfigState(void)
 531 {
 532     mspSendCommand(MSP_RX_CONFIG, NULL, 0);
 533     rxConfigState = RXC_RQ;
 534     fprintf(stderr, "[SERIALPROXY] Requesting RX config from proxy FC...\n");
 535 }
 536
 537 static void processMessage(void)
 538 {
 539     if ( code == MSP_RC ) {
 540         if ( reqCount > 0 ) reqCount--;
 541         int count = message_length_received / 2;
 542         if ( count <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
 543             uint16_t *channels = (uint16_t *)(&message_buffer[0]);
 544             //AETR => AERT
 545             uint v = channels[2];
 546             channels[2] = channels[3];
 547             channels[3] = v;
 548             if ( rssi > 0 ) {
 549                 rxSimSetChannelValue(channels, count);
 550             }
 551         }
 552     } else if ( code == MSP_ANALOG ) {
 553         if ( reqCount > 0 ) reqCount--;
 554         if ( message_length_received >= 7 ) {
 555             rssi = *((uint16_t *)(&message_buffer[3]));
 556             rxSimSetRssi( rssi );
 557         }
 558     } else if ( code == MSP_RX_CONFIG ) {
 559         memcpy( &(rxConfigBuffer[0]), &(message_buffer[0]), RX_CONFIG_SIZE );
 560         *((uint16_t *) & (rxConfigBuffer[1])) = 2500; //maxcheck
 561         *((uint16_t *) & (rxConfigBuffer[5])) = 500; //mincheck
 562
 563         mspSendCommand( MSP_SET_RX_CONFIG, rxConfigBuffer, RX_CONFIG_SIZE );
 564         rxConfigState = RXC_DONE;
 565         fprintf(stderr, "[SERIALPROXY] Setting RX config on proxy FC...\n");
 566     }
 567 }
 568
 569 static void decodeProxyMessages(unsigned char *data, int len)
 570 {
 571     for (int i = 0; i < len; i++) {
 572         switch (decoderState) {
 573             case DS_IDLE: // sync char 1
 574                 if (data[i] == SYM_BEGIN) {
 575                     decoderState = DS_PROTO_IDENTIFIER;
 576                 }
 577                 break;
 578
 579             case DS_PROTO_IDENTIFIER: // sync char 2
 580                 switch (data[i]) {
 581                     case SYM_PROTO_V1:
 582                         decoderState = DS_DIRECTION_V1;
 583                         break;
 584                     case SYM_PROTO_V2:
 585                         decoderState = DS_DIRECTION_V2;
 586                         break;
 587                     default:
 588                         //unknown protocol
 589                         decoderState = DS_IDLE;
 590                 }
 591                 break;
 592
 593             case DS_DIRECTION_V1: // direction (should be >)
 594
 595             case DS_DIRECTION_V2:
 596                 unsupported = 0;
 597                 switch (data[i]) {
 598                     case SYM_FROM_MWC:
 599                         message_direction = 1;
 600                         break;
 601                     case SYM_TO_MWC:
 602                         message_direction = 0;
 603                         break;
 604                     case SYM_UNSUPPORTED:
 605                         unsupported = 1;
 606                         break;
 607                 }
 608                 decoderState = decoderState == DS_DIRECTION_V1 ? DS_PAYLOAD_LENGTH_V1 : DS_FLAG_V2;
 609                 break;
 610
 611             case DS_FLAG_V2:
 612                 // Ignored for now
 613                 decoderState = DS_CODE_V2_LOW;
 614                 break;
 615
 616             case DS_PAYLOAD_LENGTH_V1:
 617                 message_length_expected = data[i];
 618
 619                 if (message_length_expected == JUMBO_FRAME_MIN_SIZE) {
 620                     decoderState = DS_CODE_JUMBO_V1;
 621                 } else {
 622                     message_length_received = 0;
 623                     decoderState = DS_CODE_V1;
 624                 }
 625                 break;
 626
 627             case DS_PAYLOAD_LENGTH_V2_LOW:
 628                 message_length_expected = data[i];
 629                 decoderState = DS_PAYLOAD_LENGTH_V2_HIGH;
 630                 break;
 631
 632             case DS_PAYLOAD_LENGTH_V2_HIGH:
 633                 message_length_expected |= data[i] << 8;
 634                 message_length_received = 0;
 635                 if (message_length_expected <= MAX_MSP_MESSAGE) {
 636                     decoderState = message_length_expected > 0 ? DS_PAYLOAD_V2 : DS_CHECKSUM_V2;
 637                 } else {
 638                     //too large payload
 639                     decoderState = DS_IDLE;
 640                 }
 641                 break;
 642
 643             case DS_CODE_V1:
 644             case DS_CODE_JUMBO_V1:
 645                 code = data[i];
 646                 if (message_length_expected > 0) {
 647                     // process payload
 648                     if (decoderState == DS_CODE_JUMBO_V1) {
 649                         decoderState = DS_PAYLOAD_LENGTH_JUMBO_LOW;
 650                     } else {
 651                         decoderState = DS_PAYLOAD_V1;
 652                     }
 653                 } else {
 654                     // no payload
 655                     decoderState = DS_CHECKSUM_V1;
 656                 }
 657                 break;
 658
 659             case DS_CODE_V2_LOW:
 660                 code = data[i];
 661                 decoderState = DS_CODE_V2_HIGH;
 662                 break;
 663
 664             case DS_CODE_V2_HIGH:
 665                 code |= data[i] << 8;
 666                 decoderState = DS_PAYLOAD_LENGTH_V2_LOW;
 667                 break;
 668
 669             case DS_PAYLOAD_LENGTH_JUMBO_LOW:
 670                 message_length_expected = data[i];
 671                 decoderState = DS_PAYLOAD_LENGTH_JUMBO_HIGH;
 672                 break;
 673
 674             case DS_PAYLOAD_LENGTH_JUMBO_HIGH:
 675                 message_length_expected |= data[i] << 8;
 676                 message_length_received = 0;
 677                 decoderState = DS_PAYLOAD_V1;
 678                 break;
 679
 680             case DS_PAYLOAD_V1:
 681             case DS_PAYLOAD_V2:
 682                 message_buffer[message_length_received] = data[i];
 683                 message_length_received++;
 684
 685                 if (message_length_received >= message_length_expected) {
 686                     decoderState = decoderState == DS_PAYLOAD_V1 ? DS_CHECKSUM_V1 : DS_CHECKSUM_V2;
 687                 }
 688                 break;
 689
 690             case DS_CHECKSUM_V1:
 691                 if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
 692                     message_checksum = JUMBO_FRAME_MIN_SIZE;
 693                 } else {
 694                     message_checksum = message_length_expected;
 695                 }
 696                 message_checksum ^= code;
 697                 if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
 698                     message_checksum ^= message_length_expected & 0xFF;
 699                     message_checksum ^= (message_length_expected & 0xFF00) >> 8;
 700                 }
 701                 for (int ii = 0; ii < message_length_received; ii++) {
 702                     message_checksum ^= message_buffer[ii];
 703                 }
 704                 if (message_checksum == data[i]) {
 705                     processMessage();
 706                 }
 707                 decoderState = DS_IDLE;
 708                 break;
 709
 710             case DS_CHECKSUM_V2:
 711                 message_checksum = 0;
 712                 message_checksum = crc8_dvb_s2(message_checksum, 0); // flag
 713                 message_checksum = crc8_dvb_s2(message_checksum, code & 0xFF);
 714                 message_checksum = crc8_dvb_s2(message_checksum, (code & 0xFF00) >> 8);
 715                 message_checksum = crc8_dvb_s2(message_checksum, message_length_expected & 0xFF);
 716                 message_checksum = crc8_dvb_s2(message_checksum, (message_length_expected & 0xFF00) >> 8);
 717                 for (int ii = 0; ii < message_length_received; ii++) {
 718                     message_checksum = crc8_dvb_s2(message_checksum, message_buffer[ii]);
 719                 }
 720                 if (message_checksum == data[i]) {
 721                     processMessage();
 722                 }
 723                 decoderState = DS_IDLE;
 724                 break;
 725
 726             default:
 727                 break;
 728         }
 729     }
 730 }
 731
 732 void serialProxyProcess(void)
 733 {
 734
 735     if (!started) return;
 736     if (!connected) return;
 737
 738     if ((millis() - lastVisit) > 500) {
 739         if ( lastVisit > 0 ) {
 740             fprintf(stderr, "timeout=%dms\n", millis() - lastVisit);
 741         }
 742     }
 743     lastVisit = millis();
 744
 745     if ( serialFCProxy ) {
 746         //first we have to change FC min_check and max_check thresholds to avoid activating stick commands on proxy FC
 747         if ( rxConfigState == RXC_IDLE ) {
 748             requestRXConfigState();
 749         } else if ( rxConfigState == RXC_DONE ) {
 750             if ( (nextProxyRequestTimeout <= millis()) || ((reqCount == 0) && (nextProxyRequestMin <= millis()))) {
 751                 nextProxyRequestTimeout = millis() + FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS;
 752                 nextProxyRequestMin = millis() + FC_PROXY_REQUEST_PERIOD_MIN_MS;
 753                 mspRequestChannels();
 754                 reqCount++;
 755             }
 756
 757             if ( nextProxyRequestRssi <= millis()) {
 758                 nextProxyRequestRssi = millis() + FC_PROXY_REQUEST_PERIOD_RSSI_MS;
 759                 mspRequestRssi();
 760                 reqCount++;
 761             }
 762         }
 763
 764         unsigned char buf[SERIAL_BUFFER_SIZE];
 765         int count = serialProxyReadData(buf, SERIAL_BUFFER_SIZE);
 766         if (count == 0) return;
 767         decodeProxyMessages(buf, count);
 768
 769     } else {
 770
 771         if ( serialUartIndex == -1 )  return;
 772         unsigned char buf[SERIAL_BUFFER_SIZE];
 773         uint32_t avail = tcpRXBytesFree(serialUartIndex - 1);
 774         if ( avail == 0 ) return;
 775         if (avail > SERIAL_BUFFER_SIZE) avail = SERIAL_BUFFER_SIZE;
 776
 777         int count = serialProxyReadData(buf, avail);
 778         if (count == 0) return;
 779
 780         tcpReceiveBytesEx( serialUartIndex - 1, buf, count);
 781     }
 782
 783 }
 784 #endif