Add latitude, longitude, course and heading

1 files changed, 64 insertions, 19 deletions

diff --git a/tracker-stm32/src/main.ino b/tracker-stm32/src/main.ino
index ed1d570..6a98cf9 100644
--- a/tracker-stm32/src/main.ino
+++ b/tracker-stm32/src/main.ino
@@ -1,13 +1,18 @@
+// Licence: MIT
 #include <Arduino.h>
 #include <RadioLib.h>
 #include <Wire.h>
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 #include <SD.h>
 
-constexpr char *CALLSIGN = "HB9EGM";
+// TODO: read these settings from the SD Card
+constexpr char CALLSIGN[] = "HB9EGM";
 constexpr int SSID = 12;
+constexpr int DATE_TYPE_CODE = 0; // compressed geolocation
+constexpr int PATH_CODE = 2; // metropolitan mobile
 constexpr char SYMBOL_TABLE_IDENTIFIER = '/';
 constexpr char SYMBOL_CODE_BICYCLE = 'b';
+constexpr long TX_INTERVAL = 15000;
 
 #if 0
 File myFile;
@@ -32,6 +37,8 @@ uint8_t report[MAX_REPORT_LEN];
 const char* digits = " 0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ-./?@";
 
 #define callsign_int_t uint32_t
+// This function is from aprs434.github.io,
+// MIT License, Copyright (c) 2022 Serge Y. Stroobandt, ON4AA
 callsign_int_t encodeCallsign(const char *callsign) {
     // Encode CCCC according to aprs434.github.io/code/codec.cpp
     const int b = 37;
@@ -57,6 +64,19 @@ callsign_int_t encodeCallsign(const char *callsign) {
     return result;                                // result can be pow(42,51)!
 }
 
+// This function is from ESP32 lora.tracker,
+// MIT License, Copyright (c) 2020 Peter Buchegger
+static void ax25_base91enc(char *s, uint8_t n, uint32_t v) {
+    /* Creates a Base-91 representation of the value in v in the string
+     * pointed to by s, n-characters long. String length should be n+1.
+     */
+
+    for (s += n, *s = '\0'; n; n--) {
+        *(--s) = v % 91 + 33;
+        v /= 91;
+    }
+}
+
 
 static char letterize(int x) {
     return (char) x + 65;
@@ -169,11 +189,12 @@ void loop()
     if (now - lastGnssPoll > 1000) {
         lastGnssPoll = now;
 
-        long latitude = gnss.getLatitude();
+        // longitude and latitude are in degrees*1e7
+        const long latitude = gnss.getLatitude();
         Serial.print(F("Lat: "));
         Serial.print(latitude);
 
-        long longitude = gnss.getLongitude();
+        const long longitude = gnss.getLongitude();
         Serial.print(F(" Long: "));
         Serial.print(longitude);
 
@@ -181,29 +202,28 @@ void loop()
         Serial.print(F(" Loc: "));
         Serial.print(locator);
 
-        long altitude = gnss.getAltitude();
+        const long altitude = gnss.getAltitude();
         Serial.print(F(" Alt: "));
         Serial.print(altitude);
         Serial.print(F(" (mm)"));
 
-        long speed = gnss.getGroundSpeed();
+        const long speed = gnss.getGroundSpeed();
         Serial.print(F(" Speed: "));
         Serial.print(speed);
         Serial.print(F(" (mm/s)"));
+        // 1m/s = 900/463 knots
+        const double speed_kn = speed / 1000.0 * 900.0 / 463.0;
 
-        /*
-           long heading = gnss.getHeading();
-           Serial.print(F(" Heading: "));
-           Serial.print(heading);
-           Serial.print(F(" (degrees * 10^-5)"));
-         */
+        const long heading = gnss.getHeading();
+        Serial.print(F(" Heading: "));
+        Serial.print(heading);
+        Serial.print(F(" (degrees * 10^-5)"));
 
-        byte SIV = gnss.getSIV();
+        const byte SIV = gnss.getSIV();
         Serial.print(F(" SIV: "));
         Serial.print(SIV);
 
         Serial.print(F(" TX in: "));
-        constexpr long TX_INTERVAL = 15000;
         Serial.print(TX_INTERVAL - (now - lastPositionReport));
 
         Serial.println();
@@ -224,27 +244,52 @@ void loop()
             report[report_len++] = callsign_EEEE & 0xFF;
 
             // D SSID Path Code and Data Type Code
-
-            const int date_type_code = 0; // compressed geolocation
-            const int path_code = 2; // metropolitan mobile
-            report[report_len++] = SSID * 16 + path_code * 4 + date_type_code;
+            report[report_len++] = SSID * 16 + PATH_CODE * 4 + DATE_TYPE_CODE;
 
             // / Symbol Table Identifier
             report[report_len++] = SYMBOL_TABLE_IDENTIFIER;
-#warn "TODO longitude and latitude"
+
             // XXXX Base91 Longitude
             // YYYY Base91 Latitude
+            uint32_t aprs_lat = 900000000 - latitude;
+            aprs_lat = aprs_lat / 26 - aprs_lat / 2710 + aprs_lat / 15384615;
+            uint32_t aprs_lon = 900000000 + longitude / 2;
+            aprs_lon = aprs_lon / 26 - aprs_lon / 2710 + aprs_lon / 15384615;
+
+            char tmp_base91[5];
+            ax25_base91enc(tmp_base91, 4, aprs_lat);
+            for (int i=0; i<4; i++) {
+                report[report_len++] = tmp_base91[i];
+            }
+            ax25_base91enc(tmp_base91, 4, aprs_lon);
+            for (int i=0; i<4; i++) {
+                report[report_len++] = tmp_base91[i];
+            }
+
             // $ Symbol Code
             report[report_len++] = SYMBOL_CODE_BICYCLE;
+
             // cs Course and Speed
+            ax25_base91enc(tmp_base91, 1, heading * 10000 / 4);
+            report[report_len++] = tmp_base91[0];
+
+            ax25_base91enc(tmp_base91, 1, (uint32_t)(log1p(speed_kn) / 0.07696));
+            report[report_len++] = tmp_base91[0];
 
             Serial.print(F("TX: "));
             // This is the old custom report I used before adopting aprs434.github.io,
             // it is just callsign SPACE locator sent as ASCII
             Serial.print(CALLSIGN);
-            Serial.print(" ");
+            Serial.print(F(" "));
             Serial.println(locator);
 
+            // Debug print
+            Serial.println(F("Bytes: "));
+            for (int i = 0; i < report_len; i++) {
+                Serial.print((uint32_t)report[i]);
+            }
+            Serial.println(F(""));
+
             int state = radio.transmit(report, report_len);
 
             if (state == RADIOLIB_ERR_NONE) {