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:
parent
775a080768
commit
7115d03276
2 changed files with 22 additions and 3 deletions
|
@ -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;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue