src/main/tracker/protocol_detection.c

   1 /*
   2  * This file is part of u360gts, aka amv-open360tracker 32bits:
   3  * https://github.com/raul-ortega/amv-open360tracker-32bits
   4  *
   5  * The code below is an adaptation by Ra�l Ortega of the original code of Ghettostation antenna tracker
   6  * https://github.com/KipK/Ghettostation
   7  *
   8  * u360gts is free software: you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation, either version 3 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * u360gts is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with u360gts.  If not, see <http://www.gnu.org/licenses/>.
  20  *
  21  */
  22
  23 /*
  24
  25 */
  26 #include "config.h"
  27 #include <stdint.h>
  28 #include <stdbool.h>
  29 #include "config/runtime_config.h"
  30
  31
  32 // machine states
  33 enum protocolDetectionStates {
  34     DETECTION_STATE_IDLE,
  35     DETECTION_STATE_START,
  36         DETECTION_STATE_START_FRXKY,
  37         DETECTION_STATE_START_MAVLINK,
  38         DETECTION_STATE_START_MFD,
  39         DETECTION_STATE_START_PITLAB,
  40         DETECTION_STATE_START_CROSSFIRE0,
  41         DETECTION_STATE_START_CROSSFIRE1,
  42     DETECTION_STATE_DETECTED
  43   };
  44
  45 static uint8_t detectionState = DETECTION_STATE_IDLE;
  46 static uint8_t detectionPacketIdex=0;
  47 static uint16_t protocolDetected = 0;
  48
  49 bool detectionIsEnabled = false;
  50
  51 void enableProtocolDetection(void){
  52         protocolDetected = 0;
  53         detectionIsEnabled = true;
  54 }
  55
  56 void disableProtocolDetection(void){
  57         detectionIsEnabled = false;
  58 }
  59
  60 bool isProtocolDetectionEnabled(void){
  61         return detectionIsEnabled;
  62 }
  63
  64 uint16_t getProtocol(void){
  65         return protocolDetected;
  66 }
  67
  68 void protocolDetectionParser(uint8_t c){
  69         if(!detectionIsEnabled)
  70                 return;
  71
  72         switch(detectionState){
  73                 case DETECTION_STATE_IDLE:
  74                     if (c == 0x08 ) {
  75                         detectionState = DETECTION_STATE_START_CROSSFIRE0;
  76                         detectionPacketIdex = 0;
  77                         break;
  78                     } else if (c =='#' || c == 'X') {
  79                                 detectionState = DETECTION_STATE_START_MFD;
  80                                 detectionPacketIdex = 0;
  81                         } else if (c == 0x7E)
  82                                 detectionState = DETECTION_STATE_START_FRXKY;
  83                         else if (c == 254 && detectionPacketIdex > 10) {
  84                                 protocolDetected = TP_MAVLINK;
  85                                 detectionState = DETECTION_STATE_DETECTED;
  86                         } else if (c == '$'){
  87                                 detectionState = DETECTION_STATE_START_PITLAB;
  88                                 detectionPacketIdex = 0;
  89                                 return;
  90                         }
  91                         detectionPacketIdex ++;
  92                         break;
  93                 case DETECTION_STATE_START_CROSSFIRE0:
  94                     if (c == 0x00){
  95                         detectionState = DETECTION_STATE_START_CROSSFIRE1;
  96                         detectionPacketIdex++;
  97                     }
  98                     break;
  99                 case DETECTION_STATE_START_CROSSFIRE1:
 100             if (c == 0xA7 && detectionPacketIdex == 1) {
 101                 protocolDetected = TP_CROSSFIRE;
 102                 detectionState = DETECTION_STATE_DETECTED;
 103             }
 104             break;
 105                 case DETECTION_STATE_START_MFD:
 106                         if ((c == '#' || c == 'X'))
 107                                 detectionPacketIdex++;
 108                         else if (detectionPacketIdex > 5){
 109                                 protocolDetected = TP_MFD;
 110                                 detectionState = DETECTION_STATE_DETECTED;
 111                         } else
 112                                 detectionState = DETECTION_STATE_IDLE;
 113                         break;
 114                 case DETECTION_STATE_START_FRXKY:
 115                         if (c == 0xFD) {
 116                                 protocolDetected = TP_FRSKY_D;
 117                                 detectionState = DETECTION_STATE_DETECTED;
 118                         } else if (detectionState == DETECTION_STATE_START_FRXKY && c ==0x10){
 119                                 protocolDetected = TP_FRSKY_X;
 120                                 detectionState = DETECTION_STATE_DETECTED;
 121                                 detectionPacketIdex = 0;
 122                         } else if (detectionPacketIdex > 10)
 123                                 detectionState = DETECTION_STATE_IDLE;
 124                         break;
 125                 case DETECTION_STATE_START_MAVLINK:
 126                         if (detectionPacketIdex < 5)
 127                                 detectionPacketIdex++;
 128                         else if (c == 24 || c == 33){
 129                                 protocolDetected = TP_MAVLINK;
 130                                 detectionState = DETECTION_STATE_DETECTED;
 131                         } else
 132                                 detectionState = DETECTION_STATE_IDLE;
 133                         break;
 134                 case DETECTION_STATE_START_PITLAB:
 135                         if(c == 'T' && detectionPacketIdex == 0 ){ //This is not PITLAB, it is LTM
 136                                 protocolDetected = TP_LTM;
 137                                 detectionState = DETECTION_STATE_DETECTED;
 138                                 break;
 139                         } else if(c == '$' && detectionPacketIdex == 9 ){
 140                                 protocolDetected = TP_PITLAB;
 141                                 detectionState = DETECTION_STATE_DETECTED;
 142                                 break;
 143                         } else if(c == '$' && detectionPacketIdex > 9){
 144                                 detectionState = DETECTION_STATE_START;
 145                                 break;
 146                         }
 147                         detectionPacketIdex++;
 148                         break;
 149                 case DETECTION_STATE_START:
 150                         switch(c){
 151                                 case 'T':
 152                                         protocolDetected = TP_LTM;
 153                                         detectionState = DETECTION_STATE_DETECTED;
 154                                         break;
 155                                 case 'G':
 156                                         protocolDetected = TP_GPS_TELEMETRY;
 157                                         detectionState = DETECTION_STATE_DETECTED;
 158                                         break;
 159                                 case '1':
 160                                 case 'R':
 161                                 case 'V':
 162                                         protocolDetected = TP_RVOSD;
 163                                         detectionState = DETECTION_STATE_DETECTED;
 164                                         break;
 165                                 default:
 166                                         detectionState = DETECTION_STATE_IDLE;
 167                                         break;
 168                         }
 169                         break;
 170                 case DETECTION_STATE_DETECTED:
 171                         detectionState = DETECTION_STATE_IDLE;
 172                         detectionPacketIdex = 0;
 173                         disableProtocolDetection();
 174                         break;
 175         }
 176 }