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         protocolDetectionTimer = millis();
  55         protocol_fixes = 0;
  56         current_protocol=0;
  57
  58         //set default protocol
  59         for(uint8_t i=0; i < sizeof(protocols) / sizeof(uint8_t); i++){
  60             if(protocols[i] == default_protocol){
  61                 current_protocol = i;
  62                 break;
  63             }
  64         }
  65 }
  66
  67 void disableProtocolDetection(void){
  68         detectionIsEnabled = false;
  69         protocolDetected = 0;
  70 }
  71
  72 bool isProtocolDetectionEnabled(void){
  73         return detectionIsEnabled;
  74 }
  75
  76 uint16_t getProtocol(void){
  77         return protocolDetected;
  78 }
  79
  80 void protocolDetectionParser(uint8_t c)
  81 {
  82     if(!detectionIsEnabled)
  83             return;
  84
  85     if(millis() - protocolDetectionTimer >= 3000){
  86       current_protocol ++;
  87       if(current_protocol >= sizeof(protocols) / sizeof(uint8_t)) current_protocol = 0;
  88       protocolDetectionTimer = millis();
  89       gotFix = false;
  90     }
  91
  92     switch(protocols[current_protocol]){
  93         case TP_MFD:
  94             mfd_encodeTargetData(c);
  95             break;
  96         case TP_GPS_TELEMETRY:
  97             gps_encodeTargetData(c);
  98             break;
  99         case TP_MAVLINK:
 100             mavlink_encodeTargetData(c);
 101             break;
 102         case TP_RVOSD:
 103             rvosd_encodeTargetData(c);
 104             break;
 105         case TP_FRSKY_D:
 106             frskyd_encodeTargetData(c);
 107             break;
 108         case TP_FRSKY_X:
 109             frskyx_encodeTargetData(c);
 110             break;
 111         case TP_LTM:
 112             ltm_encodeTargetData(c);
 113             break;
 114         case TP_PITLAB:
 115             pitlab_encodeTargetData(c);
 116             break;
 117         case TP_CROSSFIRE:
 118             crossfire_encodeTargetData(c);
 119             break;
 120     }
 121
 122     if(gotFix){
 123         protocol_fixes ++;
 124         if(protocol_fixes > 2) {
 125             protocolDetected = protocols[current_protocol];
 126         }
 127         gotFix = false;
 128     }
 129 }