diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index b6f2780870..e23d915c8f 100644 --- a/docs/Programming Framework.md +++ b/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 @@ -125,8 +126,9 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | | 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 | +| 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 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 9845bde147..d9f6e668be 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/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; diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 5fa6f92733..44c0d213c6 100644 --- a/src/main/programming/logic_condition.c +++ b/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; + } +} diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 3b795ba43e..ad0abf90f7 100644 --- a/src/main/programming/logic_condition.h +++ b/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);