Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 19:40:27 +03:00
Code Issues Wiki Activity

Merge pull request #8579 from iNavFlight/MrD_Enhance-programming-options-for-waypoint-missions

Browse source 
Enhance programming options for waypoint missions
This commit is contained in:
Paweł Spychalski 2023-01-13 08:21:12 +01:00 • committed by GitHub
commit c57e112ecc
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 186 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

77
docs/Programming Framework.md
View file

@ -125,37 +125,21 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` | | 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` | | 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` | | 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_WP | boolean `0`/`1` | | 23 | IS_LANDING | boolean `0`/`1` |
| 24 | IS_LANDING | boolean `0`/`1` | | 24 | IS_FAILSAFE | boolean `0`/`1` |
| 25 | IS_FAILSAFE | boolean `0`/`1` | | 25 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` |
| 26 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` | | 26 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` |
| 27 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` | | 27 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` |
| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | | 28 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` | | 29 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 30 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | | 31 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | | 32 | LOITER_RADIUS | The current loiter radius in cm. |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | | 33 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | | 34 | BATT_CELLS | Number of battery cells detected |
| 35 | LOITER_RADIUS | The current loiter radius in cm. | | 35 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | | 36 | AGL | integer Above The Groud Altitude in `cm` |
| 37 | BATT_CELLS | Number of battery cells detected | | 37 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` |
| 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 |
#### FLIGHT_MODE #### FLIGHT_MODE
@ -173,6 +157,37 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 9 | USER1 | | | 9 | USER1 | |
| 10 | USER2 | | | 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 ### Flags

4
src/main/navigation/navigation.c
View file

@ -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.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE; 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; NAV_Status.activeWpAction = 0;
if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action; NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;

1
src/main/navigation/navigation.h
View file

@ -471,6 +471,7 @@ typedef struct {
navSystemStatus_Error_e error; navSystemStatus_Error_e error;
navSystemStatus_Flags_e flags; navSystemStatus_Flags_e flags;
uint8_t activeWpNumber; uint8_t activeWpNumber;
uint8_t activeWpIndex;
navWaypointActions_e activeWpAction; navWaypointActions_e activeWpAction;
} navSystemStatus_t; } navSystemStatus_t;

115
src/main/programming/logic_condition.c
View file

@ -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) { static int logicConditionGetFlightOperandValue(int operand) {
switch (operand) { switch (operand) {
@ -624,10 +723,6 @@ static int logicConditionGetFlightOperandValue(int operand) {
return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0;
break; 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 case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0; return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
break; break;
@ -648,14 +743,6 @@ static int logicConditionGetFlightOperandValue(int operand) {
return axisPID[PITCH]; return axisPID[PITCH];
break; 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 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); return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT16_MAX);
break; break;
@ -808,6 +895,10 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
retVal = programmingPidGetOutput(operand); retVal = programmingPidGetOutput(operand);
} }
break; break;
case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS:
retVal = logicConditionGetWaypointOperandValue(operand);
break;
default: default:
break; break;

51
src/main/programming/logic_condition.h
View file

@ -92,6 +92,7 @@ typedef enum logicOperandType_s {
LOGIC_CONDITION_OPERAND_TYPE_LC, // Result of different LC and LC operand 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_GVAR, // Value from a global variable
LOGIC_CONDITION_OPERAND_TYPE_PID, // Value from a Programming PID LOGIC_CONDITION_OPERAND_TYPE_PID, // Value from a Programming PID
LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS,
LOGIC_CONDITION_OPERAND_TYPE_LAST LOGIC_CONDITION_OPERAND_TYPE_LAST
} logicOperandType_e; } logicOperandType_e;
@ -119,24 +120,21 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL, // 0/1 // 20 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_EMERGENCY_LANDING, // 0/1 // 21
LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH, // 0/1 // 22 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 // 23
LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1 // 24 LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 24
LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1 // 25 LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL, // 25
LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL, // 26 LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26
LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 27 LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27
LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 28 LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28
LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_INDEX, // 29 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29
LOGIC_CONDITION_OPERAND_FLIGHT_WAYPOINT_ACTION, // 30 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39
LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 34
LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 35 LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 35
LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36 LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 36
LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 37 LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 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
} logicFlightOperands_e; } logicFlightOperands_e;
typedef enum { typedef enum {
@ -156,6 +154,23 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13
} logicFlightModeOperands_e; } 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 { typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY = (1 << 0),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE = (1 << 1),