From 2557dc0f40253c1666855d3042dd66540263a165 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 22 Apr 2022 12:06:10 +0200
Subject: [PATCH] Add rangefinder operands

---
 src/main/navigation/navigation.h               |  3 +++
 src/main/navigation/navigation_pos_estimator.c |  8 ++++++++
 src/main/programming/logic_condition.c         | 13 +++++++++++++
 src/main/programming/logic_condition.h         |  4 +++-
 4 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 16c00439e..ac91fe79f 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -584,6 +584,9 @@ int32_t getCruiseHeadingAdjustment(void);
 bool isAdjustingPosition(void);
 bool isAdjustingHeading(void);
 
+float getEstimatedAglPosition(void);
+bool isEstimatedAglTrusted(void);
+
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
  * are in the [0, 360 * 100) interval.
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 35cb78acd..615697602 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -809,6 +809,14 @@ bool isGPSGlitchDetected(void)
 }
 #endif
 
+float getEstimatedAglPosition(void) {
+    return posEstimator.est.aglAlt;
+}
+
+bool isEstimatedAglTrusted(void) {
+    return (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) ? true : false;
+}
+
 /**
  * Initialize position estimator
  *  Should be called once before any update occurs
diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index cb013e087..db447e6df 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -42,6 +42,7 @@
 #include "navigation/navigation.h"
 #include "sensors/battery.h"
 #include "sensors/pitotmeter.h"
+#include "sensors/rangefinder.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
 #include "drivers/io_port_expander.h"
@@ -612,6 +613,18 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
             return getLoiterRadius(navConfig()->fw.loiter_radius);
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
+            return isEstimatedAglTrusted();
+            break;
+    
+        case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
+            return getEstimatedAglPosition();
+            break;    
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
+            return rangefinderGetLatestRawAltitude();
+            break; 
+
         default:
             return 0;
             break;
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index 8eb602b45..359d5aa17 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -129,7 +129,9 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS,                           // 35
     LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int                    // 36
     LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS,                              // 37
-
+    LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2                      // 38
+    LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2                             // 39
+    LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int                   // 40
 } logicFlightOperands_e;
 
 typedef enum {