Adding loiter radius to programming

Adding a loiter radius override to the programming. This change allows pilots to change their loiter radius on the fly. The minimum loiter radius will always be that set in Advanced Tuning, for safety. The maximum radius is 100000cm.

As part of this change, I increased the min and max constraints of the basic maths conditions. The were set to int16, however operands can be int32. Before the change, this caused clipping of the radius when performing basic calculus on it.

docs/Programming Framework.md

@@ -74,6 +74,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 38            | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
 | 39            | SET_HEADING_TARGET  | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
 | 40            | MOD           | Divide `Operand A` by `Operand B` and returns the remainder |
+| 41			| LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
 
 
 ### Operands
@@ -127,6 +128,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 32            | CROSSFIRE LQ          | Crossfire Link quality as returned by the CRSF protocol   | 
 | 33            | CROSSFIRE SNR         | Crossfire SNR as returned by the CRSF protocol   |
 | 34            | GPS_VALID             | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
+| 35			| LOITER_RADIUS			| The current loiter radius in cm. |
 
 #### ACTIVE_WAYPOINT_ACTION
 

src/main/navigation/navigation_fixedwing.c

@@ -49,6 +49,8 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
+#include "programming/logic_condition.h"
+
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
@@ -271,10 +273,16 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
     // Limit minimum forward velocity to 1 m/s
     float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
 
+    uint32_t navLoiterRadius = navConfig()->fw.loiter_radius;
+
+#ifdef USE_PROGRAMMING_FRAMEWORK
+    navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
+#endif
+
     // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
     #define TAN_15DEG    0.26795f
     bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
-                                            && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
+                                            && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
                                             && (distanceToActualTarget > 50.0f)
                                             && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
 
@@ -283,8 +291,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
         // We are closing in on a waypoint, calculate circular loiter
         float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
 
-        float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
-        float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
+        float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
+        float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
 
         // We have temporary loiter target. Recalculate distance and position error
         posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;

src/main/programming/logic_condition.c

@@ -176,20 +176,20 @@ static int logicConditionCompute(
             break;
 
         case LOGIC_CONDITION_ADD:
-            return constrain(operandA + operandB, INT16_MIN, INT16_MAX);
+            return constrain(operandA + operandB, INT32_MIN, INT32_MAX);
             break;
 
         case LOGIC_CONDITION_SUB:
-            return constrain(operandA - operandB, INT16_MIN, INT16_MAX);
+            return constrain(operandA - operandB, INT32_MIN, INT32_MAX);
             break;
 
         case LOGIC_CONDITION_MUL:
-            return constrain(operandA * operandB, INT16_MIN, INT16_MAX);
+            return constrain(operandA * operandB, INT32_MIN, INT32_MAX);
             break;
 
         case LOGIC_CONDITION_DIV:
             if (operandB != 0) {
-                return constrain(operandA / operandB, INT16_MIN, INT16_MAX);
+                return constrain(operandA / operandB, INT32_MIN, INT32_MAX);
             } else {
                 return operandA;
             }
@@ -333,6 +333,11 @@ static int logicConditionCompute(
             }
             break;
 
+        case LOGIC_CONDITION_LOITER_OVERRIDE:
+            logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
+            LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);
+            return true;
+            break;
         default:
             return false;
             break; 
@@ -533,6 +538,9 @@ static int logicConditionGetFlightOperandValue(int operand) {
         #endif
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
+            return getLoiterRadius(navConfig()->fw.loiter_radius);
+
         default:
             return 0;
             break;
@@ -720,3 +728,11 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
         return originalValue;
     }
 }
+
+uint32_t getLoiterRadius(uint32_t loiterRadius) {
+    if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS)) {
+        return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
+    } else {
+        return loiterRadius;
+    }
+}

src/main/programming/logic_condition.h

@@ -70,7 +70,8 @@ typedef enum {
     LOGIC_CONDITION_RC_CHANNEL_OVERRIDE         = 38,
     LOGIC_CONDITION_SET_HEADING_TARGET          = 39,
     LOGIC_CONDITION_MODULUS                     = 40,
-    LOGIC_CONDITION_LAST                        = 41,
+    LOGIC_CONDITION_LOITER_OVERRIDE             = 41,
+    LOGIC_CONDITION_LAST                        = 42,
 } logicOperation_e;
 
 typedef enum logicOperandType_s {
@@ -120,6 +121,7 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ,                                 // 32
     LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR,                                // 33
     LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1                        // 34
+    LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS                            // 35
 
 } logicFlightOperands_e;
 
@@ -148,6 +150,7 @@ typedef enum {
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
     LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
+    LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
 } logicConditionsGlobalFlags_t;
 
 typedef enum {
@@ -198,3 +201,4 @@ void logicConditionReset(void);
 float getThrottleScale(float globalThrottleScale);
 int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
 int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
+uint32_t getLoiterRadius(uint32_t loiterRadius);