From 775a080768fa475c65510abc203fc78d05311afa Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 22 Nov 2022 13:40:36 +0000 Subject: [PATCH 01/11] Initial code This will break LCs backwards compatibility. But a small conversion tool will be simple to make. --- src/main/programming/logic_condition.c | 37 ++++++++++++++++-------- src/main/programming/logic_condition.h | 40 ++++++++++++++------------ 2 files changed, 47 insertions(+), 30 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d96871a43e..54481b6e3a 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -442,6 +442,27 @@ void logicConditionProcess(uint8_t i) { } } +static int logicConditionGetWaypointOperandValue(int operand) { + + switch (operand) { + case LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP: // 0/1 + return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX: + return NAV_Status.activeWpNumber; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION: + return NAV_Status.activeWpAction; + break; + + default: + return 0; + break; + } +} + static int logicConditionGetFlightOperandValue(int operand) { switch (operand) { @@ -555,10 +576,6 @@ static int logicConditionGetFlightOperandValue(int operand) { return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0; break; - case LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP: // 0/1 - return (navGetCurrentStateFlags() & NAV_AUTO_WP) ? 1 : 0; - break; - case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0; break; @@ -579,14 +596,6 @@ static int logicConditionGetFlightOperandValue(int operand) { return axisPID[PITCH]; break; - case LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX: - return NAV_Status.activeWpNumber; - break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION: - return NAV_Status.activeWpAction; - break; - case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX); break; @@ -739,6 +748,10 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { retVal = programmingPidGetOutput(operand); } break; + + case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS: + retVal = logicConditionGetWaypointOperandValue(operand); + break; default: break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 1d1f106dd3..1a93db6e92 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -87,6 +87,7 @@ typedef enum logicOperandType_s { LOGIC_CONDITION_OPERAND_TYPE_LC, // Result of different LC and LC operand LOGIC_CONDITION_OPERAND_TYPE_GVAR, // Value from a global variable LOGIC_CONDITION_OPERAND_TYPE_PID, // Value from a Programming PID + LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS, LOGIC_CONDITION_OPERAND_TYPE_LAST } logicOperandType_e; @@ -114,24 +115,21 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1 // 20 LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING, // 0/1 // 21 LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1 // 22 - LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1 // 23 - LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 24 - LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 25 - LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL, // 26 - LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 27 - LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 28 - LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX, // 29 - LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION, // 30 - LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 - 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 - LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36 - LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 37 - LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 38 - LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 39 - LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 40 + LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 23 - was 24 + LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 24 - was 25 + LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL, // 25 - was 26 + LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26 - was 27 + LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27 - was 28 + LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28 - was 31 + LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29 - was 32 + LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39 - was 33 + LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31 - was 34 + LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32 - was 35 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33 - was 36 + LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 34 - was 37 + LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 35 - was 38 + LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 36 - was 39 + LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 37 - was 40 } logicFlightOperands_e; typedef enum { @@ -151,6 +149,12 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13 } logicFlightModeOperands_e; +typedef enum { + LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 1 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 2 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 3 +} logicWaypointOperands_e; + typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1), From 7115d03276b30367ff41afe90618388d7a72444f Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 23 Nov 2022 13:35:18 +0000 Subject: [PATCH 02/11] add distance to WP needs testing ... and compiling even. Coding at work is blind xD --- src/main/programming/logic_condition.c | 18 ++++++++++++++++++ src/main/programming/logic_condition.h | 7 ++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 54481b6e3a..643264dc17 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -457,6 +457,24 @@ static int logicConditionGetWaypointOperandValue(int operand) { return NAV_Status.activeWpAction; break; + case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE: + { + uint32_t distance = 0; + if ((navGetCurrentStateFlags() & NAV_AUTO_WP)) { + fpVector3_t poi; + gpsLocation_t wp; + wp.lat = posControl.waypointList[NAV_Status.activeWpNumber].lat; + wp.lon = posControl.waypointList[NAV_Status.activeWpNumber].lon; + wp.alt = posControl.waypointList[NAV_Status.activeWpNumber].alt; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, waypointMissionAltConvMode(posControl.waypointList[NAV_Status.activeWpNumber].p3)); + + distance = calculateDistanceToDestination(&poi); + } + + return distance; + } + break; + default: return 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 1a93db6e92..53a3b8400e 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -150,9 +150,10 @@ typedef enum { } logicFlightModeOperands_e; typedef enum { - LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 1 - LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 2 - LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 3 + LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 1 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 2 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 3 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE, // 4 } logicWaypointOperands_e; typedef enum { From b2ad70329ba516393f822bad183d1cc27621f42c Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 23 Nov 2022 22:39:22 +0000 Subject: [PATCH 03/11] fixed initial code and added more Now working: - Distance to WP - Distance from WP - User actions 1 to 4 --- src/main/programming/logic_condition.c | 46 ++++++++++++++++++++++---- src/main/programming/logic_condition.h | 14 +++++--- 2 files changed, 50 insertions(+), 10 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 643264dc17..ceba2d4db6 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -460,21 +460,55 @@ static int logicConditionGetWaypointOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE: { uint32_t distance = 0; - if ((navGetCurrentStateFlags() & NAV_AUTO_WP)) { + if (navGetCurrentStateFlags() & NAV_AUTO_WP) { fpVector3_t poi; gpsLocation_t wp; - wp.lat = posControl.waypointList[NAV_Status.activeWpNumber].lat; - wp.lon = posControl.waypointList[NAV_Status.activeWpNumber].lon; - wp.alt = posControl.waypointList[NAV_Status.activeWpNumber].alt; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, waypointMissionAltConvMode(posControl.waypointList[NAV_Status.activeWpNumber].p3)); + wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-1].lat; + wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-1].lon; + wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-1].alt; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE); - distance = calculateDistanceToDestination(&poi); + distance = calculateDistanceToDestination(&poi) / 100; } return distance; } break; + case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT: + { + uint32_t distance = 0; + if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpNumber > 1) { + fpVector3_t poi; + gpsLocation_t wp; + wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-2].lat; + wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-2].lon; + wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-2].alt; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE); + + distance = calculateDistanceToDestination(&poi) / 100; + } + + return distance; + } + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: + return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER1; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: + return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER2; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: + return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER3; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: + return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER4; + break; + default: return 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 53a3b8400e..69ebd46524 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -150,10 +150,16 @@ typedef enum { } logicFlightModeOperands_e; typedef enum { - LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 1 - LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 2 - LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 3 - LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE, // 4 + LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 0 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 1 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 2 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE, // 3 + LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT, // 4 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION, // 5 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION, // 6 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION, // 7 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION, // 8 + } logicWaypointOperands_e; typedef enum { From b5da2ca7d3bd551f3a98a0daf100e57f17ef3d41 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 24 Nov 2022 08:04:52 +0000 Subject: [PATCH 04/11] Corrected user actions and updated docs --- docs/Programming Framework.md | 72 +++++++++++++++----------- src/main/programming/logic_condition.c | 8 +-- 2 files changed, 45 insertions(+), 35 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index f51f841bb9..241b604f7d 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -119,37 +119,21 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 20 | IS_POSITION_CONTROL | boolean `0`/`1` | | 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` | | 22 | IS_RTH | boolean `0`/`1` | -| 23 | IS_WP | boolean `0`/`1` | -| 24 | IS_LANDING | boolean `0`/`1` | -| 25 | IS_FAILSAFE | boolean `0`/`1` | -| 26 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` | -| 27 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` | -| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | -| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` | -| 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 | -| 35 | LOITER_RADIUS | The current loiter radius in cm. | -| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | -| 37 | BATT_CELLS | Number of battery cells detected | -| 38 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted | -| 39 | AGL | integer Above The Groud Altitude in `cm` | -| 40 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` | - -#### ACTIVE_WAYPOINT_ACTION - -| Action | Value | -|---------------|-------| -| WAYPOINT | 1 | -| HOLD_TIME | 3 | -| RTH | 4 | -| SET_POI | 5 | -| JUMP | 6 | -| SET_HEAD | 7 | -| LAND | 8 | - +| 23 | IS_LANDING | boolean `0`/`1` | +| 24 | IS_FAILSAFE | boolean `0`/`1` | +| 25 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` | +| 26 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` | +| 27 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | +| 28 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | +| 29 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | +| 30 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 31 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | +| 32 | LOITER_RADIUS | The current loiter radius in cm. | +| 33 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | +| 34 | BATT_CELLS | Number of battery cells detected | +| 35 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted | +| 36 | AGL | integer Above The Groud Altitude in `cm` | +| 37 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` | #### FLIGHT_MODE @@ -167,6 +151,32 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 9 | USER1 | | | 10 | USER2 | | +#### WAYPOINTS + +| Operand Value | Name | Notes | +|---------------|-------------------------------|-------| +| 0 | Is WP | boolean `0`/`1` | +| 1 | Current Waypoint Index | Current waypoint leg. Indexed from `1`. To verify WP is in progress, use `Is WP` | +| 2 | Current Waypoint Action | Action active in current leg. See ACTIVE_WAYPOINT_ACTION table | +| 3 | Distance to next Waypoint | Distance to next WP in metres | +| 4 | Distance from Waypoint | Distance from the last WP in metres | +| 5 | User Action 1 | User Action 1 is active on this waypoint leg [boolean `0`/`1`] | +| 6 | User Action 2 | User Action 2 is active on this waypoint leg [boolean `0`/`1`] | +| 7 | User Action 3 | User Action 3 is active on this waypoint leg [boolean `0`/`1`] | +| 8 | User Action 4 | User Action 4 is active on this waypoint leg [boolean `0`/`1`] | + + +#### ACTIVE_WAYPOINT_ACTION + +| Action | Value | +|---------------|-------| +| WAYPOINT | 1 | +| HOLD_TIME | 3 | +| RTH | 4 | +| SET_POI | 5 | +| JUMP | 6 | +| SET_HEAD | 7 | +| LAND | 8 | ### Flags diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index ceba2d4db6..09afeec6f8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -494,19 +494,19 @@ static int logicConditionGetWaypointOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER1; + return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER1; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER2; + return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER2; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER3; + return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER3; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber-1].p3 == NAV_WP_USER4; + return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER4; break; default: From 63652aa4dd74b57492c19df542b604a579b3ffaa Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 25 Nov 2022 18:44:06 +0000 Subject: [PATCH 05/11] Removed comments for old IDs --- src/main/programming/logic_condition.h | 30 +++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 69ebd46524..c43400904e 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -115,21 +115,21 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1 // 20 LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING, // 0/1 // 21 LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1 // 22 - LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 23 - was 24 - LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 24 - was 25 - LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL, // 25 - was 26 - LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26 - was 27 - LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27 - was 28 - LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28 - was 31 - LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29 - was 32 - LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39 - was 33 - LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31 - was 34 - LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32 - was 35 - LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33 - was 36 - LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 34 - was 37 - LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 35 - was 38 - LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 36 - was 39 - LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 37 - was 40 + LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 23 + LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 24 + LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL, // 25 + LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26 + LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27 + LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28 + LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29 + LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31 + LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33 + LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 34 + LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 35 + LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 36 + LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 37 } logicFlightOperands_e; typedef enum { From 8f4fc0754bb9b8a2135b3f8123389f6765cf995e Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 11 Dec 2022 21:18:51 +0000 Subject: [PATCH 06/11] Update for when User action is triggered --- src/main/programming/logic_condition.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 09afeec6f8..0a47a778a2 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -494,19 +494,19 @@ static int logicConditionGetWaypointOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER1; + return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER1; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER2; + return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER2; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER3; + return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER3; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber].p3 == NAV_WP_USER4; + return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER4; break; default: From 374a8f8ce82e0bd5bc1ff58a3d97f613575be6fe Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 11 Dec 2022 21:34:42 +0000 Subject: [PATCH 07/11] Check the bit correctly --- src/main/programming/logic_condition.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 0a47a778a2..fae42b19a1 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -460,7 +460,7 @@ static int logicConditionGetWaypointOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE: { uint32_t distance = 0; - if (navGetCurrentStateFlags() & NAV_AUTO_WP) { + if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpNumber > 0) { fpVector3_t poi; gpsLocation_t wp; wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-1].lat; @@ -494,19 +494,19 @@ static int logicConditionGetWaypointOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER1; + return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER1 : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER2; + return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER2 : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER3; + return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER3 : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: - return posControl.waypointList[NAV_Status.activeWpNumber+1].p3 == NAV_WP_USER4; + return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER4 : 0; break; default: From bc5cc39a1bcfabd2a81e8f119f09bf611452e37d Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 14 Dec 2022 19:05:18 +0000 Subject: [PATCH 08/11] Fixed return issue Returning the bit, not a Boolean, fixed. --- src/main/programming/logic_condition.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index fae42b19a1..c505a9fe7c 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -494,19 +494,19 @@ static int logicConditionGetWaypointOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: - return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER1 : 0; + return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: - return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER2 : 0; + return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: - return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER3 : 0; + return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: - return (NAV_Status.activeWpNumber > 0) ? posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER4 : 0; + return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; break; default: From fe2e151583efec5bed72a78b15641c1f60759ca0 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 14 Dec 2022 21:52:53 +0000 Subject: [PATCH 09/11] Reduced calculations on WpNumber --- src/main/navigation/navigation.c | 4 +++- src/main/navigation/navigation.h | 1 + src/main/programming/logic_condition.c | 24 ++++++++++++------------ 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 05f4c4756c..f2273dc481 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1932,7 +1932,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION; if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE; - NAV_Status.activeWpNumber = posControl.activeWaypointIndex - posControl.startWpIndex + 1; + NAV_Status.activeWpIndex = posControl.activeWaypointIndex - posControl.startWpIndex; + NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1; + NAV_Status.activeWpAction = 0; if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 339da15392..d178d27bd0 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -468,6 +468,7 @@ typedef struct { navSystemStatus_Error_e error; navSystemStatus_Flags_e flags; uint8_t activeWpNumber; + uint8_t activeWpIndex; navWaypointActions_e activeWpAction; } navSystemStatus_t; diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index c505a9fe7c..ba61583331 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -460,12 +460,12 @@ static int logicConditionGetWaypointOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE: { uint32_t distance = 0; - if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpNumber > 0) { + if (navGetCurrentStateFlags() & NAV_AUTO_WP) { fpVector3_t poi; gpsLocation_t wp; - wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-1].lat; - wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-1].lon; - wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-1].alt; + wp.lat = posControl.waypointList[NAV_Status.activeWpIndex].lat; + wp.lon = posControl.waypointList[NAV_Status.activeWpIndex].lon; + wp.alt = posControl.waypointList[NAV_Status.activeWpIndex].alt; geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE); distance = calculateDistanceToDestination(&poi) / 100; @@ -478,12 +478,12 @@ static int logicConditionGetWaypointOperandValue(int operand) { case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT: { uint32_t distance = 0; - if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpNumber > 1) { + if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) { fpVector3_t poi; gpsLocation_t wp; - wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-2].lat; - wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-2].lon; - wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-2].alt; + wp.lat = posControl.waypointList[NAV_Status.activeWpIndex-1].lat; + wp.lon = posControl.waypointList[NAV_Status.activeWpIndex-1].lon; + wp.alt = posControl.waypointList[NAV_Status.activeWpIndex-1].alt; geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE); distance = calculateDistanceToDestination(&poi) / 100; @@ -494,19 +494,19 @@ static int logicConditionGetWaypointOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: - return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: - return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0; + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: - return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0; + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0; break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: - return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; break; default: From 812308a4820d545d18f15731871ab4e0dbf1fd87 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 19 Dec 2022 13:01:32 +0000 Subject: [PATCH 10/11] Add ability to check actions on next WP --- src/main/programming/logic_condition.c | 26 ++++++++++++++++++++++++++ src/main/programming/logic_condition.h | 18 +++++++++++------- 2 files changed, 37 insertions(+), 7 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index ba61583331..7972b6b413 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -457,6 +457,16 @@ static int logicConditionGetWaypointOperandValue(int operand) { return NAV_Status.activeWpAction; break; + case LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION: + { + uint8_t wpIndex = posControl.activeWaypointIndex + 1; + if ((wpIndex > 0) && (wpIndex < NAV_MAX_WAYPOINTS)) { + return posControl.waypointList[wpIndex].action; + } + return false; + } + break; + case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE: { uint32_t distance = 0; @@ -509,6 +519,22 @@ static int logicConditionGetWaypointOperandValue(int operand) { return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; break; + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP: + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP: + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP: + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0; + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP: + return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; + break; + default: return 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index c43400904e..4c67906d3f 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -153,13 +153,17 @@ typedef enum { LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 0 LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 1 LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 2 - LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE, // 3 - LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT, // 4 - LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION, // 5 - LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION, // 6 - LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION, // 7 - LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION, // 8 - + LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION, // 3 + LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE, // 4 + LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT, // 5 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION, // 6 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION, // 7 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION, // 8 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION, // 9 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP, // 10 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP, // 11 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP, // 12 + LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP, // 13 } logicWaypointOperands_e; typedef enum { From 9aa6e2fb824ff0129598602d2a827f2d887fda7c Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 5 Jan 2023 22:20:05 +0000 Subject: [PATCH 11/11] Changes after testing and docs update --- docs/Programming Framework.md | 17 +++++++++++------ src/main/programming/logic_condition.c | 8 ++++---- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 51c497d3b6..0eec2cf9c4 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -164,12 +164,17 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 0 | Is WP | boolean `0`/`1` | | 1 | Current Waypoint Index | Current waypoint leg. Indexed from `1`. To verify WP is in progress, use `Is WP` | | 2 | Current Waypoint Action | Action active in current leg. See ACTIVE_WAYPOINT_ACTION table | -| 3 | Distance to next Waypoint | Distance to next WP in metres | -| 4 | Distance from Waypoint | Distance from the last WP in metres | -| 5 | User Action 1 | User Action 1 is active on this waypoint leg [boolean `0`/`1`] | -| 6 | User Action 2 | User Action 2 is active on this waypoint leg [boolean `0`/`1`] | -| 7 | User Action 3 | User Action 3 is active on this waypoint leg [boolean `0`/`1`] | -| 8 | User Action 4 | User Action 4 is active on this waypoint leg [boolean `0`/`1`] | +| 3 | Next Waypoint Action | Action active in next leg. See ACTIVE_WAYPOINT_ACTION table | +| 4 | Distance to next Waypoint | Distance to next WP in metres | +| 5 | Distance from Waypoint | Distance from the last WP in metres | +| 6 | User Action 1 | User Action 1 is active on this waypoint leg [boolean `0`/`1`] | +| 7 | User Action 2 | User Action 2 is active on this waypoint leg [boolean `0`/`1`] | +| 8 | User Action 3 | User Action 3 is active on this waypoint leg [boolean `0`/`1`] | +| 9 | User Action 4 | User Action 4 is active on this waypoint leg [boolean `0`/`1`] | +| 10 | Next Waypoint User Action 1 | User Action 1 is active on the next waypoint leg [boolean `0`/`1`] | +| 11 | Next Waypoint User Action 2 | User Action 2 is active on the next waypoint leg [boolean `0`/`1`] | +| 12 | Next Waypoint User Action 3 | User Action 3 is active on the next waypoint leg [boolean `0`/`1`] | +| 13 | Next Waypoint User Action 4 | User Action 4 is active on the next waypoint leg [boolean `0`/`1`] | #### ACTIVE_WAYPOINT_ACTION diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d127f74beb..ea5f426a38 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -589,19 +589,19 @@ static int logicConditionGetWaypointOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP: - return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1); break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP: - return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0; + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2); break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP: - return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0; + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3); break; case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP: - return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4); break; default: