Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

NAV: Allow position control from EzGUI (follow me etc) only if new 'GCS NAV' mode is enabled

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2016-03-08 19:35:20 +02:00
parent 2cd83a3110
commit fbeff0a1be
4 changed files with 17 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/flight/navigation_rewrite.c
View file

@ -1738,8 +1738,8 @@ void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
} }
// WP #255 - special waypoint - directly set desiredPosition // WP #255 - special waypoint - directly set desiredPosition
// Only valid when armed and in poshold mode // Only valid when armed and in poshold mode
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { (posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
// Convert to local coordinates // Convert to local coordinates
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
@ -2058,6 +2058,11 @@ static void updateReadyStatus(void)
} }
} }
void updateFlightBehaviorModifiers(void)
{
posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
}
/** /**
* Process NAV mode transition and WP/RTH state machine * Process NAV mode transition and WP/RTH state machine
* Update rate: RX (data driven or 50Hz) * Update rate: RX (data driven or 50Hz)
@ -2070,6 +2075,9 @@ void updateWaypointsAndNavigationMode(void)
/* Update NAV ready status */ /* Update NAV ready status */
updateReadyStatus(); updateReadyStatus();
// Update flight behaviour modifiers
updateFlightBehaviorModifiers();
// Process switch to a different navigation mode (if needed) // Process switch to a different navigation mode (if needed)
navProcessFSMEvents(selectNavEventFromBoxModeInput()); navProcessFSMEvents(selectNavEventFromBoxModeInput());

3
src/main/flight/navigation_rewrite_private.h
View file

@ -59,6 +59,9 @@ typedef struct navigationFlags_s {
bool isAdjustingAltitude; bool isAdjustingAltitude;
bool isAdjustingHeading; bool isAdjustingHeading;
// Behaviour modifiers
bool isGCSAssistedNavigationEnabled; // Does iNav accept WP#255 - follow-me etc.
bool forcedRTHActivated; bool forcedRTHActivated;
} navigationFlags_t; } navigationFlags_t;

2
src/main/io/rc_controls.h
View file

@ -37,7 +37,7 @@ typedef enum {
BOXLEDMAX, BOXLEDMAX,
BOXLEDLOW, BOXLEDLOW,
BOXLLIGHTS, BOXLLIGHTS,
//BOXCALIB, BOXGCSNAV,
BOXGOV, BOXGOV,
BOXOSD, BOXOSD,
BOXTELEMETRY, BOXTELEMETRY,

3
src/main/io/serial_msp.c
View file

@ -365,6 +365,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXNAVWP, "NAV WP;", 28 }, { BOXNAVWP, "NAV WP;", 28 },
{ BOXAIRMODE, "AIR MODE;", 29 }, { BOXAIRMODE, "AIR MODE;", 29 },
{ BOXHOMERESET, "HOME RESET;", 30 }, { BOXHOMERESET, "HOME RESET;", 30 },
{ BOXGCSNAV, "GCS NAV;", 31 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF } { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
}; };
@ -670,6 +671,7 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
} }
#endif #endif
@ -749,6 +751,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)) << BOXNAVRTH | IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)) << BOXNAVRTH |
IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)) << BOXNAVWP | IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)) << BOXNAVWP |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)) << BOXGCSNAV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET; IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
for (i = 0; i < activeBoxIdCount; i++) { for (i = 0; i < activeBoxIdCount; i++) {