From 596c025ef1a629aa8738ae2faaa55b9b5362062b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 25 Jun 2024 14:36:59 +0200 Subject: [PATCH 1/7] Remove of register optimization in favor or relying on GCC optimization. --- src/main/common/lulu.c | 41 +++++++++++++++++++++-------------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/src/main/common/lulu.c b/src/main/common/lulu.c index c44d6724ec..8ab7803ed1 100644 --- a/src/main/common/lulu.c +++ b/src/main/common/lulu.c @@ -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; From e05c38fbcbf30b0708c93b835d712b05992ff663 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 29 Jun 2024 12:12:19 +0200 Subject: [PATCH 2/7] Add some docs on Serial Gimbal --- docs/Serial Gimbal.mb | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 docs/Serial Gimbal.mb diff --git a/docs/Serial Gimbal.mb b/docs/Serial Gimbal.mb new file mode 100644 index 0000000000..795cb5490a --- /dev/null +++ b/docs/Serial Gimbal.mb @@ -0,0 +1,33 @@ +# 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, and just lets the Gimbal use the value of a given RC PWM Channel. You can control all 3 gimbal axis, plust the Gimbal sensitivity. Unlike the raw PWM input, modes are controlled by INAV modes, instead of a PWM channels. If an rc channel is set to 0, that input will be ignore. 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. +Since it is using rc channels as inputs, you can have a mixer in your radio and setup a head tracker in the traditional way, like you would with home made servo gimbal. + +## 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```. From 17729ed230477c81bc920bc3a79a031fc959de62 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 29 Jun 2024 12:17:43 +0200 Subject: [PATCH 3/7] Oops... --- docs/{Serial Gimbal.mb => Serial Gimbal.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename docs/{Serial Gimbal.mb => Serial Gimbal.md} (100%) diff --git a/docs/Serial Gimbal.mb b/docs/Serial Gimbal.md similarity index 100% rename from docs/Serial Gimbal.mb rename to docs/Serial Gimbal.md From a9cdb7456008b22d40c88d11545c691df16a580c Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 29 Jun 2024 12:26:37 +0200 Subject: [PATCH 4/7] More changes --- docs/Serial Gimbal.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docs/Serial Gimbal.md b/docs/Serial Gimbal.md index 795cb5490a..7b1b20a1bb 100644 --- a/docs/Serial Gimbal.md +++ b/docs/Serial Gimbal.md @@ -7,8 +7,7 @@ While these gimbals also support PWM as input, using the Serial protocol gives i The Serial Gimbal supports 2 differents inputs. ## PWM Channels -This is the simplest way to control the Gimbal, and just lets the Gimbal use the value of a given RC PWM Channel. You can control all 3 gimbal axis, plust the Gimbal sensitivity. Unlike the raw PWM input, modes are controlled by INAV modes, instead of a PWM channels. If an rc channel is set to 0, that input will be ignore. 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. -Since it is using rc channels as inputs, you can have a mixer in your radio and setup a head tracker in the traditional way, like you would with home made servo gimbal. +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, plust the Gimbal sensitivity. 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. From dc3e723ccdf0d93f213a3a8c2ad17f40f67a48ec Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 29 Jun 2024 06:45:14 -0400 Subject: [PATCH 5/7] Update Serial Gimbal.md --- docs/Serial Gimbal.md | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/docs/Serial Gimbal.md b/docs/Serial Gimbal.md index 7b1b20a1bb..984e00f29d 100644 --- a/docs/Serial Gimbal.md +++ b/docs/Serial Gimbal.md @@ -7,7 +7,7 @@ While these gimbals also support PWM as input, using the Serial protocol gives i 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, plust the Gimbal sensitivity. 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. +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. @@ -30,3 +30,27 @@ This mode locks the camera tilt (pitch axis) and keeps it level with the horizon ## 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 +``` From 817981fa4b4c3d9ba6bb4a6975b799057dba9b9c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 29 Jun 2024 13:43:10 +0100 Subject: [PATCH 6/7] add flown loiter radius for fixed wing --- src/main/navigation/navigation.c | 9 ++- src/main/navigation/navigation.h | 1 + src/main/programming/logic_condition.c | 105 +++++++++++++------------ src/main/programming/logic_condition.h | 1 + 4 files changed, 65 insertions(+), 51 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6eed742e80..f7c7303d13 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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; +} diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 101b41cd63..1b7734c8b1 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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 diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f927ac18d1..6e43d2394c 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -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 { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 5defafaee6..25d6a5a9e1 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -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 { From 698ecad29b6695ad3404b9b5cc20035eead12787 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 29 Jun 2024 17:17:51 +0100 Subject: [PATCH 7/7] Update Programming Framework.md --- docs/Programming Framework.md | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 4eb7744e54..b2ce6fe3b9 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -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.