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 remote-tracking branch 'upstream/master' into abo_acro_attitude_hold

Browse source
This commit is contained in:
breadoven 2023-10-13 10:53:43 +01:00
commit 7547d67eae
235 changed files with 3556 additions and 2022 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -47,7 +47,7 @@
#include "fc/settings.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/mixer_profile.h"
#include "flight/pid.h"
#include "io/beeper.h"
@ -69,6 +69,7 @@
#define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
#define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
#define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set
#define MR_RTH_LAND_MARGIN_CM 2000 // pause landing if this amount of cm *before* remaining to the home point (2D distance)
// Planes:
#define FW_RTH_CLIMB_OVERSHOOT_CM 100
@ -308,6 +309,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState);
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
/** Idle state ******************************************************/
@ -328,6 +332,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
}
},
@ -596,6 +601,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
}
},
@ -654,6 +660,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
}
},
@ -807,6 +814,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
}
},
@ -941,6 +949,52 @@ 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,
.onEntry = navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT,
.mapToFlightModes = NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_MIXERAT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_MIXERAT_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT,
.mapToFlightModes = NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT,
[NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state
}
},
[NAV_STATE_MIXERAT_ABORT] = {
.persistentId = NAV_PERSISTENT_ID_MIXERAT_ABORT,
.onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, //will not switch to its pending state
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
.mapToFlightModes = NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
};
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
@ -1366,6 +1420,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
}
if (navConfig()->general.flags.rth_use_linear_descent && navConfig()->general.rth_home_altitude > 0) {
// Check linear descent status
uint32_t homeDistance = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
@ -1456,12 +1514,22 @@ 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;
}
float descentVelLimited = 0;
int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint;
uint32_t remaning_distance = calculateDistanceToDestination(&tmpHomePos);
int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
if(STATE(MULTIROTOR) && (remaning_distance>MR_RTH_LAND_MARGIN_CM)){
descentVelLimited = navConfig()->general.land_minalt_vspd;
}
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
// Do not allow descent velocity slower than -30cm/s so the landing detector works.
descentVelLimited = navConfig()->general.land_minalt_vspd;
@ -1854,6 +1922,70 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi
return NAV_FSM_EVENT_NONE;
}
navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE;
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState)
{
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
// Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed
if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP)) {
resetAltitudeController(false);
setupAltitudeController();
}
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
navMixerATPendingState = previousState;
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState)
{
UNUSED(previousState);
mixerProfileATRequest_e required_action;
switch (navMixerATPendingState)
{
case NAV_STATE_RTH_HEAD_HOME:
required_action = MIXERAT_REQUEST_RTH;
break;
case NAV_STATE_RTH_LANDING:
required_action = MIXERAT_REQUEST_LAND;
break;
default:
required_action = MIXERAT_REQUEST_NONE;
break;
}
if (mixerATUpdateState(required_action)){
// MixerAT is done, switch to next state
resetPositionController();
resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL
mixerATUpdateState(MIXERAT_REQUEST_ABORT);
switch (navMixerATPendingState)
{
case NAV_STATE_RTH_HEAD_HOME:
setupAltitudeController();
return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME;
break;
case NAV_STATE_RTH_LANDING:
setupAltitudeController();
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;
break;
default:
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
break;
}
}
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState)
{
UNUSED(previousState);
mixerATUpdateState(MIXERAT_REQUEST_ABORT);
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
{
navigationFSMState_t previousState;
@ -2747,6 +2879,9 @@ static void updateNavigationFlightStatistics(void)
}
}
/*
* Total travel distance in cm
*/
uint32_t getTotalTravelDistance(void)
{
return lrintf(posControl.totalTripDistance);
@ -3782,14 +3917,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// CRUISE has priority over COURSE_HOLD and AH
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) {
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)) {
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) {
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;
}
@ -3803,8 +3938,7 @@ 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)
@ -3917,13 +4051,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){
@ -4203,15 +4337,6 @@ void navigationInit(void)
/* Use system config */
navigationUsePIDs();
if (
mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER ||
navConfig()->fw.useFwNavYawControl
) {
ENABLE_STATE(FW_HEADING_USE_YAW);
} else {
DISABLE_STATE(FW_HEADING_USE_YAW);
}
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
/* configure WP missions at boot */
#ifdef USE_MULTI_MISSION
@ -4272,7 +4397,7 @@ void abortForcedRTH(void)
rthState_e getStateOfForcedRTH(void)
{
/* If forced RTH activated and in AUTO_RTH or EMERG state */
if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) {
if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) {
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
return RTH_HAS_LANDED;
}
@ -4367,7 +4492,7 @@ bool navigationRTHAllowsLanding(void)
bool isNavLaunchEnabled(void)
{
return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE);
}
bool abortLaunchAllowed(void)