add terrain data to blackbox

Author: error414

src/main/blackbox/blackbox.c

@@ -520,6 +520,10 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
     {"escRPM",                -1, UNSIGNED, PREDICT(0),             ENCODING(UNSIGNED_VB)},
     {"escTemperature",        -1, SIGNED,   PREDICT(PREVIOUS),      ENCODING(SIGNED_VB)},
 #endif
+#ifdef USE_TERRAIN
+    {"terrainAGL",                -1, UNSIGNED, PREDICT(0),             ENCODING(SIGNED_VB)},
+    {"terrainAMSL",               -1, UNSIGNED, PREDICT(0),             ENCODING(SIGNED_VB)},
+#endif
 };
 
 #define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER
@@ -624,6 +628,10 @@ typedef struct blackboxSlowState_s {
 #ifdef USE_ESC_SENSOR
     uint32_t escRPM;
     int8_t escTemperature;
+#endif
+#ifdef USE_TERRAIN
+    int32_t terrainAGL;
+    int32_t terrainAMSL;
 #endif
     uint16_t rxUpdateRate;
     uint8_t activeWpNumber;
@@ -1409,6 +1417,10 @@ static void writeSlowFrame(void)
     blackboxWriteUnsignedVB(slowHistory.escRPM);
     blackboxWriteSignedVB(slowHistory.escTemperature);
 #endif
+#ifdef USE_TERRAIN
+    blackboxWriteSignedVB(slowHistory.terrainAGL);
+    blackboxWriteSignedVB(slowHistory.terrainAMSL);
+#endif
 
     blackboxSlowFrameIterationTimer = 0;
 }
@@ -1484,6 +1496,10 @@ static void loadSlowState(blackboxSlowState_t *slow)
     slow->escRPM = escSensor->rpm;
     slow->escTemperature = escSensor->temperature;
 #endif
+#ifdef USE_TERRAIN
+    slow->terrainAGL = terrainGetLastDistanceCm();
+    slow->terrainAMSL = terrainGetLastAMSL();
+#endif
 }
 
 /**

src/main/terrain/terrain.c

@@ -65,10 +65,12 @@ PG_RESET_TEMPLATE(terrainConfig_t, terrainConfig,
 
 static struct {
     timeMs_t lastUpdate;
-    int32_t terrainAGL;
+    int32_t terrainAGL;     // cm
+    int32_t terrainAMSL;    // cm
 } terrainHeight = {
     .terrainAGL = 0,
     .lastUpdate = 0,
+    .terrainAMSL = 0,
 };
 
 static struct {
@@ -182,21 +184,30 @@ void terrainUpdateTask(timeUs_t currentTimeUs)
     {
         float heightASL = getHeightAmslMeters(&gpsSol.llh);
         if(heightASL >= 0){
-            terrainHeight.terrainAGL =  MAX(0, ((int32_t)getEstimatedActualPosition(Z) + (int32_t)(terrainHomePos.homeAltitudeM * 100.0f)) - (int32_t)(heightASL * 100.0f));
+            terrainHeight.terrainAMSL = (int32_t)(heightASL * 100.0f);
+            terrainHeight.terrainAGL =  MAX(0, ((int32_t)getEstimatedActualPosition(Z) + (int32_t)(terrainHomePos.homeAltitudeM * 100.0f)) - terrainHeight.terrainAMSL);
             terrainHeight.lastUpdate = millis();
         }
     }
     /////////////////////////////////////////////////////////////////////////////////////////////////////
 }
 
+int32_t terrainGetLastAMSL(void){
+    if(!terrainConfig()->terrainEnabled){
+        return TERRAIN_STATUS_NO_DATA;
+    }
+
+    return terrainHeight.lastUpdate + TERRAIN_NO_DATA_DELAY_MS < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAMSL;
+}
+
 int32_t terrainGetLastDistanceCm(void)
 {
     //start a terrain system after the first query to terrain height
     if(!terrainConfig()->terrainEnabled){
         return TERRAIN_STATUS_NO_DATA;
     }
 
-    return terrainHeight.lastUpdate + 1000 < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAGL;
+    return terrainHeight.lastUpdate + TERRAIN_NO_DATA_DELAY_MS < millis() ? TERRAIN_STATUS_NO_DATA : terrainHeight.terrainAGL;
 }
 
 #endif

src/main/terrain/terrain.h

@@ -30,7 +30,7 @@
 #include "navigation/navigation.h"
 
 #define TERRAIN_TASK_RATE_HZ 5
-#define TERRAIN_TASK_ 200
+#define TERRAIN_NO_DATA_DELAY_MS 1000
 
 #define TERRAIN_STATUS_FAILURE (-1)
 #define TERRAIN_STATUS_WRONG_BLOC_SIZE (-2)
@@ -148,4 +148,6 @@ PG_DECLARE(terrainConfig_t, terrainConfig);
 
 void terrainInit(void);
 void terrainUpdateTask(timeUs_t currentTimeUs);
+int32_t terrainGetLastAMSL(void);
 int32_t terrainGetLastDistanceCm(void);
+

src/main/terrain/terrain_location.c

@@ -84,7 +84,7 @@ neVector_t gpsGetDistanceNE(const gpsLocation_t *a, const gpsLocation_t *b)
 {
     neVector_t v;
     v.north = (float)(b->lat - a->lat) * LOCATION_SCALING_FACTOR;
-    v.east  = (float)diffLongitude(b->lon, a->lon) * LOCATION_SCALING_FACTOR * longitudeScale((b->lat + b->lat)/2);
+    v.east  = (float)diffLongitude(b->lon, a->lon) * LOCATION_SCALING_FACTOR * longitudeScale((b->lat + a->lat)/2);
     return v;
 }