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 <stdint.h>
  27 #include <stdbool.h>
  28 #include <stdio.h>
  29 #include "config/runtime_config.h"
  30
  31 static uint16_t protocolDetected = 0;
  32 uint8_t current_protocol = 0;
  33 uint32_t protocolDetectionTimer = 0;
  34 bool detectionIsEnabled = false;
  35 uint8_t protocol_fixes = 0;
  36
  37 extern bool gotFix;
  38
  39 static const uint16_t protocols[] = {
  40     TP_MAVLINK,
  41     TP_FRSKY_X,
  42     TP_CROSSFIRE,
  43     TP_MFD,
  44     TP_LTM,
  45     TP_GPS_TELEMETRY,
  46     TP_FRSKY_D,
  47     TP_PITLAB,
  48     TP_RVOSD
  49 };
  50
  51 void enableProtocolDetection(uint16_t default_protocol) {
  52         protocolDetected = 0;
  53         detectionIsEnabled = true;
  54         protocol_fixes = 0;
  55         current_protocol=0;
  56
  57         //set default protocol
  58         for(uint8_t i=0; i < sizeof(protocols) / sizeof(uint8_t); i++){
  59             if(protocols[i] == default_protocol){
  60                 current_protocol = i;
  61                 break;
  62             }
  63         }
  64
  65         protocolDetectionTimer = 0;
  66 }
  67
  68 void disableProtocolDetection(void){
  69         detectionIsEnabled = false;
  70         protocolDetected = 0;
  71 }
  72
  73 bool isProtocolDetectionEnabled(void){
  74         return detectionIsEnabled;
  75 }
  76
  77 uint16_t getProtocol(void){
  78         return protocolDetected;
  79 }
  80
  81 void protocolDetectionParser(uint8_t c)
  82 {
  83     if(!detectionIsEnabled)
  84             return;
  85     if(protocolDetectionTimer == 0)
  86         protocolDetectionTimer = millis();
  87
  88     if(millis() - protocolDetectionTimer >= 3000){
  89       current_protocol ++;
  90       if(current_protocol >= sizeof(protocols) / sizeof(uint16_t)) current_protocol = 0;
  91       protocolDetectionTimer = millis();
  92       gotFix = false;
  93     }
  94
  95     switch(protocols[current_protocol]){
  96         case TP_MFD:
  97             mfd_encodeTargetData(c);
  98             break;
  99         case TP_GPS_TELEMETRY:
 100             gps_encodeTargetData(c);
 101             break;
 102         case TP_MAVLINK:
 103             mavlink_encodeTargetData(c);
 104             break;
 105         case TP_RVOSD:
 106             rvosd_encodeTargetData(c);
 107             break;
 108         case TP_FRSKY_D:
 109             frskyd_encodeTargetData(c);
 110             break;
 111         case TP_FRSKY_X:
 112             frskyx_encodeTargetData(c);
 113             break;
 114         case TP_LTM:
 115             ltm_encodeTargetData(c);
 116             break;
 117         case TP_PITLAB:
 118             pitlab_encodeTargetData(c);
 119             break;
 120         case TP_CROSSFIRE:
 121             crossfire_encodeTargetData(c);
 122             break;
 123     }
 124
 125     if(gotFix){
 126         protocol_fixes ++;
 127         if(protocol_fixes > 1) {
 128             protocolDetected = protocols[current_protocol];
 129             return;
 130         }
 131         gotFix = false;
 132
 133     }
 134 }