Merge pull request #7984 from iNavFlight/dzikuvx-add-rangefinder-to-logic-conditions

Dzikuvx add rangefinder to logic conditions

docs/Programming Framework.md

@@ -133,6 +133,10 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 34            | GPS_VALID                     | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
 | 35            | LOITER_RADIUS                 | The current loiter radius in cm. |
 | 36            | ACTIVE_PROFILE                | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
+| 37            | BATT_CELLS                    | Number of battery cells detected |
+| 38            | AGL_STATUS                    | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
+| 39            | AGL                           | integer Above The Groud Altitude in `cm` |
+| 40            | RANGEFINDER_RAW               | integer raw distance provided by the rangefinder in `cm` |
 
 #### ACTIVE_WAYPOINT_ACTION
 

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.

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

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;

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 {