From 5b89e28322218bd70eae9b815421f80e37199a4a Mon Sep 17 00:00:00 2001
From: Linar Yusupov <linar.r.yusupov@gmail.com>
Date: Thu, 14 Oct 2021 09:48:59 +0300
Subject: [PATCH] GDL90: report WGS-84 ellipsoid altitude again as the
 'Ownership Geometric Altitude' [skip ci]

---
 software/firmware/source/SoftRF/src/platform/CC13XX.h  |  1 +
 software/firmware/source/SoftRF/src/platform/ESP32.h   |  1 +
 software/firmware/source/SoftRF/src/platform/STM32.h   |  1 +
 software/firmware/source/SoftRF/src/platform/nRF52.cpp |  2 +-
 software/firmware/source/SoftRF/src/platform/nRF52.h   |  1 +
 .../firmware/source/SoftRF/src/protocol/data/GDL90.cpp | 10 +++++++---
 6 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/software/firmware/source/SoftRF/src/platform/CC13XX.h b/software/firmware/source/SoftRF/src/platform/CC13XX.h
index 1584ca872..8f1725283 100644
--- a/software/firmware/source/SoftRF/src/platform/CC13XX.h
+++ b/software/firmware/source/SoftRF/src/platform/CC13XX.h
@@ -189,6 +189,7 @@ extern SCSerial                 scSerial;
 //#define EXCLUDE_OLED_BARO_PAGE
 //#define EXCLUDE_LK8EX1
 #define USE_GNSS_PSM
+//#define USE_GDL90_MSL
 
 //#define USE_BASICMAC
 #define USE_TIME_SLOTS
diff --git a/software/firmware/source/SoftRF/src/platform/ESP32.h b/software/firmware/source/SoftRF/src/platform/ESP32.h
index 37d4fef14..d9d01bfac 100644
--- a/software/firmware/source/SoftRF/src/platform/ESP32.h
+++ b/software/firmware/source/SoftRF/src/platform/ESP32.h
@@ -239,6 +239,7 @@ struct rst_info {
 
 /* Experimental */
 //#define USE_BLE_MIDI
+//#define USE_GDL90_MSL
 
 //#define EXCLUDE_GNSS_UBLOX    /* Neo-6/7/8 */
 #define ENABLE_UBLOX_RFS        /* revert factory settings (when necessary)  */
diff --git a/software/firmware/source/SoftRF/src/platform/STM32.h b/software/firmware/source/SoftRF/src/platform/STM32.h
index f6145bf79..78ec99674 100644
--- a/software/firmware/source/SoftRF/src/platform/STM32.h
+++ b/software/firmware/source/SoftRF/src/platform/STM32.h
@@ -223,6 +223,7 @@ typedef struct stm32_backup_struct {
 #define EXCLUDE_LED_RING         //  -    kb
 #define EXCLUDE_SOUND
 //#define USE_GNSS_PSM
+//#define USE_GDL90_MSL
 #define EXCLUDE_LK8EX1
 #define EXCLUDE_IMU
 
diff --git a/software/firmware/source/SoftRF/src/platform/nRF52.cpp b/software/firmware/source/SoftRF/src/platform/nRF52.cpp
index f7a66e84f..87dd31b4f 100644
--- a/software/firmware/source/SoftRF/src/platform/nRF52.cpp
+++ b/software/firmware/source/SoftRF/src/platform/nRF52.cpp
@@ -271,7 +271,7 @@ static void nRF52_setup()
   ui = &ui_settings;
 
 #if 0
-  uint32_t reset_reason = getResetReason();
+  uint32_t reset_reason = readResetReason();
 
   if      (reset_reason & POWER_RESETREAS_RESETPIN_Msk)
   {
diff --git a/software/firmware/source/SoftRF/src/platform/nRF52.h b/software/firmware/source/SoftRF/src/platform/nRF52.h
index 7393b5977..72355b51f 100644
--- a/software/firmware/source/SoftRF/src/platform/nRF52.h
+++ b/software/firmware/source/SoftRF/src/platform/nRF52.h
@@ -257,6 +257,7 @@ struct rst_info {
 //#define USE_USB_MIDI
 //#define USE_BLE_MIDI
 //#define USE_PWM_SOUND
+//#define USE_GDL90_MSL
 //#define EXCLUDE_NUS
 #define EXCLUDE_BOARD_SELF_DETECT
 
diff --git a/software/firmware/source/SoftRF/src/protocol/data/GDL90.cpp b/software/firmware/source/SoftRF/src/protocol/data/GDL90.cpp
index 6d068fd4f..2f843dd2b 100644
--- a/software/firmware/source/SoftRF/src/protocol/data/GDL90.cpp
+++ b/software/firmware/source/SoftRF/src/protocol/data/GDL90.cpp
@@ -101,9 +101,13 @@ const GDL90_Msg_FF_ID_t msgFFid = {
   .ShortName    = {'S', 'o', 'f', 't', 'R', 'F', ' ', ' ' },
   .LongName     = {'S', 'o', 'f', 't', 'R', 'F',
                     ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ' },
+#if !defined(USE_GDL90_MSL)
+  .Capabilities = {0x00, 0x00, 0x00, 0x00}, /* WGS-84 ellipsoid altitude for Ownship Geometric report */
+#else
   .Capabilities = {0x00, 0x00, 0x00, 0x01}, /* MSL altitude for Ownship Geometric report */
+#endif /* USE_GDL90_MSL */
 };
-#endif
+#endif /* DO_GDL90_FF_EXT */
 
 /* convert a signed latitude to 2s complement ready for 24-bit packing */
 static uint32_t makeLatitude(float latitude)
@@ -313,7 +317,7 @@ static void *msgOwnershipGeometricAltitude(ufo_t *aircraft)
 {
   uint16_t vfom = 0x000A;
 
-#if 0
+#if !defined(USE_GDL90_MSL)
   /*
    * The Geo Altitude field is a 16-bit signed integer that represents
    * the geometric altitude (height above WGS-84 ellipsoid),
@@ -328,7 +332,7 @@ static void *msgOwnershipGeometricAltitude(ufo_t *aircraft)
    * SkyDemon is the only known exception which uses WGS-84 altitude still.
    */
   uint16_t altitude = (int16_t)(aircraft->altitude * _GPS_FEET_PER_METER / 5);
-#endif
+#endif /* USE_GDL90_MSL */
 
   GeometricAltitude.geo_altitude  = ((altitude & 0x00FF) << 8) | ((altitude & 0xFF00) >> 8) ;
   GeometricAltitude.VFOM          = ((vfom & 0x00FF) << 8) | ((vfom & 0xFF00) >> 8);