From 450269510a063f8728f29f36e715874a7d605484 Mon Sep 17 00:00:00 2001
From: "Hansi, dl9rdz" <dl9rdz@darc.de>
Date: Tue, 15 Oct 2019 13:53:04 +0200
Subject: [PATCH] Display options for GPS course and bearing relative to GPS
 course

---
 RX_FSK/RX_FSK.ino              |  4 +++-
 RX_FSK/data/screens.txt        |  6 +++++-
 RX_FSK/version.h               |  2 +-
 libraries/SondeLib/Display.cpp | 34 +++++++++++++++++++++++++++++++++-
 4 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/RX_FSK/RX_FSK.ino b/RX_FSK/RX_FSK.ino
index acfffab..3e931f4 100644
--- a/RX_FSK/RX_FSK.ino
+++ b/RX_FSK/RX_FSK.ino
@@ -895,13 +895,15 @@ void gpsTask(void *parameter) {
       char c = Serial2.read();
       //Serial.print(c);
       if (nmea.process(c)) {
+        //Serial.println(nmea.getSentence());
         long lat = nmea.getLatitude();
         long lon = nmea.getLongitude();
         long alt = -1;
         bool b = nmea.getAltitude(alt);
         bool valid = nmea.isValid();
+        int course = nmea.getCourse()/1000;
         uint8_t hdop = nmea.getHDOP();
-        Serial.printf("\nDecode: valid: %d  N %ld  E %ld  alt %ld (%d) dop:%d", valid ? 1 : 0, lat, lon, alt, b, hdop);
+        //Serial.printf("\nDecode: valid: %d  N %ld  E %ld  alt %ld (%d)  course:%d dop:%d", valid ? 1 : 0, lat, lon, alt, b, c, hdop);
       }
     }
     delay(50);
diff --git a/RX_FSK/data/screens.txt b/RX_FSK/data/screens.txt
index 69a6264..4975950 100644
--- a/RX_FSK/data/screens.txt
+++ b/RX_FSK/data/screens.txt
@@ -34,7 +34,9 @@
 # N ip address (only tiny font)
 # S launch site
 # Mx telemetry value x (t temp p preassure h hyg)
-# Gx value relativ to GPS reference point (x: D dist, I direction)  GV. GPS valid symbol;  GL, GO, GA: ref lat long alt
+# Gx  GPS-related data
+#     raw data from GPS:  GL, GO, GA, GC: Latitude, lOngitude, Altutide, Course
+#     relative to sonde:  GD, GI, GB: Distance, dIrection (absolute), relative Bearing
 # R RSSI
 #
 # fonts=x,y can be used to select font (x=small, y=large) for all items below
@@ -174,6 +176,8 @@ timeaction=#,#,#
 2,10=a
 3,10=h
 4,9=v
+5,9=gC
+5,13=gB
 6,7=Q
 7,0=gV
 7,2=xd=
diff --git a/RX_FSK/version.h b/RX_FSK/version.h
index 478e1f8..6e38ee3 100644
--- a/RX_FSK/version.h
+++ b/RX_FSK/version.h
@@ -1,2 +1,2 @@
 const char *version_name = "rdzTTGOsonde";
-const char *version_id = "devel20191014b";
+const char *version_id = "devel20191015";
diff --git a/libraries/SondeLib/Display.cpp b/libraries/SondeLib/Display.cpp
index 416c950..4ad2b7e 100644
--- a/libraries/SondeLib/Display.cpp
+++ b/libraries/SondeLib/Display.cpp
@@ -877,10 +877,15 @@ void Display::drawGPS(DispEntry *de) {
 		{
 		long alt = -1;
 		nmea.getAltitude(alt);
-		snprintf(buf, 16, "%5fm", alt*0.00001);
+		snprintf(buf, 16, "%4.0fm", alt*0.001);
 		drawString(de,buf);
 		}
 		break;
+	case 'C':
+		// GPS Course over ground
+		snprintf(buf, 4, "%3d", (int)(nmea.getCourse()/1000));
+		drawString(de, buf);
+		break;
 	case 'D':
 		{
 		// distance
@@ -934,6 +939,33 @@ void Display::drawGPS(DispEntry *de) {
 			rdis->drawTile(de->x+3, de->y, 1, deg_tile);
 		}
 		break;
+	case 'B':
+		// relative bearing
+		if( (!nmea.isValid()) || ((sonde.si()->validPos&0x03)!=0x03 ) ) {
+			drawString(de, "---");
+			break;
+		}
+		{
+		float lat1 = radians(nmea.getLatitude()*0.000001);
+                float lat2 = radians(sonde.si()->lat);
+		float lon1 = radians(nmea.getLongitude()*0.000001);
+                float lon2 = radians(sonde.si()->lon);
+		float y = sin(lon2-lon1)*cos(lat2);
+		float x = cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(lon2-lon1);
+		float dir = atan2(y, x)/PI*180;
+		if(dir<0) dir+=360;
+		Serial.printf("direction is %.2f\n", dir);
+		float course = nmea.getCourse()*0.001;
+		float bearing = dir - course;
+		if(bearing<0) bearing += 360;
+		if(bearing>=360) bearing -= 360;
+		snprintf(buf, 16, "%3d", (int)bearing);
+		buf[3]=0;
+		drawString(de, buf);
+		if(de->extra[1]==(char)176)
+			rdis->drawTile(de->x+3, de->y, 1, deg_tile);
+		}
+		break;
 	case 'E':
 		// elevation
 		break;