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
  84     if(protocolDetectionTimer == 0)
  85         protocolDetectionTimer = millis();
  86
  87     if(millis() - protocolDetectionTimer >= 3000){
  88       current_protocol ++;
  89       if(current_protocol >= sizeof(protocols) / sizeof(uint16_t)) current_protocol = 0;
  90       protocolDetectionTimer = millis();
  91       gotFix = false;
  92     }
  93
  94     switch(protocols[current_protocol]){
  95         case TP_MFD:
  96             mfd_encodeTargetData(c);
  97             break;
  98         case TP_GPS_TELEMETRY:
  99             gps_encodeTargetData(c);
 100             break;
 101         case TP_MAVLINK:
 102             mavlink_encodeTargetData(c);
 103             break;
 104         case TP_RVOSD:
 105             rvosd_encodeTargetData(c);
 106             break;
 107         case TP_FRSKY_D:
 108             frskyd_encodeTargetData(c);
 109             break;
 110         case TP_FRSKY_X:
 111             frskyx_encodeTargetData(c);
 112             break;
 113         case TP_LTM:
 114             ltm_encodeTargetData(c);
 115             break;
 116         case TP_PITLAB:
 117             pitlab_encodeTargetData(c);
 118             break;
 119         case TP_CROSSFIRE:
 120             crossfire_encodeTargetData(c);
 121             break;
 122     }
 123
 124     if(gotFix){
 125         protocol_fixes ++;
 126         if(protocol_fixes > 1) {
 127             protocolDetected = protocols[current_protocol];
 128             return;
 129         }
 130         gotFix = false;
 131
 132     }
 133 }