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

Merge branch 'master' of https://github.com/dragnea/inav into dragnea_autolaunch_refactor

Browse source
This commit is contained in:
root 2020-10-17 13:47:12 +03:00
commit 91e69c9614
579 changed files with 17653 additions and 10892 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -18,7 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "platform.h"
@ -74,6 +74,13 @@ uint32_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home point in degrees
radar_pois_t radar_pois[RADAR_MAX_POIS];
#if defined(USE_SAFE_HOME)
int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
uint32_t safehome_distance; // distance to the selected safehome
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
#endif
#if defined(USE_NAV)
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
@ -115,8 +122,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_home_altitude = 0, // altitude in centimeters
.rth_abort_threshold = 50000, // centimeters - 500m should be safe for all aircraft
.max_terrain_follow_altitude = 100, // max altitude in centimeters in terrain following mode
.rth_home_offset_distance = 0, // Distance offset from GPS established home to "safe" position used for RTH (cm, 0 disables)
.rth_home_offset_direction = 0, // Direction offset from GPS established home to "safe" position used for RTH (degrees, 0=N, 90=E, 180=S, 270=W, requires non-zero offset distance)
},
// MC-specific
@ -2404,6 +2409,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
return flags;
}
#if defined(USE_SAFE_HOME)
/*******************************************************
* Is a safehome being used instead of the arming point?
*******************************************************/
bool isSafeHomeInUse(void)
{
return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
}
/***********************************************************
* See if there are any safehomes near where we are arming.
* If so, use it instead of the arming point for home.
**********************************************************/
bool foundNearbySafeHome(void)
{
safehome_used = -1;
fpVector3_t safeHome;
gpsLocation_t shLLH;
shLLH.alt = 0;
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
shLLH.lat = safeHomeConfig(i)->lat;
shLLH.lon = safeHomeConfig(i)->lon;
geoConvertGeodeticToLocal(&safeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
safehome_distance = calculateDistanceToDestination(&safeHome);
if (safehome_distance < 20000) { // 200 m
safehome_used = i;
setHomePosition(&safeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
return true;
}
}
safehome_distance = 0;
return false;
}
#endif
/*-----------------------------------------------------------
* Update home position, calculate distance and bearing to home
*-----------------------------------------------------------*/
@ -2425,15 +2466,10 @@ void updateHomePosition(void)
break;
}
if (setHome) {
if (navConfig()->general.rth_home_offset_distance != 0) { // apply user defined offset
fpVector3_t offsetHome;
offsetHome.x = posControl.actualState.abs.pos.x + navConfig()->general.rth_home_offset_distance * cos_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction));
offsetHome.y = posControl.actualState.abs.pos.y + navConfig()->general.rth_home_offset_distance * sin_approx(DEGREES_TO_RADIANS(navConfig()->general.rth_home_offset_direction));
offsetHome.z = posControl.actualState.abs.pos.z;
setHomePosition(&offsetHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
} else {
#if defined(USE_SAFE_HOME)
if (!foundNearbySafeHome())
#endif
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
}
}
}
}
@ -2881,6 +2917,14 @@ bool saveNonVolatileWaypointList(void)
}
#endif
#if defined(USE_SAFE_HOME)
void resetSafeHomes(void)
{
memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
}
#endif
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
{
gpsLocation_t wpLLH;
@ -3098,6 +3142,11 @@ static bool canActivateNavigationModes(void)
return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
}
static bool isWaypointMissionValid(void)
{
return posControl.waypointListValid && (posControl.waypointCount > 0);
}
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
{
static bool canActivateWaypoint = false;
@ -3111,9 +3160,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode();
bool canActivateNavigation = canActivateNavigationModes();
const bool canActivateAltHold = canActivateAltHoldMode();
const bool canActivatePosHold = canActivatePosHoldMode();
const bool canActivateNavigation = canActivateNavigationModes();
const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
// Keep canActivateWaypoint flag at FALSE if there is no mission loaded
// Also block WP mission if we are executing RTH
if (!isWaypointMissionValid() || isExecutingRTH) {
canActivateWaypoint = false;
}
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING_LEGACY)) {
@ -3140,21 +3196,32 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// RTH/Failsafe_RTH can override MANUAL
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
if (posControl.flags.forcedRTHActivated) {
// If we request forced RTH - attempt to activate it no matter what
// This might switch to emergency landing controller if GPS is unavailable
canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
// Pilot-triggered RTH, also fall-back for WP if there is no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint)) {
// Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
// If don't keep this, loss of any of the canActivatePosHold && canActivateNavigation && canActivateAltHold
// will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
// logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc)
if (isExecutingRTH || (canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
}
// MANUAL mode has priority over WP/PH/AH
if (IS_RC_MODE_ACTIVE(BOXMANUAL)) {
canActivateWaypoint = false; // Block WP mode if we are in PASSTHROUGH mode
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
else {