Merge pull request #10 from raul-ortega/Automatic-protocol-detection

Automatic protocol detection
raul-ortega · May 7, 2017 · f08786a · f08786a
2 parents 4185155 + 19c79c0
commit f08786a
Show file tree

Hide file tree

Showing 9 changed files with 106 additions and 69 deletions.
diff --git 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/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<<i)) {
@@ -396,6 +395,10 @@ void showTitle()
     			break;
     		}
     	}
+    	if(master_telemetry_protocol == 0 && feature(FEATURE_AUTODETECT)){
+			tfp_sprintf(lineBuffer, "Autodetecting");
+			i2c_OLED_send_string(lineBuffer);
+		}
     } else if(pageState.pageId==PAGE_MENU){
     	return;
     } else if(pageState.pageId==PAGE_BATTERY) {
@@ -952,6 +955,11 @@ void showDebugPage(void)
 }
 #endif
 
+void showAutodetectingTitle(uint16_t protocol){
+	master_telemetry_protocol = protocol;
+	showTitle();
+}
+
 void updateDisplayProtocolTitle(uint16_t protocol){
 	master_telemetry_protocol = protocol;
 	telemetry_failed_cs = 0;

diff --git a/src/main/io/display.h b/src/main/io/display.h
@@ -138,3 +138,4 @@ void displayDisablePageCycling(void);
 void displayResetPageCycling(void);
 void displaySetNextPageChangeAt(uint32_t futureMicros);
 void updateDisplayProtocolTitle(uint16_t protocol);
+void showAutodetectingTitle(uint16_t protocol);
diff --git 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
@@ -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);
 void telemetryPortInit(void);
@@ -306,6 +306,9 @@ void tracker_setup(void)
 
   protocolInit();
 
+  if(feature(FEATURE_AUTODETECT))
+	  enableProtocolDetection();
+
   trackingInit();
 
   telemetryPortInit();
@@ -407,6 +410,8 @@ void tracker_loop(void)
 
 		updateTelemetryLost();
 
+		updateProtocolDetection();
+
 		updateTargetPosition();
 
 		updateHeading();
@@ -782,7 +787,10 @@ void updateTelemetryLost(void){
 
 	if(!gotTelemetry && (millis() - lostTelemetry_timer > 3000)){
 		lostTelemetry = true;
-		enableProtocolDetection();
+		if(feature(FEATURE_AUTODETECT)){
+			showAutodetectingTitle(0);
+			enableProtocolDetection();
+		}
 	}
 }
 
@@ -1479,54 +1487,66 @@ void updateEPSParams(){
 	EPS_FREQUENCY = masterConfig.eps_frequency;
 }
 
-void updateTelemetryProtocol(uint16_t protocol){
-	if (protocol == 0)
+void updateProtocolDetection(void){
+	uint16_t protocol;
+
+	if(!feature(FEATURE_AUTODETECT) || cliMode)
 		return;
 
+	protocol = getProtocol();
+
+	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();
+		if(PROTOCOL(TP_MFD))
+			settingHome = true;
 		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
@@ -28,7 +28,7 @@
 #include <stdbool.h>
 #include "config/runtime_config.h"
 
-uint16_t protocolDetectionParser(uint8_t c);
+
 // machine states
 enum protocolDetectionStates {
     DETECTION_STATE_IDLE,
@@ -42,13 +42,31 @@ 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;
+}
+
+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:
-			protocolDetected = 0;
 			if (c =='#' || c == 'X') {
 				detectionState = DETECTION_STATE_START_MFD;
 				detectionPacketIdex = 0;
@@ -63,9 +81,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 +131,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
@@ -25,6 +25,8 @@
 */
 #include <stdint.h>
 
-uint16_t protocolDetectionParser(uint8_t c);
-
-
+void enableProtocolDetection(void);
+void disableProtocolDetection(void);
+void protocolDetectionParser(uint8_t c);
+uint16_t getProtocol(void);
+bool isPorotocolDetectionEnabled(void);
diff --git 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);

diff --git 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