From 91c48b2d0099ed8c48d2c8858639a0cd8cb1f40f Mon Sep 17 00:00:00 2001 From: raul-ortega Date: Sun, 7 May 2017 10:37:35 +0200 Subject: [PATCH 1/4] FEATURE AUTODETECT --- src/main/config/config.h | 1 + src/main/io/serial_cli.c | 1 + src/main/tracker/main.c | 88 +++++++++++++++------------ src/main/tracker/protocol_detection.c | 29 ++++++--- src/main/tracker/protocol_detection.h | 6 +- src/main/tracker/telemetry.c | 17 +----- 6 files changed, 77 insertions(+), 65 deletions(-) diff --git a/src/main/config/config.h b/src/main/config/config.h index 0d80578..ac827d2 100755 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -37,6 +37,7 @@ typedef enum { FEATURE_DEBUG = 1 << 11, FEATURE_EPS = 1 << 12, FEATURE_RSSI_ADC = 1 << 13, + FEATURE_AUTODETECT = 1 << 14, /*FEATURE_RX_PARALLEL_PWM = 1 << 14, FEATURE_RX_PPM = 1 << 15,*/ } features_e; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 18d326d..96a6052 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -200,6 +200,7 @@ static const char * const featureNames[] = { "DEBUG", "EPS", "RSSI_ADC", + "AUTODETECT", NULL }; diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index 8189bd0..ca895a1 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -146,7 +146,7 @@ void updateCalibratePan(); uint16_t calculateDeltaHeading(uint16_t heading1, uint16_t heading2); void setEpsMode(void); int16_t getOffset(int16_t offset_master,int8_t offset_trim); -void updateTelemetryProtocol(uint16_t protocol); +void updateProtocolDetection(void); void protocolInit(void); void trackingInit(void); @@ -306,6 +306,9 @@ void tracker_setup(void) protocolInit(); + if(feature(FEATURE_AUTODETECT)) + enableProtocolDetection(); + trackingInit(); trackerSerial = openSerialPort(masterConfig.serialConfig.portConfigs[0].identifier, FUNCTION_NONE, NULL, baudRates[masterConfig.serialConfig.portConfigs[0].msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); @@ -402,6 +405,8 @@ void tracker_loop(void) updateTelemetryLost(); + updateProtocolDetection(); + updateTargetPosition(); updateHeading(); @@ -777,7 +782,8 @@ void updateTelemetryLost(void){ if(!gotTelemetry && (millis() - lostTelemetry_timer > 3000)){ lostTelemetry = true; - enableProtocolDetection(); + if(feature(FEATURE_AUTODETECT)) + enableProtocolDetection(); } } @@ -1474,7 +1480,13 @@ void updateEPSParams(){ EPS_FREQUENCY = masterConfig.eps_frequency; } -void updateTelemetryProtocol(uint16_t protocol){ +void updateProtocolDetection(void){ + uint16_t protocol; + + if(!feature(FEATURE_AUTODETECT)) + return; + + protocol = getProtocol(); if (protocol == 0) return; @@ -1483,45 +1495,43 @@ void updateTelemetryProtocol(uint16_t protocol){ protocolInit(); trackingInit(); updateDisplayProtocolTitle(protocol); - disableProtocolDetection(); } } void protocolInit(void){ DISABLE_PROTOCOL(0b111111111111); - switch(masterConfig.telemetry_protocol) - { - case TP_SERVOTEST: - ENABLE_PROTOCOL(TP_SERVOTEST); - break; - case TP_MFD: - ENABLE_PROTOCOL(TP_MFD); - featureClear(FEATURE_EPS); - featureClear(FEATURE_GPS); - mfdTestMode = false; - break; - case TP_GPS_TELEMETRY: - ENABLE_PROTOCOL(TP_GPS_TELEMETRY); - break; - case TP_MAVLINK: - ENABLE_PROTOCOL(TP_MAVLINK); - break; - case TP_RVOSD: - ENABLE_PROTOCOL(TP_RVOSD); - break; - case TP_FRSKY_D: - ENABLE_PROTOCOL(TP_FRSKY_D); - telemetry_diy_gps = masterConfig.telemetry_diy_gps; - break; - case TP_FRSKY_X: - ENABLE_PROTOCOL(TP_FRSKY_X); - telemetry_diy_gps = masterConfig.telemetry_diy_gps; - break; - case TP_LTM: - ENABLE_PROTOCOL(TP_LTM); - break; - case TP_LTM_FRSKYD: - ENABLE_PROTOCOL(TP_LTM_FRSKYD); - break; - } + switch(masterConfig.telemetry_protocol) { + case TP_SERVOTEST: + ENABLE_PROTOCOL(TP_SERVOTEST); + break; + case TP_MFD: + ENABLE_PROTOCOL(TP_MFD); + featureClear(FEATURE_EPS); + featureClear(FEATURE_GPS); + mfdTestMode = false; + break; + case TP_GPS_TELEMETRY: + ENABLE_PROTOCOL(TP_GPS_TELEMETRY); + break; + case TP_MAVLINK: + ENABLE_PROTOCOL(TP_MAVLINK); + break; + case TP_RVOSD: + ENABLE_PROTOCOL(TP_RVOSD); + break; + case TP_FRSKY_D: + ENABLE_PROTOCOL(TP_FRSKY_D); + telemetry_diy_gps = masterConfig.telemetry_diy_gps; + break; + case TP_FRSKY_X: + ENABLE_PROTOCOL(TP_FRSKY_X); + telemetry_diy_gps = masterConfig.telemetry_diy_gps; + break; + case TP_LTM: + ENABLE_PROTOCOL(TP_LTM); + break; + case TP_LTM_FRSKYD: + ENABLE_PROTOCOL(TP_LTM_FRSKYD); + break; + } } diff --git a/src/main/tracker/protocol_detection.c b/src/main/tracker/protocol_detection.c index c0d5d97..cc0e702 100644 --- a/src/main/tracker/protocol_detection.c +++ b/src/main/tracker/protocol_detection.c @@ -28,7 +28,7 @@ #include #include "config/runtime_config.h" -uint16_t protocolDetectionParser(uint8_t c); + // machine states enum protocolDetectionStates { DETECTION_STATE_IDLE, @@ -42,13 +42,27 @@ enum protocolDetectionStates { static uint8_t detectionState = DETECTION_STATE_IDLE; static uint8_t detectionPacketIdex=0; static uint16_t protocolDetected = 0; -static uint16_t lastProtocolDetected = 0; -uint16_t protocolDetectionParser(uint8_t c){ +bool detectionIsEnabled = false; + +void enableProtocolDetection(void){ + detectionIsEnabled = true; +} + +void disableProtocolDetection(void){ + detectionIsEnabled = false; +} + +uint16_t getProtocol(void){ + return protocolDetected; +} + +void protocolDetectionParser(uint8_t c){ + if(!detectionIsEnabled) + return; switch(detectionState){ case DETECTION_STATE_IDLE: - protocolDetected = 0; if (c =='#' || c == 'X') { detectionState = DETECTION_STATE_START_MFD; detectionPacketIdex = 0; @@ -63,9 +77,9 @@ uint16_t protocolDetectionParser(uint8_t c){ detectionPacketIdex ++; break; case DETECTION_STATE_START_MFD: - if ((c == '#' || c == 'X') && detectionPacketIdex < 3) + if ((c == '#' || c == 'X')) detectionPacketIdex++; - else if (detectionPacketIdex > 5 && c == 'D'){ + else if (detectionPacketIdex > 5){ protocolDetected = TP_MFD; detectionState = DETECTION_STATE_DETECTED; } else @@ -113,8 +127,7 @@ uint16_t protocolDetectionParser(uint8_t c){ case DETECTION_STATE_DETECTED: detectionState = DETECTION_STATE_IDLE; detectionPacketIdex = 0; + disableProtocolDetection(); break; } - return protocolDetected; - } diff --git a/src/main/tracker/protocol_detection.h b/src/main/tracker/protocol_detection.h index c653915..156f1a4 100644 --- a/src/main/tracker/protocol_detection.h +++ b/src/main/tracker/protocol_detection.h @@ -25,6 +25,6 @@ */ #include -uint16_t protocolDetectionParser(uint8_t c); - - +void enableProtocolDetection(void); +void disableProtocolDetection(void); +void protocolDetectionParser(uint8_t c); diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c index de2ffb7..2556c17 100755 --- a/src/main/tracker/telemetry.c +++ b/src/main/tracker/telemetry.c @@ -23,6 +23,7 @@ #include "config.h" #include "telemetry.h" #include "TinyGPS.h" +#include "protocol_detection.h" int32_t telemetry_lat = 0; int32_t telemetry_lon = 0; @@ -53,8 +54,6 @@ uint16_t chars = 0; uint8_t sentences = 0; -bool detectionIsEnabled = false; - int32_t getTargetLat() { return telemetry_lat; } @@ -71,24 +70,12 @@ uint16_t getSats() { return telemetry_sats; } -void enableProtocolDetection(void){ - detectionIsEnabled = true; -} - -void disableProtocolDetection(void){ - detectionIsEnabled = false; -} - void encodeTargetData(uint8_t c) { uint16_t chars = 0; uint8_t sentences = 0; - uint16_t protocolDetected = 0; - if(detectionIsEnabled) { - protocolDetected = protocolDetectionParser(c); - updateTelemetryProtocol(protocolDetected); - } + protocolDetectionParser(c); if(PROTOCOL(TP_MFD)) mfd_encodeTargetData(c); From c98a1012fc1d4d23028326fc2f3262bb9e9f2204 Mon Sep 17 00:00:00 2001 From: raul-ortega Date: Sun, 7 May 2017 13:05:20 +0200 Subject: [PATCH 2/4] Show autodetecting on OLED display --- src/main/io/display.c | 12 ++++++++++-- src/main/io/display.h | 1 + src/main/tracker/main.c | 18 +++++++++++++----- src/main/tracker/protocol_detection.c | 4 ++++ src/main/tracker/protocol_detection.h | 2 ++ 5 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/main/io/display.c b/src/main/io/display.c index 74610d4..e65c04a 100755 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -381,8 +381,7 @@ void showTitle() { i2c_OLED_set_line(0); //i2c_OLED_send_string(pageTitles[pageState.pageId]); - - if(pageState.pageId==PAGE_TELEMETRY) { + if(pageState.pageId==PAGE_TELEMETRY) { int16_t i; for(i=0;i<10;i++) { if(master_telemetry_protocol & (1< 3000)){ lostTelemetry = true; - if(feature(FEATURE_AUTODETECT)) + if(feature(FEATURE_AUTODETECT)){ + showAutodetectingTitle(0); enableProtocolDetection(); + } } } @@ -1483,19 +1486,23 @@ void updateEPSParams(){ void updateProtocolDetection(void){ uint16_t protocol; - if(!feature(FEATURE_AUTODETECT)) + if(!feature(FEATURE_AUTODETECT) && !cliMode) return; protocol = getProtocol(); - if (protocol == 0) - return; + + if(protocol == masterConfig.telemetry_protocol && isProtocolDetectionEnabled() && !lostTelemetry) + showAutodetectingTitle(protocol); if(protocol != masterConfig.telemetry_protocol) { masterConfig.telemetry_protocol = protocol; protocolInit(); trackingInit(); - updateDisplayProtocolTitle(protocol); + //updateDisplayProtocolTitle(protocol); + showAutodetectingTitle(protocol); } + + } void protocolInit(void){ @@ -1534,4 +1541,5 @@ void protocolInit(void){ ENABLE_PROTOCOL(TP_LTM_FRSKYD); break; } + currentProtocol = masterConfig.telemetry_protocol; } diff --git a/src/main/tracker/protocol_detection.c b/src/main/tracker/protocol_detection.c index cc0e702..02095ed 100644 --- a/src/main/tracker/protocol_detection.c +++ b/src/main/tracker/protocol_detection.c @@ -53,6 +53,10 @@ void disableProtocolDetection(void){ detectionIsEnabled = false; } +bool isProtocolDetectionEnabled(void){ + return detectionIsEnabled; +} + uint16_t getProtocol(void){ return protocolDetected; } diff --git a/src/main/tracker/protocol_detection.h b/src/main/tracker/protocol_detection.h index 156f1a4..5416e97 100644 --- a/src/main/tracker/protocol_detection.h +++ b/src/main/tracker/protocol_detection.h @@ -28,3 +28,5 @@ void enableProtocolDetection(void); void disableProtocolDetection(void); void protocolDetectionParser(uint8_t c); +uint16_t getProtocol(void); +bool isPorotocolDetectionEnabled(void); From 063606d6a1cd5868438abeee15e3d262cae34ed4 Mon Sep 17 00:00:00 2001 From: raul-ortega Date: Sun, 7 May 2017 13:05:38 +0200 Subject: [PATCH 3/4] version bump 9.1.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index 475afd7..73088a1 100755 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 9 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x From 19c79c0b9050b88567276926dc6c250f40840804 Mon Sep 17 00:00:00 2001 From: raul-ortega Date: Sun, 7 May 2017 14:07:21 +0200 Subject: [PATCH 4/4] setting home if MFD --- src/main/tracker/main.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index ad71822..cd4af8a 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -210,7 +210,6 @@ bool gotFix; bool settingHome; bool gotTelemetry=false; bool lostTelemetry=true; -uint16_t currentProtocol; //TRACKER STATE VARS bool homeSet; @@ -1486,20 +1485,24 @@ void updateEPSParams(){ void updateProtocolDetection(void){ uint16_t protocol; - if(!feature(FEATURE_AUTODETECT) && !cliMode) + if(!feature(FEATURE_AUTODETECT) || cliMode) return; protocol = getProtocol(); - if(protocol == masterConfig.telemetry_protocol && isProtocolDetectionEnabled() && !lostTelemetry) + if(protocol == masterConfig.telemetry_protocol && isProtocolDetectionEnabled() && !lostTelemetry){ showAutodetectingTitle(protocol); + if(PROTOCOL(TP_MFD)) + settingHome = true; + } if(protocol != masterConfig.telemetry_protocol) { masterConfig.telemetry_protocol = protocol; protocolInit(); trackingInit(); - //updateDisplayProtocolTitle(protocol); - showAutodetectingTitle(protocol); + if(PROTOCOL(TP_MFD)) + settingHome = true; + updateDisplayProtocolTitle(protocol); } @@ -1541,5 +1544,4 @@ void protocolInit(void){ ENABLE_PROTOCOL(TP_LTM_FRSKYD); break; } - currentProtocol = masterConfig.telemetry_protocol; }