1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-16 21:05:32 +03:00

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.
This commit is contained in:
Darren Lines 2021-09-22 22:32:56 +01:00
parent b51cb7739a
commit b66caae07a
4 changed files with 40 additions and 10 deletions

View file

@ -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` | | 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. | | 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 | | 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 ### 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 | | 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR 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 | | 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 #### ACTIVE_WAYPOINT_ACTION

View file

@ -49,6 +49,8 @@
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "programming/logic_condition.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/battery.h" #include "sensors/battery.h"
@ -271,10 +273,16 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Limit minimum forward velocity to 1 m/s // Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); 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 // 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 #define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait()) bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG)) && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f) && (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE); && !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 // We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f); 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 loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle); float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error // We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;

View file

@ -176,20 +176,20 @@ static int logicConditionCompute(
break; break;
case LOGIC_CONDITION_ADD: case LOGIC_CONDITION_ADD:
return constrain(operandA + operandB, INT16_MIN, INT16_MAX); return constrain(operandA + operandB, INT32_MIN, INT32_MAX);
break; break;
case LOGIC_CONDITION_SUB: case LOGIC_CONDITION_SUB:
return constrain(operandA - operandB, INT16_MIN, INT16_MAX); return constrain(operandA - operandB, INT32_MIN, INT32_MAX);
break; break;
case LOGIC_CONDITION_MUL: case LOGIC_CONDITION_MUL:
return constrain(operandA * operandB, INT16_MIN, INT16_MAX); return constrain(operandA * operandB, INT32_MIN, INT32_MAX);
break; break;
case LOGIC_CONDITION_DIV: case LOGIC_CONDITION_DIV:
if (operandB != 0) { if (operandB != 0) {
return constrain(operandA / operandB, INT16_MIN, INT16_MAX); return constrain(operandA / operandB, INT32_MIN, INT32_MAX);
} else { } else {
return operandA; return operandA;
} }
@ -333,6 +333,11 @@ static int logicConditionCompute(
} }
break; 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: default:
return false; return false;
break; break;
@ -533,6 +538,9 @@ static int logicConditionGetFlightOperandValue(int operand) {
#endif #endif
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
return getLoiterRadius(navConfig()->fw.loiter_radius);
default: default:
return 0; return 0;
break; break;
@ -720,3 +728,11 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
return 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;
}
}

View file

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