From f01c5849c1cd89b519bb87f20a15012ebde3fe6d Mon Sep 17 00:00:00 2001 From: =?utf8?q?Ra=C3=BAl=20Ortega?= Date: Tue, 27 Apr 2021 19:18:34 +0200 Subject: [PATCH] Protocol detection reworked --- src/main/tracker/main.c | 32 ++-- src/main/tracker/protocol_detection.c | 305 ++++++++++++++-------------------- src/main/tracker/protocol_detection.h | 8 +- src/main/tracker/telemetry.h | 1 - 4 files changed, 152 insertions(+), 194 deletions(-) rewrite src/main/tracker/protocol_detection.c (73%) diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index 673bfa5..717537a 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -318,7 +318,7 @@ void tracker_setup(void) protocolInit(masterConfig.telemetry_protocol); if(feature(FEATURE_AUTODETECT)) - enableProtocolDetection(); + enableProtocolDetection(masterConfig.telemetry_protocol); trackingInit(); @@ -829,7 +829,8 @@ void updateTelemetryLost(void){ lostTelemetry = true; if(feature(FEATURE_AUTODETECT)){ showAutodetectingTitle(0); - enableProtocolDetection(); + enableProtocolDetection(masterConfig.telemetry_protocol); + detection_title_updated = false; } } } @@ -1556,28 +1557,27 @@ void updateEPSParams(){ void updateProtocolDetection(void){ uint16_t detected_protocol; - if(!feature(FEATURE_AUTODETECT) || cliMode) + if(!feature(FEATURE_AUTODETECT) || cliMode || !isProtocolDetectionEnabled()) return; detected_protocol = getProtocol(); - if(detected_protocol == 0 && !detection_title_updated ){ - detection_title_updated = true; - updateDisplayProtocolTitle(detected_protocol); - return; - } - - - if(detected_protocol > 0 && isProtocolDetectionEnabled()){ - updateDisplayProtocolTitle(detected_protocol); - detection_title_updated = false; - protocolInit(detected_protocol); - if(!homeSet){ + if(detected_protocol > 0 ) { + protocolInit(detected_protocol); + if(!homeSet){ trackingInit(); if(PROTOCOL(TP_MFD)) settingHome = true; } - } + disableProtocolDetection(); + detection_title_updated = false; + } + + if(detection_title_updated == false){ + updateDisplayProtocolTitle(detected_protocol); + detection_title_updated = true; + } + } void protocolInit(uint16_t protocol){ diff --git a/src/main/tracker/protocol_detection.c b/src/main/tracker/protocol_detection.c dissimilarity index 73% index e9689f5..76a881a 100644 --- a/src/main/tracker/protocol_detection.c +++ b/src/main/tracker/protocol_detection.c @@ -1,176 +1,129 @@ -/* - * This file is part of u360gts, aka amv-open360tracker 32bits: - * https://github.com/raul-ortega/amv-open360tracker-32bits - * - * The code below is an adaptation by Ra�l Ortega of the original code of Ghettostation antenna tracker - * https://github.com/KipK/Ghettostation - * - * u360gts is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * u360gts is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with u360gts. If not, see . - * - */ - -/* - -*/ -#include "config.h" -#include -#include -#include "config/runtime_config.h" - - -// machine states -enum protocolDetectionStates { - DETECTION_STATE_IDLE, - DETECTION_STATE_START, - DETECTION_STATE_START_FRXKY, - DETECTION_STATE_START_MAVLINK, - DETECTION_STATE_START_MFD, - DETECTION_STATE_START_PITLAB, - DETECTION_STATE_START_CROSSFIRE0, - DETECTION_STATE_START_CROSSFIRE1, - DETECTION_STATE_DETECTED - }; - -static uint8_t detectionState = DETECTION_STATE_IDLE; -static uint8_t detectionPacketIdex=0; -static uint16_t protocolDetected = 0; - -bool detectionIsEnabled = false; - -void enableProtocolDetection(void){ - protocolDetected = 0; - detectionIsEnabled = true; -} - -void disableProtocolDetection(void){ - detectionIsEnabled = false; -} - -bool isProtocolDetectionEnabled(void){ - return detectionIsEnabled; -} - -uint16_t getProtocol(void){ - return protocolDetected; -} - -void protocolDetectionParser(uint8_t c){ - if(!detectionIsEnabled) - return; - - switch(detectionState){ - case DETECTION_STATE_IDLE: - if (c == 0x08 ) { - detectionState = DETECTION_STATE_START_CROSSFIRE0; - detectionPacketIdex = 0; - break; - } else if (c =='#' || c == 'X') { - detectionState = DETECTION_STATE_START_MFD; - detectionPacketIdex = 0; - } else if (c == 0x7E) - detectionState = DETECTION_STATE_START_FRXKY; - else if (c == 254 && detectionPacketIdex > 10) { - protocolDetected = TP_MAVLINK; - detectionState = DETECTION_STATE_DETECTED; - } else if (c == '$'){ - detectionState = DETECTION_STATE_START_PITLAB; - detectionPacketIdex = 0; - return; - } - detectionPacketIdex ++; - break; - case DETECTION_STATE_START_CROSSFIRE0: - if (c == 0x00){ - detectionState = DETECTION_STATE_START_CROSSFIRE1; - detectionPacketIdex++; - } - break; - case DETECTION_STATE_START_CROSSFIRE1: - if (c == 0xA7 && detectionPacketIdex == 1) { - protocolDetected = TP_CROSSFIRE; - detectionState = DETECTION_STATE_DETECTED; - } - break; - case DETECTION_STATE_START_MFD: - if ((c == '#' || c == 'X')) - detectionPacketIdex++; - else if (detectionPacketIdex > 5){ - protocolDetected = TP_MFD; - detectionState = DETECTION_STATE_DETECTED; - } else - detectionState = DETECTION_STATE_IDLE; - break; - case DETECTION_STATE_START_FRXKY: - if (c == 0xFD) { - protocolDetected = TP_FRSKY_D; - detectionState = DETECTION_STATE_DETECTED; - } else if (detectionState == DETECTION_STATE_START_FRXKY && c ==0x10){ - protocolDetected = TP_FRSKY_X; - detectionState = DETECTION_STATE_DETECTED; - detectionPacketIdex = 0; - } else if (detectionPacketIdex > 10) - detectionState = DETECTION_STATE_IDLE; - break; - case DETECTION_STATE_START_MAVLINK: - if (detectionPacketIdex < 5) - detectionPacketIdex++; - else if (c == 24 || c == 33){ - protocolDetected = TP_MAVLINK; - detectionState = DETECTION_STATE_DETECTED; - } else - detectionState = DETECTION_STATE_IDLE; - break; - case DETECTION_STATE_START_PITLAB: - if(c == 'T' && detectionPacketIdex == 0 ){ //This is not PITLAB, it is LTM - protocolDetected = TP_LTM; - detectionState = DETECTION_STATE_DETECTED; - break; - } else if(c == '$' && detectionPacketIdex == 9 ){ - protocolDetected = TP_PITLAB; - detectionState = DETECTION_STATE_DETECTED; - break; - } else if(c == '$' && detectionPacketIdex > 9){ - detectionState = DETECTION_STATE_START; - break; - } - detectionPacketIdex++; - break; - case DETECTION_STATE_START: - switch(c){ - case 'T': - protocolDetected = TP_LTM; - detectionState = DETECTION_STATE_DETECTED; - break; - case 'G': - protocolDetected = TP_GPS_TELEMETRY; - detectionState = DETECTION_STATE_DETECTED; - break; - case '1': - case 'R': - case 'V': - protocolDetected = TP_RVOSD; - detectionState = DETECTION_STATE_DETECTED; - break; - default: - detectionState = DETECTION_STATE_IDLE; - break; - } - break; - case DETECTION_STATE_DETECTED: - detectionState = DETECTION_STATE_IDLE; - detectionPacketIdex = 0; - disableProtocolDetection(); - break; - } -} +/* + * This file is part of u360gts, aka amv-open360tracker 32bits: + * https://github.com/raul-ortega/amv-open360tracker-32bits + * + * The code below is an adaptation by Ra�l Ortega of the original code of Ghettostation antenna tracker + * https://github.com/KipK/Ghettostation + * + * u360gts is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * u360gts is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with u360gts. If not, see . + * + */ + +/* + +*/ +#include +#include +#include +#include "config/runtime_config.h" + +static uint16_t protocolDetected = 0; +uint8_t current_protocol = 0; +uint32_t protocolDetectionTimer = 0; +bool detectionIsEnabled = false; +uint8_t protocol_fixes = 0; + +extern bool gotFix; + +static const uint16_t protocols[] = { + TP_MAVLINK, + TP_FRSKY_X, + TP_CROSSFIRE, + TP_MFD, + TP_LTM, + TP_GPS_TELEMETRY, + TP_FRSKY_D, + TP_PITLAB, + TP_RVOSD +}; + +void enableProtocolDetection(uint16_t default_protocol) { + protocolDetected = 0; + detectionIsEnabled = true; + protocolDetectionTimer = millis(); + protocol_fixes = 0; + current_protocol=0; + + //set default protocol + for(uint8_t i=0; i < sizeof(protocols) / sizeof(uint8_t); i++){ + if(protocols[i] == default_protocol){ + current_protocol = i; + break; + } + } +} + +void disableProtocolDetection(void){ + detectionIsEnabled = false; + protocolDetected = 0; +} + +bool isProtocolDetectionEnabled(void){ + return detectionIsEnabled; +} + +uint16_t getProtocol(void){ + return protocolDetected; +} + +void protocolDetectionParser(uint8_t c) +{ + if(!detectionIsEnabled) + return; + + if(millis() - protocolDetectionTimer >= 3000){ + current_protocol ++; + if(current_protocol >= sizeof(protocols) / sizeof(uint8_t)) current_protocol = 0; + protocolDetectionTimer = millis(); + gotFix = false; + } + + switch(protocols[current_protocol]){ + case TP_MFD: + mfd_encodeTargetData(c); + break; + case TP_GPS_TELEMETRY: + gps_encodeTargetData(c); + break; + case TP_MAVLINK: + mavlink_encodeTargetData(c); + break; + case TP_RVOSD: + rvosd_encodeTargetData(c); + break; + case TP_FRSKY_D: + frskyd_encodeTargetData(c); + break; + case TP_FRSKY_X: + frskyx_encodeTargetData(c); + break; + case TP_LTM: + ltm_encodeTargetData(c); + break; + case TP_PITLAB: + pitlab_encodeTargetData(c); + break; + case TP_CROSSFIRE: + crossfire_encodeTargetData(c); + break; + } + + if(gotFix){ + protocol_fixes ++; + if(protocol_fixes > 2) { + protocolDetected = protocols[current_protocol]; + } + gotFix = false; + } +} diff --git a/src/main/tracker/protocol_detection.h b/src/main/tracker/protocol_detection.h index 99d3ce2..22bde45 100644 --- a/src/main/tracker/protocol_detection.h +++ b/src/main/tracker/protocol_detection.h @@ -24,9 +24,15 @@ */ #include +#include + +#ifndef PROTOCOL_DETECTION_H_ +#define PROTOCOL_DETECTION_H_ -void enableProtocolDetection(void); void disableProtocolDetection(void); void protocolDetectionParser(uint8_t c); uint16_t getProtocol(void); bool isProtocolDetectionEnabled(void); +void enableProtocolDetection(uint16_t default_protocol); + +#endif /* PROTOCOL_DETECTION_H_ */ diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h index cc709a0..53c3c78 100755 --- a/src/main/tracker/telemetry.h +++ b/src/main/tracker/telemetry.h @@ -68,7 +68,6 @@ int32_t getTargetLon(); uint16_t getSats(); uint16_t getDistance(); uint16_t getAzimuth(); -void enableProtocolDetection(void); void disableProtocolDetection(void); void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt); -- 2.11.4.GIT