1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00

Merge remote-tracking branch 'origin/master' into mmosca-ublox-satinfo

This commit is contained in:
Marcelo Bezerra 2024-07-03 18:16:00 +02:00
commit ceebe355e8
7 changed files with 151 additions and 79 deletions

View file

@ -1,10 +1,10 @@
# INAV Programming Framework
INAV Programming Framework (IPF) is a mechanism that allows you to to create
INAV Programming Framework (IPF) is a mechanism that allows you to to create
custom functionality in INAV. You can choose for certain actions to be done,
based on custom conditions you select.
Logic conditions can be based on things such as RC channel values, switches, altitude,
Logic conditions can be based on things such as RC channel values, switches, altitude,
distance, timers, etc. The conditions you create can also make use of other conditions
you've entered previously.
The results can be used in:
@ -56,8 +56,8 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 9 | XOR | `true` if `Operand A` or `Operand B` is `true`, but not both |
| 10 | NAND | `false` if `Operand A` and `Operand B` are both `true`|
| 11 | NOR | `true` if `Operand A` and `Operand B` are both `false` |
| 12 | NOT | The boolean opposite to `Operand A` |
| 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|
| 12 | NOT | The boolean opposite to `Operand A` |
| 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|
| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result |
| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result |
| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result |
@ -147,7 +147,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 26 | Stabilized Pitch | Pitch PID controller output `[-500:500]` |
| 27 | Stabilized Yaw | Yaw PID controller output `[-500:500]` |
| 28 | 3D home distance [m] | 3D distance to home in `meters`. Calculated from Home distance and Altitude using Pythagorean theorem |
| 29 | CRSF LQ | Link quality as returned by the CRSF protocol |
| 29 | CRSF LQ | Link quality as returned by the CRSF protocol |
| 30 | CRSF SNR | SNR as returned by the CRSF protocol |
| 31 | GPS Valid Fix | Boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 32 | Loiter Radius [cm] | The current loiter radius in cm. |
@ -161,6 +161,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 40 | Yaw [deg] | Current heading (yaw) in `degrees` |
| 41 | FW Land Sate | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
| 42 | Current battery profile | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` |
| 43 | Flown Loiter Radius [m] | The actual loiter radius flown by a fixed wing during hold modes, in `meters` |
#### FLIGHT_MODE
@ -183,7 +184,7 @@ The flight mode operands return `true` when the mode is active. These are modes
| 12 | USER 3 | `true` when the **USER 3** mode is active. |
| 13 | USER 4 | `true` when the **USER 4** mode is active. |
| 14 | Acro | `true` when you are in the **Acro** flight mode. |
| 15 | Waypoint Mission | `true` when you are in the **WP Mission** flight mode. |
| 15 | Waypoint Mission | `true` when you are in the **WP Mission** flight mode. |
#### WAYPOINTS
@ -216,7 +217,7 @@ The flight mode operands return `true` when the mode is active. These are modes
| JUMP | 6 |
| SET_HEAD | 7 |
| LAND | 8 |
### Flags
All flags are reseted on ARM and DISARM event.
@ -333,7 +334,7 @@ Steps:
## Common issues / questions about IPF
One common mistake involves setting RC channel values. To override (set) the
One common mistake involves setting RC channel values. To override (set) the
value of a specific RC channel, choose "Override RC value", then for operand A
choose *value* and enter the channel number. Choosing "get RC value" is a common mistake,
which does something other than what you probably want.

56
docs/Serial Gimbal.md Normal file
View file

@ -0,0 +1,56 @@
# Serial Gimbal
INAV 8.0 introduces support for serial Gimbals. Currently, it is compatible with the protocol used by the Walksnail GM series gimbals.
While these gimbals also support PWM as input, using the Serial protocol gives it more flexibility and saves up to 4 PWM channels. The downside of the Serial protocol vs PWM input is that you don't have access to the full power of INAV's mixers. The main advantage is that you gain easy control of gimbal functions using INAV's modes.
# Axis Input
The Serial Gimbal supports 2 differents inputs.
## PWM Channels
This is the simplest way to control the Gimbal, as you can use your radio mixer and sliders to Control the gimbal by assigning RC channels to functions in the ```Configuration``` tab. You can control all 3 gimbal axis and unlike the raw PWM input, gimbal modes are controlled by INAV modes and you can control roll channel as well, instead of wiring 4 servo outputs. If an rc channel is set to 0, that input will be ignore and will be equivalent to a centered RC channel. So, if you setup the serial gimbal and don't assign any rc channels, it will stay centered, with default sensitivity and will obey the Gimbal MODES setup in the Modes tab.
## Headtracker Input
Headtracker input is only used when you have a Headtracker device configured and the ```Gimbal Headtracker``` mode is active.
A Headtracker device is a device that transmits headtracker information by a side channel, instead of relying on your rc link.
In head tracker mode, the Serial Gimbal will ignore the axis rc channel inputs and replace it with the inputs coming from the Headtracker device.
# Gimbal Modes
## No Gimbal mode selected
Like ACRO is the default mode for flight modes, the Gimbal will default to ```FPV Mode``` or ```Follow Mode``` when no mode is selected. The gimbal will try to stablized the footag and will follow the aircraft pitch, roll and yaw movements and use user inputs to point the camera where the user wants.
## Gimbal Center
This locks the gimbal camera to the center position and ignores any user input. Useful to reset the camera if you loose orientation.
## Gimbal Headtracker
Switches inputs to the configured Headtracker device. If no device is configured it will behave like Gimbal Center.
## Gimbal Level Tilt
This mode locks the camera tilt (pitch axis) and keeps it level with the horizon. Pitching the aircraft up and down, will move the camera so it stays pointing at the horizon. It can be combined with ```Gimbal Level Roll```.
## Gimbal Level Roll
This mode locks the camera roll and keeps it level with the horizon. Rolling the aircraft will move the camera so it stays level with the horizon. It can be combined with ```Gimbal Level Tilt```.
# Advanced settings
The gimbal also supports some advanced settings not exposed in the configurator.
## Gimbal Trim
You can set a trim setting for the gimbal, the idea is that it will shift the notion of center of the gimbal, like a trim and let you setup a fixed camera up tilt, like you would have in a traditional fpv quad setup.
```
gimbal_pan_trim = 0
Allowed range: -500 - 500
gimbal_tilt_trim = 0
Allowed range: -500 - 500
gimbal_roll_trim = 0
Allowed range: -500 - 500
```
## Gimbal and Headtracker on a single uart
As INAV does not process any inputs from the Walksnail Gimbal, it is possible to share the uard with the Walksnail Headtracking output by connect the fc TX to the gimbal and RX to receive the headtracker input.
```
gimbal_serial_single_uart = OFF
Allowed values: OFF, ON
```

View file

@ -28,16 +28,17 @@ void luluFilterInit(luluFilter_t *filter, int N)
FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
{
register float curVal = 0;
register float curValB = 0;
float curVal = 0;
float curValB = 0;
for (int N = 1; N <= filterN; N++)
{
int indexNeg = (index + windowSize - 2 * N) % windowSize;
register int curIndex = (indexNeg + 1) % windowSize;
register float prevVal = series[indexNeg];
register float prevValB = seriesB[indexNeg];
register int indexPos = (curIndex + N) % windowSize;
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
int curIndex = (indexNeg + 1) % windowSize;
float prevVal = series[indexNeg];
float prevValB = seriesB[indexNeg];
int indexPos = (curIndex + N) % windowSize;
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
{
if (indexPos >= windowSize)
{
@ -50,8 +51,8 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
// curIndex = (2 - 1) % 3 = 1
curVal = series[curIndex];
curValB = seriesB[curIndex];
register float nextVal = series[indexPos];
register float nextValB = seriesB[indexPos];
float nextVal = series[indexPos];
float nextValB = seriesB[indexPos];
// onbump (s, 1, 1, 3)
// if(onBump(series, curIndex, N, windowSize))
if (prevVal < curVal && curVal > nextVal)
@ -59,7 +60,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
float maxValue = MAX(prevVal, nextVal);
series[curIndex] = maxValue;
register int k = curIndex;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
@ -76,7 +77,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
curVal = maxValue;
seriesB[curIndex] = maxValue;
register int k = curIndex;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
@ -109,8 +110,8 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
// curIndex = (2 - 1) % 3 = 1
curVal = series[curIndex];
curValB = seriesB[curIndex];
register float nextVal = series[indexPos];
register float nextValB = seriesB[indexPos];
float nextVal = series[indexPos];
float nextValB = seriesB[indexPos];
if (prevVal > curVal && curVal < nextVal)
{
@ -118,7 +119,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
curVal = minValue;
series[curIndex] = minValue;
register int k = curIndex;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
@ -134,7 +135,7 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
float minValue = MIN(prevValB, nextValB);
curValB = minValue;
seriesB[curIndex] = minValue;
register int k = curIndex;
int k = curIndex;
for (int j = 1; j < N; j++)
{
if (++k >= windowSize)
@ -156,13 +157,13 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i
FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
{
// This is the value N of the LULU filter.
register int filterN = filter->N;
int filterN = filter->N;
// This is the total window size for the rolling buffer
register int filterWindow = filter->windowSize;
int filterWindow = filter->windowSize;
register int windowIndex = filter->windowBufIndex;
register float inputVal = input;
register int newIndex = (windowIndex + 1) % filterWindow;
int windowIndex = filter->windowBufIndex;
float inputVal = input;
int newIndex = (windowIndex + 1) % filterWindow;
filter->windowBufIndex = newIndex;
filter->luluInterim[windowIndex] = inputVal;
filter->luluInterimB[windowIndex] = -inputVal;

View file

@ -5074,5 +5074,12 @@ bool canFwLandingBeCancelled(void)
{
return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
}
#endif
uint16_t getFlownLoiterRadius(void)
{
if (STATE(AIRPLANE) && navGetCurrentStateFlags() & NAV_CTL_HOLD) {
return CENTIMETERS_TO_METERS(calculateDistanceToDestination(&posControl.desiredState.pos));
}
return 0;
}

View file

@ -694,6 +694,7 @@ bool rthAltControlStickOverrideCheck(uint8_t axis);
int8_t navCheckActiveAngleHoldAxis(void);
uint8_t getActiveWpNumber(void);
uint16_t getFlownLoiterRadius(void);
/* Returns the heading recorded when home position was acquired.
* Note that the navigation system uses deg*100 as unit and angles

View file

@ -97,7 +97,7 @@ static int logicConditionCompute(
int temporaryValue;
#if defined(USE_VTX_CONTROL)
vtxDeviceCapability_t vtxDeviceCapability;
#endif
#endif
switch (operation) {
@ -154,7 +154,7 @@ static int logicConditionCompute(
case LOGIC_CONDITION_NOR:
return !(operandA || operandB);
break;
break;
case LOGIC_CONDITION_NOT:
return !operandA;
@ -170,7 +170,7 @@ static int logicConditionCompute(
return false;
}
//When both operands are not met, keep current value
//When both operands are not met, keep current value
return currentValue;
break;
@ -238,7 +238,7 @@ static int logicConditionCompute(
gvSet(operandA, operandB);
return operandB;
break;
case LOGIC_CONDITION_GVAR_INC:
temporaryValue = gvGet(operandA) + operandB;
gvSet(operandA, temporaryValue);
@ -270,7 +270,7 @@ static int logicConditionCompute(
return operandA;
}
break;
case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY);
return true;
@ -293,12 +293,12 @@ static int logicConditionCompute(
ENABLE_STATE(CALIBRATE_MAG);
return true;
break;
#endif
#endif
case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
#if defined(USE_VTX_CONTROL)
#if defined(USE_VTX_CONTROL)
#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
if (
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
) {
logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
@ -346,18 +346,18 @@ static int logicConditionCompute(
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH);
return true;
break;
case LOGIC_CONDITION_INVERT_YAW:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW);
return true;
break;
case LOGIC_CONDITION_OVERRIDE_THROTTLE:
logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE);
return operandA;
break;
case LOGIC_CONDITION_SET_OSD_LAYOUT:
logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA;
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT);
@ -373,18 +373,18 @@ static int logicConditionCompute(
case LOGIC_CONDITION_SIN:
temporaryValue = (operandB == 0) ? 500 : operandB;
return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break;
case LOGIC_CONDITION_COS:
temporaryValue = (operandB == 0) ? 500 : operandB;
return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break;
break;
case LOGIC_CONDITION_TAN:
temporaryValue = (operandB == 0) ? 500 : operandB;
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue;
break;
case LOGIC_CONDITION_MIN:
@ -394,11 +394,11 @@ static int logicConditionCompute(
case LOGIC_CONDITION_MAX:
return (operandA > operandB) ? operandA : operandB;
break;
case LOGIC_CONDITION_MAP_INPUT:
return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000);
break;
case LOGIC_CONDITION_MAP_OUTPUT:
return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB);
break;
@ -507,7 +507,7 @@ static int logicConditionCompute(
default:
return false;
break;
break;
}
}
@ -516,7 +516,7 @@ void logicConditionProcess(uint8_t i) {
const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
if (logicConditions(i)->enabled && activatorValue && !cliMode) {
/*
* Process condition only when latch flag is not set
* Latched LCs can only go from OFF to ON, not the other way
@ -525,13 +525,13 @@ void logicConditionProcess(uint8_t i) {
const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value);
const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value);
const int newValue = logicConditionCompute(
logicConditionStates[i].value,
logicConditions(i)->operation,
operandAValue,
logicConditionStates[i].value,
logicConditions(i)->operation,
operandAValue,
operandBValue,
i
);
logicConditionStates[i].value = newValue;
/*
@ -606,7 +606,7 @@ static int logicConditionGetWaypointOperandValue(int operand) {
return distance;
}
break;
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
break;
@ -652,11 +652,11 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
return constrain(GPS_distanceToHome, 0, INT16_MAX);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
break;
@ -664,7 +664,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
return getBatteryVoltage();
break;
@ -680,7 +680,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100
return getAmperage();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh
return getMAhDrawn();
break;
@ -697,7 +697,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
return gpsSol.numSat;
}
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
return STATE(GPS_FIX) ? 1 : 0;
break;
@ -749,15 +749,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0;
break;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0;
break;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0;
break;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0;
@ -765,7 +765,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
break;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
#ifdef USE_FW_AUTOLAND
@ -773,22 +773,22 @@ static int logicConditionGetFlightOperandValue(int operand) {
#else
return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
#endif
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: //
return axisPID[YAW];
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: //
return axisPID[ROLL];
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: //
return axisPID[PITCH];
break;
@ -819,7 +819,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int
return getConfigBatteryProfile() + 1;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
return currentMixerProfileIndex + 1;
break;
@ -830,15 +830,20 @@ static int logicConditionGetFlightOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
return getLoiterRadius(navConfig()->fw.loiter_radius);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS:
return getFlownLoiterRadius();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS:
return isEstimatedAglTrusted();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_AGL:
return getEstimatedAglPosition();
break;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW:
return rangefinderGetLatestRawAltitude();
break;
@ -946,7 +951,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
//Extract RC channel raw value
if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
retVal = rxGetChannelValue(operand - 1);
}
}
break;
case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT:
@ -974,7 +979,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
retVal = programmingPidGetOutput(operand);
}
break;
case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
retVal = logicConditionGetWaypointOperandValue(operand);
break;
@ -988,7 +993,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
/*
* conditionId == -1 is always evaluated as true
*/
*/
int logicConditionGetValue(int8_t conditionId) {
if (conditionId >= 0) {
return logicConditionStates[conditionId].value;
@ -1070,7 +1075,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
uint32_t getLoiterRadius(uint32_t loiterRadius) {
#ifdef USE_PROGRAMMING_FRAMEWORK
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
!(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
} else {

View file

@ -143,6 +143,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40
LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41
LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42
LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43
} logicFlightOperands_e;
typedef enum {