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' of https://github.com/iNavFlight/inav into submit-gps-fix-estimation

Browse source
This commit is contained in:
Roman Lut 2023-10-27 12:04:17 +02:00
commit a08e8c4285
218 changed files with 3194 additions and 1437 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -62,6 +62,7 @@
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#include "programming/global_variables.h"
@ -223,6 +224,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;
@ -949,7 +951,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
/** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/
[NAV_STATE_MIXERAT_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE,
@ -992,7 +994,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
};
@ -1514,7 +1516,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
}
@ -2671,11 +2673,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;
@ -2686,24 +2692,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;
}
}
@ -2718,6 +2716,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;
}
}
@ -2890,6 +2904,9 @@ static void updateNavigationFlightStatistics(void)
}
}
/*
* Total travel distance in cm
*/
uint32_t getTotalTravelDistance(void)
{
return lrintf(posControl.totalTripDistance);
@ -2960,14 +2977,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);
}
@ -3004,11 +3022,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
*-----------------------------------------------------------*/
@ -3927,14 +3962,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// CRUISE has priority over COURSE_HOLD and AH
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) {
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
}
// PH has priority over COURSE_HOLD
// CRUISE has priority on AH
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) {
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
}
@ -3948,12 +3983,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
}
}
else {
} 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;
@ -4062,13 +4096,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
}
/*
* Don't allow arming if any of JUMP waypoint has invalid settings
* First WP can't be JUMP
* Can't jump to immediately adjacent WPs (pointless)
* Can't jump beyond WP list
* Only jump to geo-referenced WP types
*/
/*
* Don't allow arming if any of JUMP waypoint has invalid settings
* First WP can't be JUMP
* Can't jump to immediately adjacent WPs (pointless)
* Can't jump beyond WP list
* Only jump to geo-referenced WP types
*/
if (posControl.waypointCount) {
for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){