1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 06:15:14 +03:00

add distance to WP

needs testing ... and compiling even. Coding at work is blind xD
This commit is contained in:
Darren Lines 2022-11-23 13:35:18 +00:00
parent 775a080768
commit 7115d03276
2 changed files with 22 additions and 3 deletions

View file

@ -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;