Merge pull request #21 from raul-ortega/development

GPS and Telemetry altitudes treated as relative
raul-ortega · Apr 13, 2018 · bbb1904 · bbb1904
2 parents a907819 + e5e6337
commit bbb1904
Show file tree

Hide file tree

Showing 8 changed files with 32 additions and 10 deletions.
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
@@ -156,3 +156,5 @@ void suspendRxSignal(void);
 void resumeRxSignal(void);
 
 void initRxRefreshRate(uint16_t *rxRefreshRatePtr);
+
+uint8_t calculateRssiPercentage(void);
diff --git a/src/main/tracker/TinyGPS.c b/src/main/tracker/TinyGPS.c
@@ -28,7 +28,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 #define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
 
 
-bool TinyGPS_encode(char c);
+
 int from_hex(char a);
 unsigned long parse_decimal();
 unsigned long parse_degrees();

diff --git a/src/main/tracker/TinyGPS.h b/src/main/tracker/TinyGPS.h
@@ -125,7 +125,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
   enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GNGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER};
 
-
+  bool TinyGPS_encode(char c);
 
   // internal utilities
   /*int from_hex(char a);

diff --git a/src/main/tracker/defines.h b/src/main/tracker/defines.h
@@ -40,6 +40,7 @@ typedef struct {
   int16_t alt;
   uint16_t heading;
   uint32_t distance;
+  int16_t home_alt;
 }
 positionVector_t;
 

diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c
@@ -380,6 +380,8 @@ void trackingInit(void){
 	gotNewHeading = false;
 
 	menuState = 0;
+
+	targetPosition.home_alt = -32768;
 }
 
 void tracker_loop(void)
@@ -680,6 +682,9 @@ void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) {
 	  tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474;
   }
 
+  tracker->alt = 0;
+  tracker->home_alt = target->alt;
+
   homeSet = true;
   homeSet_BY_GPS = false;
   homeReset = false;
@@ -692,9 +697,21 @@ void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int1
   tracker->lat = lat;
   tracker->lon = lon;
   tracker->alt = alt;
+
   if(feature(FEATURE_DEBUG)) {
 	  tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474;
   }
+
+  if(home_updated) {
+	  tracker->alt = alt - tracker->home_alt;
+  } else {
+	  tracker->alt = 0;
+	  tracker->home_alt = alt;
+  }
+
+  if(targetPosition.home_alt == -32768)
+	  targetPosition.alt = 0;
+
   homeSet = true;
   homeSet_BY_GPS = true;
   homeReset = false;
@@ -801,7 +818,10 @@ void updateTargetPosition(void){
 	if(!PROTOCOL(TP_SERVOTEST)){
 		if (gotAlt) {
 
-			targetPosition.alt = getTargetAlt();
+			if(telemetry_sats >= masterConfig.telemetry_min_sats && targetPosition.home_alt == -32768)
+				targetPosition.home_alt = getTargetAlt(0);
+
+			targetPosition.alt = getTargetAlt(targetPosition.home_alt);
 
 			if(PROTOCOL(TP_MFD)){
 				distance = getDistance();
@@ -1058,7 +1078,7 @@ void updateMFD(void){
 
 		if (mfdTestMode || (homeSet && gotFix)) {
 			targetPosition.distance = getDistance();
-			targetPosition.alt = getTargetAlt();
+			targetPosition.alt = getTargetAlt(targetPosition.home_alt);
 			targetPosition.heading = getAzimuth() * 10;
 			gotFix = false;
 		}
@@ -1572,4 +1592,3 @@ void protocolInit(void){
 		break;
 	}
 }
-
diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c
@@ -62,8 +62,8 @@ int32_t getTargetLon() {
   return telemetry_lon;
 }
 
-int16_t getTargetAlt() {
-  return telemetry_alt;
+int16_t getTargetAlt(int16_t home_alt) {
+  return telemetry_alt - home_alt;
 }
 
 uint16_t getSats() {

diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h
@@ -48,7 +48,7 @@ extern float telemetry_yaw;
 extern uint8_t telemetry_failed_cs;
 extern uint8_t telemetry_provider;
 
-int16_t getTargetAlt();
+int16_t getTargetAlt(int16_t home_alt);
 void encodeTargetData(uint8_t c);
 int32_t getTargetLat();
 int32_t getTargetLon();

diff --git a/src/main/version.h b/src/main/version.h
@@ -16,8 +16,8 @@
  */
 
 #define FC_VERSION_MAJOR            9 // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR            2 // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      4 // increment when a bug is fixed
+#define FC_VERSION_MINOR            3 // 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
 #define STR(x) STR_HELPER(x)
Original file line number	Diff line number	Diff line change
Expand Up		@@ -156,3 +156,5 @@ void suspendRxSignal(void);
		void resumeRxSignal(void);

		void initRxRefreshRate(uint16_t *rxRefreshRatePtr);

		uint8_t calculateRssiPercentage(void);