Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-15 20:35:29 +03:00
Code Issues Wiki Activity

fixed initial code and added more

Browse source 
Now working:
- Distance to WP
- Distance from WP
- User actions 1 to 4
This commit is contained in:
Darren Lines 2022-11-23 22:39:22 +00:00
parent 7115d03276
commit b2ad70329b
2 changed files with 50 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -460,21 +460,55 @@ static int logicConditionGetWaypointOperandValue(int operand) {
case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE: case LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE:
{ {
uint32_t distance = 0; uint32_t distance = 0;
if ((navGetCurrentStateFlags() & NAV_AUTO_WP)) { if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
fpVector3_t poi; fpVector3_t poi;
gpsLocation_t wp; gpsLocation_t wp;
wp.lat = posControl.waypointList[NAV_Status.activeWpNumber].lat; wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-1].lat;
wp.lon = posControl.waypointList[NAV_Status.activeWpNumber].lon; wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-1].lon;
wp.alt = posControl.waypointList[NAV_Status.activeWpNumber].alt; wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-1].alt;
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, waypointMissionAltConvMode(posControl.waypointList[NAV_Status.activeWpNumber].p3)); geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
distance = calculateDistanceToDestination(&poi); distance = calculateDistanceToDestination(&poi) / 100;
} }
return distance; return distance;
} }
break; 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: default:
return 0; return 0;
break; break;

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

@ -150,10 +150,16 @@ typedef enum {
} logicFlightModeOperands_e; } logicFlightModeOperands_e;
typedef enum { typedef enum {
LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 1 LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP, // 0/1 // 0
LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 2 LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX, // 1
LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 3 LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION, // 2
LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE, // 4 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; } logicWaypointOperands_e;
typedef enum { typedef enum {