From 6a6193551c0c9bfcbbb69265880471d59d17681c Mon Sep 17 00:00:00 2001
From: Tony Cabello <tcabello@blitworks.com>
Date: Thu, 29 Nov 2018 06:11:06 +0100
Subject: [PATCH] Numeric Vario calculated with GPS altitude

---
 src/main/flight/position.c           | 15 ++++++++++++++-
 src/main/io/gps.c                    | 11 +++++++++++
 src/main/io/gps.h                    |  1 +
 src/test/unit/flight_imu_unittest.cc |  1 +
 4 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/position.c b/src/main/flight/position.c
index 99b2818d3..afae24f68 100644
--- a/src/main/flight/position.c
+++ b/src/main/flight/position.c
@@ -82,8 +82,11 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     previousTimeUs = currentTimeUs;
 
     int32_t baroAlt = 0;
-
     int32_t gpsAlt = 0;
+
+#if defined(USE_GPS) && defined(USE_VARIO)
+    int16_t gpsVertSpeed = 0;
+#endif
     float gpsTrust = 0.3; //conservative default
     bool haveBaroAlt = false;
     bool haveGpsAlt = false;
@@ -101,6 +104,9 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 #ifdef USE_GPS
 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
     gpsAlt = gpsSol.llh.altCm;
+#ifdef USE_VARIO
+    gpsVertSpeed = GPS_verticalSpeedInCmS;
+#endif
     haveGpsAlt = true;
 
     if (gpsSol.hdop != 0) {
@@ -124,10 +130,14 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
     if (haveGpsAlt && haveBaroAlt) {
         estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
 #ifdef USE_VARIO
+        // baro is a better source for vario, so ignore gpsVertSpeed
         estimatedVario = calculateEstimatedVario(baroAlt, dTime);
 #endif
     } else if (haveGpsAlt) {
         estimatedAltitudeCm = gpsAlt;
+#if defined(USE_VARIO) && defined(USE_GPS)
+        estimatedVario = gpsVertSpeed;
+#endif
     } else if (haveBaroAlt) {
         estimatedAltitudeCm = baroAlt;
 #ifdef USE_VARIO
@@ -138,6 +148,9 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
     DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
+#ifdef USE_VARIO
+    DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
+#endif
 }
 
 bool isAltitudeOffset(void)
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index a7ef510a5..ab9676211 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -78,6 +78,7 @@ int32_t GPS_home[2];
 uint16_t GPS_distanceToHome;        // distance to home point in meters
 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
+int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
 float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
 int16_t nav_takeoff_bearing;
 navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
@@ -1318,9 +1319,14 @@ void GPS_calculateDistanceAndDirectionToHome(void)
 static void GPS_calculateDistanceFlown(bool initialize)
 {
     static int32_t lastCoord[2] = { 0, 0 };
+    static int16_t lastAlt;
+    static int32_t lastMillis;
+
+    int currentMillis = millis();
 
     if (initialize) {
         GPS_distanceFlownInCm = 0;
+        GPS_verticalSpeedInCmS = 0;
     } else {
         // Only add up movement when speed is faster than minimum threshold
         if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) {
@@ -1329,9 +1335,14 @@ static void GPS_calculateDistanceFlown(bool initialize)
             GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
             GPS_distanceFlownInCm += dist;
         }
+
+        GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
+        GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500.0f, 1500.0f);
     }
     lastCoord[LON] = gpsSol.llh.lon;
     lastCoord[LAT] = gpsSol.llh.lat;
+    lastAlt = gpsSol.llh.altCm;
+    lastMillis = currentMillis;
 }
 
 void onGpsNewData(void)
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 30ba4177a..471cf63c6 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -126,6 +126,7 @@ extern int32_t GPS_home[2];
 extern uint16_t GPS_distanceToHome;        // distance to home point in meters
 extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 extern uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
+extern int16_t GPS_verticalSpeedInCmS;     // vertical speed in cm/s
 extern int16_t GPS_angle[ANGLE_INDEX_COUNT];                // it's the angles that must be applied for GPS correction
 extern float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
 extern float GPS_scaleLonDown;  // this is used to offset the shrinking longitude as we go towards the poles
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index ce751678a..401757beb 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -208,6 +208,7 @@ acc_t acc;
 mag_t mag;
 
 gpsSolutionData_t gpsSol;
+int16_t GPS_verticalSpeedInCmS;
 
 uint8_t debugMode;
 int16_t debug[DEBUG16_VALUE_COUNT];