Merge pull request #16 from raul-ortega/development

Development

README.md

@@ -45,6 +45,7 @@ These are the protocols that are supported (and automatically detected):
 - **FRSKY D**
 - **FRSKY X (Smartport)**
 - **LTM (Light Telemetry)**
+- **PITLAB**
 
 **INPUT TELEMETRY AT HIGH FREQUENCY**
 

src/main/config/runtime_config.h

@@ -88,10 +88,11 @@ typedef enum {
 	TP_FRSKY_D			= (1 << 6),
 	TP_FRSKY_X 			= (1 << 7),
 	TP_LTM				= (1 << 8),
-	TP_LTM_FRSKYD		= (1 << 9),
-	TP_PITLAB			= (1 << 10),
+	TP_PITLAB			= (1 << 9),
+	TP_LTM_FRSKYD		= (1 << 10),
 	TP_CALIBRATING_PAN0	= (1 << 11),
-	TP_CALIBRATING_MAXPAN	= (1 << 12)
+	TP_CALIBRATING_MAXPAN	= (1 << 12),
+
 } trackerProtocolFlags_t;
 
 extern uint16_t trackerProtocolFlags;

src/main/io/display.c

@@ -170,17 +170,16 @@ typedef struct pageState_s {
 static pageState_t pageState;
 
 static const char* const telemetry_protocols_Titles[]={
-	"SERVOTEST    ",
+	"SERVOTEST     ",
 	"CALIBRATING_MAG",
-	"MFD          ",
-	"GPS_TELEMETRY",
-	"MAVLINK      ",
-	"RVOSD        ",
-	"FRSKY_D      ",
-	"FRSKY_X      ",
-	"LTM          ",
-	"LTM_FRSKYD   ",
-	"PITLAB       "
+	"MFD           ",
+	"GPS_TELEMETRY ",
+	"MAVLINK       ",
+	"RVOSD         ",
+	"FRSKY_D       ",
+	"FRSKY_X       ",
+	"LTM           ",
+	"PITLAB        "
 };
 
 // Menu
@@ -223,6 +222,7 @@ static const char* const telemetryProtocolMenu[] = {
 	"LTM          ",
 	"PITLAB       ",
 	/*"LTM_FRSKYD   ",*/
+	"AUTODETECT   ",
 	"EXIT         "
 };
 
@@ -385,7 +385,7 @@ void showTitle()
     //i2c_OLED_send_string(pageTitles[pageState.pageId]);
 	if(pageState.pageId==PAGE_TELEMETRY) {
         int16_t i;
-    	for(i=0;i<11;i++) {
+    	for(i=0;i < OP_PITLAB + 3;i++) {
     		if(master_telemetry_protocol & (1<<i)) {
     			i2c_OLED_send_string(telemetry_protocols_Titles[i]);
     			if(feature(FEATURE_EPS) && !PROTOCOL(TP_MFD)){
@@ -398,7 +398,7 @@ void showTitle()
     		}
     	}
     	if(master_telemetry_protocol == 0 && feature(FEATURE_AUTODETECT)){
-			tfp_sprintf(lineBuffer, "Autodetecting");
+			tfp_sprintf(lineBuffer, "Auto detecting");
 			i2c_OLED_send_string(lineBuffer);
 		}
     } else if(pageState.pageId==PAGE_MENU){

src/main/io/display.h

@@ -87,9 +87,11 @@ typedef enum {
 	OP_RVOSD,
 	OP_FRSKY_D,
 	OP_FRSKY_X,
-	OP_LTM,/*
-	OP_LTM_FRSKYD,*/
-	OP_TELEMETRY_PROTOCOL_EXIT,
+	OP_LTM,
+	OP_PITLAB,
+    OP_AUTODETECT,
+	OP_TELEMETRY_PROTOCOL_EXIT
+	/*OP_LTM_FRSKYD*/
 } telemetryProtocolMenu_e;
 
 typedef enum {

src/main/tracker/main.c

@@ -243,6 +243,7 @@ uint8_t displayPageIndex=0;
 extern uint8_t menuState;
 extern uint8_t indexMenuOption;
 uint8_t menuOption;
+bool detection_title_updated = false;
 
 //COMMON VARS
 serialPort_t *trackerSerial;
@@ -1253,8 +1254,12 @@ void processMenuTelemetryProtocol(void){
 	menuOption = indexMenuOption % (OP_TELEMETRY_PROTOCOL_EXIT+1);
 	if(menuOption == OP_TELEMETRY_PROTOCOL_EXIT)
 		menuState = MENU_TELEMETRY;
-	else {
-		masterConfig.telemetry_protocol = (1 << (2+menuOption));
+	else if(menuOption == OP_AUTODETECT){
+		featureSet(FEATURE_AUTODETECT);
+		menuState = MENU_TELEMETRY;
+	} else {
+		featureClear(FEATURE_AUTODETECT);
+		masterConfig.telemetry_protocol = (1 << ( 2 + menuOption));
 		menuState = MENU_TELEMETRY;
 	}
 	indexMenuOption = OP_TELMETRY_SAVE;
@@ -1500,20 +1505,28 @@ void updateProtocolDetection(void){
 
 	protocol = getProtocol();
 
+	if(protocol == 0 && !detection_title_updated ){
+		detection_title_updated = true;
+		updateDisplayProtocolTitle(protocol);
+		return;
+	}
+
+
 	if(protocol == masterConfig.telemetry_protocol && isProtocolDetectionEnabled() && !lostTelemetry){
 		showAutodetectingTitle(protocol);
 		if(PROTOCOL(TP_MFD))
 			settingHome = true;
 		return;
 	}
 
-	if(protocol != masterConfig.telemetry_protocol) {
+	if(protocol != masterConfig.telemetry_protocol && protocol > 0) {
 		masterConfig.telemetry_protocol = protocol;
 		protocolInit();
 		trackingInit();
 		if(PROTOCOL(TP_MFD))
 			settingHome = true;
 		updateDisplayProtocolTitle(protocol);
+		detection_title_updated = false;
 	}
 
 

src/main/tracker/pitlab.c

@@ -1,8 +1,8 @@
 #include "config.h"
 #include "telemetry.h"
 
-int Restore_long(int idx);
-short Restore_short(int idx);
+int32_t Restore_long(int idx);
+int16_t Restore_short(int idx);
 uint8_t Restore_byte(int idx);
 void pitlab_encodeTargetData(uint8_t c);
 void preProcessHexString(void);
@@ -24,6 +24,9 @@ DB - byte1
 15 - byte0
 */
 
+int32_t gps_lat;
+int32_t gps_lon;
+
 
 //uint8_t type is single, unsigned byte (unsigned char)
 uint8_t lsRxData[5]; //bufor na kolejne bajty odczytane z komunikatu (dan Hex zamienione na bajty)
@@ -39,14 +42,14 @@ enum PitlabDataState {
 static uint8_t dataState = IDLE;
 uint8_t dataIdx=0;
 
-int Restore_long(int idx)
+int32_t Restore_long(int idx)
 {
-  return lsRxData[idx] + (lsRxData[idx+1] << 8) + (lsRxData[idx+2] << 16) + (lsRxData[idx+3] << 24);
+  return (int32_t)lsRxData[idx] + ((int32_t)lsRxData[idx+1] << 8) + ((int32_t)lsRxData[idx+2] << 16) + ((int32_t)lsRxData[idx+3] << 24);
 }
 
-short Restore_short(int idx)
+int16_t Restore_short(int idx)
 {
-  return lsRxData[idx] + (lsRxData[idx+1] << 8);
+  return (int16_t)lsRxData[idx] + ((int16_t)lsRxData[idx+1]  << 8);
 }
 
 uint8_t Restore_byte(int idx)
@@ -84,7 +87,7 @@ void preProcessHexString(void){
 		for(uint8_t j = 0; j < 2; ++j){
 			str_buffer[j] = hexString[sIdx++];
 		}
-		lsRxData[i] = hex2int(str_buffer,2);
+		lsRxData[5-i] = hex2int(str_buffer,2);
 	}
 }
 
@@ -99,10 +102,12 @@ void processPitlabFrame(void){
 		gotAlt = true;
 		break;
 	case 2:
-		telemetry_lon = (int32_t)Restore_long(1)/100;
+		gps_lon = Restore_long(1);
+		telemetry_lon = (int32_t)(round(((double)gps_lon * 100.0)/60.0));
 		break;
 	case 3:
-		telemetry_lat = (int32_t)Restore_long(1)/100;
+		gps_lat = Restore_long(1);
+		telemetry_lat = (int32_t)(round(((double)gps_lat * 100.0)/60.0));
 		if(telemetry_sats >= 5) gotFix = true;
 		break;
 	}

src/main/tracker/protocol_detection.h

@@ -29,4 +29,4 @@ void enableProtocolDetection(void);
 void disableProtocolDetection(void);
 void protocolDetectionParser(uint8_t c);
 uint16_t getProtocol(void);
-bool isPorotocolDetectionEnabled(void);
+bool isProtocolDetectionEnabled(void);