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

Merge branch 'master' into TrackBackToHeader

Browse source
This commit is contained in:
Paweł Spychalski 2023-12-26 11:55:44 +01:00 • committed by GitHub
commit 318d298337
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
381 changed files with 6868 additions and 2422 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -63,6 +63,7 @@
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#include "programming/global_variables.h"
@ -97,7 +98,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -132,6 +133,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
#endif
.waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
.auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
.min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s)
.max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
@ -155,6 +157,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
},
// MC-specific
@ -224,6 +227,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
static navWapointHeading_t wpHeadingControl;
navigationPosControl_t posControl;
navSystemStatus_t NAV_Status;
static bool landingDetectorIsActive;
EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
@ -1219,6 +1223,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
{
UNUSED(previousState);
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
// Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
@ -1391,6 +1398,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) {
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
posControl.rthState.rthLinearDescentActive = true;
}
}
@ -1401,6 +1409,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (isWaypointReached(tmpHomePos, 0)) {
// Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
posControl.landingDelay = 0;
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
@ -1429,9 +1443,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// If position ok OR within valid timeout - continue
// Action delay before landing if in FS and option enabled
bool pauseLanding = false;
navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
if ((allow == NAV_RTH_ALLOW_LANDING_ALWAYS || allow == NAV_RTH_ALLOW_LANDING_FS_ONLY) && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) {
if (posControl.landingDelay == 0)
posControl.landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay);
batteryState_e batteryState = getBatteryState();
if (millis() < posControl.landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL)
pauseLanding = true;
else
posControl.landingDelay = 0;
}
// If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) {
resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
@ -2464,8 +2493,19 @@ bool validateRTHSanityChecker(void)
return true;
}
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
//schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true;
}
#endif
// Check at 10Hz rate
if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
@ -2621,11 +2661,15 @@ bool findNearestSafeHome(void)
*-----------------------------------------------------------*/
void updateHomePosition(void)
{
// Disarmed and have a valid position, constantly update home
// Disarmed and have a valid position, constantly update home before first arm (depending on setting)
// Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm)
static bool setHome = false;
navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING;
if (!ARMING_FLAG(ARMED)) {
if (posControl.flags.estPosStatus >= EST_USABLE) {
const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
case NAV_RESET_NEVER:
break;
@ -2636,24 +2680,16 @@ void updateHomePosition(void)
setHome = true;
break;
}
if (setHome) {
#if defined(USE_SAFE_HOME)
findNearestSafeHome();
#endif
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
// save the current location in case it is replaced by a safehome or HOME_RESET
posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
}
}
}
else {
static bool isHomeResetAllowed = false;
// If pilot so desires he may reset home position to current position
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
homeUpdateFlags = 0;
homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setHome = true;
isHomeResetAllowed = false;
}
}
@ -2668,6 +2704,22 @@ void updateHomePosition(void)
posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
updateHomePositionCompatibility();
}
setHome &= !STATE(IN_FLIGHT_EMERG_REARM); // prevent reset following emerg in flight rearm
}
if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) {
#if defined(USE_SAFE_HOME)
findNearestSafeHome();
#endif
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) {
posControl.rthState.homePosition.pos.z = 0; // force to 0 if reference altitude also reset every arm
}
// save the current location in case it is replaced by a safehome or HOME_RESET
posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
setHome = false;
}
}
@ -2810,14 +2862,15 @@ void updateLandingStatus(timeMs_t currentTimeMs)
}
lastUpdateTimeMs = currentTimeMs;
static bool landingDetectorIsActive;
DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
if (!ARMING_FLAG(ARMED)) {
if (STATE(LANDING_DETECTED)) {
landingDetectorIsActive = false;
}
resetLandingDetector();
landingDetectorIsActive = false;
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
}
@ -2854,11 +2907,28 @@ void resetLandingDetector(void)
posControl.flags.resetLandingDetector = true;
}
void resetLandingDetectorActiveState(void)
{
landingDetectorIsActive = false;
}
bool isFlightDetected(void)
{
return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
}
bool isProbablyStillFlying(void)
{
bool inFlightSanityCheck;
if (STATE(MULTIROTOR)) {
inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
} else {
inFlightSanityCheck = isGPSHeadingValid();
}
return landingDetectorIsActive && inFlightSanityCheck;
}
/*-----------------------------------------------------------
* Z-position controller
*-----------------------------------------------------------*/
@ -3637,7 +3707,6 @@ void checkManualEmergencyLandingControl(bool forcedActivation)
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
{
static bool canActivateWaypoint = false;
static bool canActivateLaunchMode = false;
//We can switch modes only when ARMED
@ -3685,10 +3754,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
posControl.rthSanityChecker.rthSanityOK = true;
// Keep canActivateWaypoint flag at FALSE if there is no mission loaded
// Also block WP mission if we are executing RTH
if (!isWaypointMissionValid() || isExecutingRTH) {
/* WP mission activation control:
* canActivateWaypoint & waypointWasActivated are used to prevent WP mission
* auto restarting after interruption by Manual or RTH modes.
* WP mode must be deselected before it can be reactivated again. */
static bool waypointWasActivated = false;
const bool isWpMissionLoaded = isWaypointMissionValid();
bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner
if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
canActivateWaypoint = false;
if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
canActivateWaypoint = true;
waypointWasActivated = false;
}
}
/* Airplane specific modes */
@ -3718,30 +3797,26 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
/* Soaring mode, disables altitude control in Position hold and Course hold modes.
* Pitch allowed to freefloat within defined Angle mode deadband */
if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
if (!FLIGHT_MODE(SOARING_MODE)) {
ENABLE_FLIGHT_MODE(SOARING_MODE);
}
ENABLE_FLIGHT_MODE(SOARING_MODE);
} else {
DISABLE_FLIGHT_MODE(SOARING_MODE);
}
}
// Failsafe_RTH (can override MANUAL)
/* If we request forced RTH - attempt to activate it no matter what
* This might switch to emergency landing controller if GPS is unavailable */
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
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
/* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
* Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
* Also prevent WP falling back to RTH if WP mission planner is active */
const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive;
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) {
/* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded.
* WP prevented from falling back to RTH if WP mission planner is active */
const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive;
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) {
// 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 canActivateNavigation && canActivateAltHold
// Without this loss of any of the 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)
// logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc)
if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
@ -3749,24 +3824,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// 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
// Block activation if using WP Mission Planner
// Also check multimission mission change status before activating WP mode
// Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded.
// Also check multimission mission change status before activating WP mode.
#ifdef USE_MULTI_MISSION
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
#else
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
#endif
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
waypointWasActivated = true;
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
else {
// Arm the state variable if the WP BOX mode is not enabled
canActivateWaypoint = true;
}
}
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
@ -3797,10 +3868,8 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
}
} else {
canActivateWaypoint = false;
// Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight)
canActivateLaunchMode = isNavLaunchEnabled();
canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid()));
}
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@ -3879,7 +3948,8 @@ bool navigationPositionEstimateIsHealthy(void)
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) ||
IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
if (usedBypass) {
*usedBypass = false;
@ -3967,7 +4037,11 @@ void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
);
posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
@ -4391,3 +4465,26 @@ int32_t navigationGetHeadingError(void)
{
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
}
int8_t navCheckActiveAngleHoldAxis(void)
{
int8_t activeAxis = -1;
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
activeAxis = FD_PITCH;
} else if (altholdActive) {
activeAxis = FD_ROLL;
}
}
return activeAxis;
}
uint8_t getActiveWpNumber(void)
{
return NAV_Status.activeWpNumber;
}