Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 11:29:56 +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

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) {
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;