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

Reduced calculations on WpNumber

Browse source
This commit is contained in:
Darren Lines 2022-12-14 21:52:53 +00:00
parent bc5cc39a1b
commit fe2e151583
3 changed files with 16 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1932,7 +1932,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

@ -468,6 +468,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;

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

@ -460,12 +460,12 @@ 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) && NAV_Status.activeWpNumber > 0) { if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
fpVector3_t poi; fpVector3_t poi;
gpsLocation_t wp; gpsLocation_t wp;
wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-1].lat; wp.lat = posControl.waypointList[NAV_Status.activeWpIndex].lat;
wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-1].lon; wp.lon = posControl.waypointList[NAV_Status.activeWpIndex].lon;
wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-1].alt; wp.alt = posControl.waypointList[NAV_Status.activeWpIndex].alt;
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE); geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
distance = calculateDistanceToDestination(&poi) / 100; distance = calculateDistanceToDestination(&poi) / 100;
@ -478,12 +478,12 @@ static int logicConditionGetWaypointOperandValue(int operand) {
case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT: case LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT:
{ {
uint32_t distance = 0; uint32_t distance = 0;
if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpNumber > 1) { if ((navGetCurrentStateFlags() & NAV_AUTO_WP) && NAV_Status.activeWpIndex > 0) {
fpVector3_t poi; fpVector3_t poi;
gpsLocation_t wp; gpsLocation_t wp;
wp.lat = posControl.waypointList[NAV_Status.activeWpNumber-2].lat; wp.lat = posControl.waypointList[NAV_Status.activeWpIndex-1].lat;
wp.lon = posControl.waypointList[NAV_Status.activeWpNumber-2].lon; wp.lon = posControl.waypointList[NAV_Status.activeWpIndex-1].lon;
wp.alt = posControl.waypointList[NAV_Status.activeWpNumber-2].alt; wp.alt = posControl.waypointList[NAV_Status.activeWpIndex-1].alt;
geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE); geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp, GEO_ALT_RELATIVE);
distance = calculateDistanceToDestination(&poi) / 100; distance = calculateDistanceToDestination(&poi) / 100;
@ -494,19 +494,19 @@ static int logicConditionGetWaypointOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION:
return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0;
break; break;
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION: case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION:
return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0; return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER2) == NAV_WP_USER2) : 0;
break; break;
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION: case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION:
return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0; return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER3) == NAV_WP_USER3) : 0;
break; break;
case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION: case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION:
return (NAV_Status.activeWpNumber > 0) ? ((posControl.waypointList[NAV_Status.activeWpNumber-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0; return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER4) == NAV_WP_USER4) : 0;
break; break;
default: default: