diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 9f8352c26b..0eec2cf9c4 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -125,37 +125,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 @@ -173,6 +157,37 @@ 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 | 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 + +| 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/navigation/navigation.c b/src/main/navigation/navigation.c index ba5a623b6d..08d19abf78 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1934,7 +1934,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 b450ae008d..b55bc15a4d 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -471,6 +471,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 60303a51fd..ff50a317d7 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -511,6 +511,105 @@ 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; + + 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; + if (navGetCurrentStateFlags() & NAV_AUTO_WP) { + fpVector3_t poi; + gpsLocation_t wp; + 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; + } + + return distance; + } + break; + + case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT: + { + uint32_t distance = 0; + if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) { + fpVector3_t poi; + gpsLocation_t wp; + 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; + } + + 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; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: + 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.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.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 ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER1) == NAV_WP_USER1); + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP: + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER2) == NAV_WP_USER2); + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP: + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER3) == NAV_WP_USER3); + break; + + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP: + return ((posControl.waypointList[NAV_Status.activeWpIndex].p3 & NAV_WP_USER4) == NAV_WP_USER4); + break; + + default: + return 0; + break; + } +} + static int logicConditionGetFlightOperandValue(int operand) { switch (operand) { @@ -624,10 +723,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; @@ -648,14 +743,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; @@ -808,6 +895,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 6dfb3c6019..1aaae7da0a 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -92,6 +92,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; @@ -119,24 +120,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 + 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 { @@ -156,6 +154,23 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13 } logicFlightModeOperands_e; +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_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 { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),