mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 06:45:14 +03:00
Remove POSHOLD_2D mode (POSHOLD now implies ALTHOLD); Initial cut on surface tracking modifier
Direct altitude control
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
5227273833
commit
73f773f23e
11 changed files with 231 additions and 263 deletions
|
|
@ -108,6 +108,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.min_rth_distance = 500, // If closer than 5m - land immediately
|
||||
.rth_altitude = 1000, // 10m
|
||||
.rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
|
||||
.max_terrain_follow_altitude = 100, // max 1m altitude in terrain following mode
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
|
@ -158,7 +159,6 @@ int16_t navActualHeading;
|
|||
int16_t navDesiredHeading;
|
||||
int16_t navTargetPosition[3];
|
||||
int32_t navLatestActualPosition[3];
|
||||
int16_t navTargetSurface;
|
||||
int16_t navActualSurface;
|
||||
uint16_t navFlags;
|
||||
uint16_t navEPH;
|
||||
|
|
@ -167,10 +167,10 @@ int16_t navAccNEU[3];
|
|||
#endif
|
||||
|
||||
static void updateDesiredRTHAltitude(void);
|
||||
static void resetAltitudeController(void);
|
||||
static void resetAltitudeController(bool useTerrainFollowing);
|
||||
static void resetPositionController(void);
|
||||
static void setupAltitudeController(void);
|
||||
void resetNavigation(void);
|
||||
static void resetHeadingController(void);
|
||||
void resetGCSFlags(void);
|
||||
|
||||
static bool posEstimationHasGlobalReference(void);
|
||||
|
|
@ -186,8 +186,6 @@ bool validateRTHSanityChecker(void);
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
|
||||
|
|
@ -222,7 +220,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
|
|
@ -256,40 +253,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
/** POSHOLD_2D mode ************************************************/
|
||||
[NAV_STATE_POSHOLD_2D_INITIALIZE] = {
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_POS | NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_POSHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_INFINIT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_2D_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_POSHOLD_2D_IN_PROGRESS] = {
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_POSHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_INFINIT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_2D_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[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_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
|
|
@ -323,7 +286,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -359,7 +321,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
|
@ -376,7 +337,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -394,7 +354,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -412,7 +371,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -442,7 +400,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_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,
|
||||
}
|
||||
|
|
@ -491,7 +448,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -512,7 +468,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -531,7 +486,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -561,7 +515,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
|
@ -672,27 +625,39 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void)
|
|||
return navGetStateFlags(posControl.navState);
|
||||
}
|
||||
|
||||
static bool navTerrainFollowingRequested(void)
|
||||
{
|
||||
// Terrain following not supported on FIXED WING aircraft yet
|
||||
return !STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXSURFACE);
|
||||
}
|
||||
|
||||
/*************************************************************************************************/
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
resetNavigation();
|
||||
|
||||
resetAltitudeController(false);
|
||||
resetHeadingController();
|
||||
resetPositionController();
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
|
||||
|
||||
resetGCSFlags();
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0) {
|
||||
resetAltitudeController();
|
||||
// If surface tracking mode changed value - reset altitude controller
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
|
||||
resetAltitudeController(navTerrainFollowingRequested());
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
|
||||
setupAltitudeController();
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
|
|
@ -704,41 +669,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav
|
|||
|
||||
// If GCS was disabled - reset altitude setpoint
|
||||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
resetGCSFlags();
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
resetGCSFlags();
|
||||
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
// If GCS was disabled - reset 2D pos setpoint
|
||||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
resetGCSFlags();
|
||||
}
|
||||
|
||||
|
|
@ -748,6 +679,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
const bool terrainFollowingToggled = (posControl.flags.isTerrainFollowEnabled != navTerrainFollowingRequested());
|
||||
|
||||
resetGCSFlags();
|
||||
|
||||
|
|
@ -755,13 +687,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
|
|||
resetPositionController();
|
||||
}
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0) {
|
||||
resetAltitudeController();
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || terrainFollowingToggled) {
|
||||
resetAltitudeController(navTerrainFollowingRequested());
|
||||
setupAltitudeController();
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0) || terrainFollowingToggled) {
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
|
||||
}
|
||||
|
||||
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
|
||||
|
|
@ -781,7 +713,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
if (posControl.flags.isGCSAssistedNavigationReset) {
|
||||
fpVector3_t targetHoldPos;
|
||||
calculateInitialHoldPosition(&targetHoldPos);
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
|
||||
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
resetGCSFlags();
|
||||
}
|
||||
|
|
@ -805,21 +737,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
}
|
||||
|
||||
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
// Reset altitude and position controllers if necessary
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0) {
|
||||
resetAltitudeController();
|
||||
// Reset altitude controller if it was not enabled or if we are in terrain follow mode
|
||||
if ((prevFlags & NAV_CTL_ALT) == 0 || posControl.flags.isTerrainFollowEnabled) {
|
||||
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
}
|
||||
|
||||
// If close to home - reset home position and land
|
||||
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
|
||||
}
|
||||
|
|
@ -862,15 +796,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
}
|
||||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const float rthAltitudeMargin = STATE(FIXED_WING) ?
|
||||
MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane
|
||||
MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters
|
||||
|
||||
if (((posControl.actualState.pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
|
||||
if (STATE(FIXED_WING)) {
|
||||
initializeRTHSanityChecker(&posControl.actualState.pos);
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
|
||||
|
|
@ -922,7 +856,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
}
|
||||
|
||||
// If we have position sensor - continue home
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
|
||||
// Successfully reached position target - update XYZ-position
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -962,7 +896,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
// Wait until target heading is reached (with 15 deg margin for error)
|
||||
if (STATE(FIXED_WING)) {
|
||||
resetLandingDetector();
|
||||
|
|
@ -1008,14 +942,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
float descentVelLimited = 0;
|
||||
|
||||
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface < 50.0f) {
|
||||
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 = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
}
|
||||
else {
|
||||
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
|
||||
float descentVelScaling = (posControl.actualState.pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
|
||||
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
|
||||
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
|
||||
|
||||
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
|
||||
|
|
@ -1060,7 +994,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
else {
|
||||
// Prepare controllers
|
||||
resetPositionController();
|
||||
resetAltitudeController();
|
||||
|
||||
// Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
|
||||
resetAltitudeController(false);
|
||||
setupAltitudeController();
|
||||
|
||||
posControl.activeWaypointIndex = 0;
|
||||
|
|
@ -1080,7 +1016,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
|
||||
case NAV_WP_ACTION_RTH:
|
||||
default:
|
||||
initializeRTHSanityChecker(&posControl.actualState.pos);
|
||||
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
|
||||
calcualteAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos);
|
||||
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
|
||||
};
|
||||
|
|
@ -1091,7 +1027,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
|
||||
|
||||
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||
|
|
@ -1175,7 +1111,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
|
||||
|
|
@ -1193,7 +1129,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
|
|||
UNUSED(previousState);
|
||||
|
||||
// Emergency landing MAY use common altitude controller if vertical position is valid - initialize it
|
||||
resetAltitudeController();
|
||||
// Make sure terrain following is not enabled
|
||||
resetAltitudeController(false);
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
|
@ -1454,7 +1391,7 @@ bool isThrustFacingDownwards(void)
|
|||
bool checkForPositionSensorTimeout(void)
|
||||
{
|
||||
if (navConfig()->general.pos_failure_timeout) {
|
||||
if ((posControl.flags.estPosStatue == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
if ((posControl.flags.estPosStatus == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
|
@ -1472,20 +1409,25 @@ bool checkForPositionSensorTimeout(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
|
||||
{
|
||||
posControl.actualState.pos.x = newX;
|
||||
posControl.actualState.pos.y = newY;
|
||||
posControl.actualState.abs.pos.x = newX;
|
||||
posControl.actualState.abs.pos.y = newY;
|
||||
posControl.actualState.abs.vel.x = newVelX;
|
||||
posControl.actualState.abs.vel.y = newVelY;
|
||||
|
||||
posControl.actualState.agl.pos.x = newX;
|
||||
posControl.actualState.agl.pos.y = newY;
|
||||
posControl.actualState.agl.vel.x = newVelX;
|
||||
posControl.actualState.agl.vel.y = newVelY;
|
||||
|
||||
posControl.actualState.vel.x = newVelX;
|
||||
posControl.actualState.vel.y = newVelY;
|
||||
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
|
||||
|
||||
if (estimateValid) {
|
||||
posControl.flags.estPosStatue = EST_TRUSTED;
|
||||
posControl.flags.estPosStatus = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
|
|
@ -1502,40 +1444,41 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
|
|||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
|
||||
{
|
||||
posControl.actualState.pos.z = newAltitude;
|
||||
posControl.actualState.vel.z = newVelocity;
|
||||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
posControl.actualState.abs.pos.z = newAltitude;
|
||||
posControl.actualState.abs.vel.z = newVelocity;
|
||||
|
||||
posControl.actualState.agl.pos.z = surfaceDistance;
|
||||
posControl.actualState.agl.vel.z = surfaceVelocity;
|
||||
|
||||
// Update altitude that would be used when executing RTH
|
||||
if (estimateValid) {
|
||||
updateDesiredRTHAltitude();
|
||||
|
||||
// If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
|
||||
if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estSurfaceStatus != EST_TRUSTED)) {
|
||||
if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estAglStatus != EST_TRUSTED)) {
|
||||
// If we are in terrain-following modes - signal that we should update the surface tracking setpoint
|
||||
// NONE/USABLE means that we were flying blind, now we should lock to surface
|
||||
//updateSurfaceTrackingSetpoint();
|
||||
}
|
||||
|
||||
posControl.flags.estSurfaceStatus = surfaceStatus; // Could be TRUSTED or USABLE
|
||||
posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE
|
||||
posControl.flags.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = 1;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface > 0) {
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && surfaceDistance > 0) {
|
||||
if (posControl.actualState.surfaceMin > 0) {
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface);
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
|
||||
}
|
||||
else {
|
||||
posControl.actualState.surfaceMin = posControl.actualState.surface;
|
||||
posControl.actualState.surfaceMin = surfaceDistance;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1544,8 +1487,8 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[Z] = constrain(posControl.actualState.pos.z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(posControl.actualState.vel.z, -32678, 32767);
|
||||
navLatestActualPosition[Z] = constrain(navGetCurrentActualPositionAndVelocity()->pos.z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -1565,21 +1508,29 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
|
|||
posControl.flags.headingDataNew = 1;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Returns pointer to currently used position (ABS or AGL) depending on surface tracking status
|
||||
*-----------------------------------------------------------*/
|
||||
const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
|
||||
{
|
||||
return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Calculates distance and bearing to destination point
|
||||
*-----------------------------------------------------------*/
|
||||
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->x - posControl.actualState.pos.x;
|
||||
const float deltaY = destinationPos->y - posControl.actualState.pos.y;
|
||||
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
return sqrtf(sq(deltaX) + sq(deltaY));
|
||||
}
|
||||
|
||||
int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
|
||||
{
|
||||
const float deltaX = destinationPos->x - posControl.actualState.pos.x;
|
||||
const float deltaY = destinationPos->y - posControl.actualState.pos.y;
|
||||
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
|
||||
}
|
||||
|
|
@ -1626,16 +1577,16 @@ static void updateDesiredRTHAltitude(void)
|
|||
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
|
||||
switch (navConfig()->general.flags.rth_alt_control_mode) {
|
||||
case NAV_RTH_NO_ALT:
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
|
||||
break;
|
||||
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z + navConfig()->general.rth_altitude;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
|
||||
break;
|
||||
case NAV_RTH_MAX_ALT:
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.pos.z);
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z);
|
||||
break;
|
||||
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.pos.z);
|
||||
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
|
||||
break;
|
||||
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
|
||||
default:
|
||||
|
|
@ -1645,7 +1596,7 @@ static void updateDesiredRTHAltitude(void)
|
|||
}
|
||||
}
|
||||
else {
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.pos.z;
|
||||
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1731,7 +1682,7 @@ void updateHomePosition(void)
|
|||
{
|
||||
// Disarmed and have a valid position, constantly update home
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (posControl.flags.estPosStatue >= EST_USABLE) {
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) {
|
||||
bool setHome = !posControl.flags.isHomeValid;
|
||||
switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
|
||||
case NAV_RESET_NEVER:
|
||||
|
|
@ -1744,7 +1695,7 @@ void updateHomePosition(void)
|
|||
break;
|
||||
}
|
||||
if (setHome) {
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1753,9 +1704,9 @@ void updateHomePosition(void)
|
|||
|
||||
// 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.estPosStatue >= EST_USABLE)) {
|
||||
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.pos, posControl.actualState.yaw, homeUpdateFlags);
|
||||
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags);
|
||||
isHomeResetAllowed = false;
|
||||
}
|
||||
}
|
||||
|
|
@ -1819,7 +1770,6 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
|
|||
// Z-position
|
||||
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
|
||||
posControl.desiredState.surface = -1; // When we directly set altitude target we must reset surface tracking
|
||||
posControl.desiredState.pos.z = pos->z;
|
||||
}
|
||||
|
||||
|
|
@ -1838,9 +1788,9 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
|
|||
|
||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
|
||||
{
|
||||
farAwayPos->x = posControl.actualState.pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->y = posControl.actualState.pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->z = posControl.actualState.pos.z;
|
||||
farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -1878,9 +1828,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
static timeUs_t lastUpdateTimeUs;
|
||||
timeUs_t currentTimeUs = micros();
|
||||
|
||||
// Terrain following uses different altitude measurement
|
||||
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
|
||||
if (mode == ROC_TO_ALT_RESET) {
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
posControl.desiredState.pos.z = posControl.actualState.pos.z;
|
||||
posControl.desiredState.pos.z = altitudeToUse;
|
||||
}
|
||||
else {
|
||||
if (STATE(FIXED_WING)) {
|
||||
|
|
@ -1892,22 +1845,23 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
|
|||
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, // FIXME: calculate sanity limits in a smarter way
|
||||
posControl.actualState.pos.z - 500,
|
||||
posControl.actualState.pos.z + 500);
|
||||
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
|
||||
posControl.desiredState.pos.z = posControl.actualState.pos.z + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
}
|
||||
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
}
|
||||
}
|
||||
|
||||
static void resetAltitudeController(void)
|
||||
static void resetAltitudeController(bool useTerrainFollowing)
|
||||
{
|
||||
// Set terrain following flag
|
||||
posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
resetFixedWingAltitudeController();
|
||||
}
|
||||
|
|
@ -2019,7 +1973,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
|
|||
else if (wpNumber == 255) {
|
||||
gpsLocation_t wpLLH;
|
||||
|
||||
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH);
|
||||
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos, &wpLLH);
|
||||
|
||||
wpData->lat = wpLLH.lat;
|
||||
wpData->lon = wpLLH.lon;
|
||||
|
|
@ -2044,7 +1998,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
wpLLH.alt = wpData->alt;
|
||||
|
||||
// WP #0 - special waypoint - HOME
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -2052,8 +2006,8 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
// WP #255 - special waypoint - directly set desiredPosition
|
||||
// Only valid when armed and in poshold mode
|
||||
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
// Convert to local coordinates
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
|
||||
|
|
@ -2221,16 +2175,6 @@ float getActiveWaypointSpeed(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* A function to reset navigation PIDs and states
|
||||
*-----------------------------------------------------------*/
|
||||
void resetNavigation(void)
|
||||
{
|
||||
resetAltitudeController();
|
||||
resetHeadingController();
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Process adjustments to alt, pos and yaw controllers
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2270,8 +2214,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
#if defined(NAV_BLACKBOX)
|
||||
navFlags = 0;
|
||||
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
|
||||
if (posControl.flags.estSurfaceStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
if (posControl.flags.estPosStatue == EST_TRUSTED) navFlags |= (1 << 2);
|
||||
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
#endif
|
||||
|
|
@ -2317,7 +2261,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.x), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.y), -32678, 32767);
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.z), -32678, 32767);
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -2342,7 +2285,7 @@ static bool canActivateAltHoldMode(void)
|
|||
|
||||
static bool canActivatePosHoldMode(void)
|
||||
{
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static bool posEstimationHasGlobalReference(void)
|
||||
|
|
@ -2409,14 +2352,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
canActivateWaypoint = true;
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
|
||||
if ((FLIGHT_MODE(NAV_ALTHOLD_MODE) && FLIGHT_MODE(NAV_POSHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) {
|
||||
if ((FLIGHT_MODE(NAV_POSHOLD_MODE)) || (canActivatePosHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D;
|
||||
if (FLIGHT_MODE(NAV_POSHOLD_MODE) || (canActivatePosHold && canActivateAltHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
|
||||
|
|
@ -2490,6 +2428,11 @@ int8_t navigationGetHeadingControlState(void)
|
|||
}
|
||||
}
|
||||
|
||||
bool navigationTerrainFollowingEnabled(void)
|
||||
{
|
||||
return posControl.flags.isTerrainFollowEnabled;
|
||||
}
|
||||
|
||||
bool navigationBlockArming(void)
|
||||
{
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD);
|
||||
|
|
@ -2500,7 +2443,7 @@ bool navigationBlockArming(void)
|
|||
return false;
|
||||
|
||||
// Apply extra arming safety only if pilot has any of GPS modes configured
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
|
||||
shouldBlockArming = true;
|
||||
}
|
||||
|
||||
|
|
@ -2527,7 +2470,7 @@ bool navigationBlockArming(void)
|
|||
|
||||
bool navigationPositionEstimateIsHealthy(void)
|
||||
{
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME);
|
||||
return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -2551,7 +2494,6 @@ void updateFlightBehaviorModifiers(void)
|
|||
}
|
||||
|
||||
posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
|
||||
posControl.flags.isTerrainFollowEnabled = IS_RC_MODE_ACTIVE(BOXSURFACE);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -2640,9 +2582,9 @@ void navigationInit(void)
|
|||
posControl.flags.headingDataNew = 0;
|
||||
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.estHeadingStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
|
||||
posControl.flags.forcedRTHActivated = 0;
|
||||
posControl.waypointCount = 0;
|
||||
|
|
@ -2650,8 +2592,6 @@ void navigationInit(void)
|
|||
posControl.waypointListValid = false;
|
||||
|
||||
/* Set initial surface invalid */
|
||||
posControl.actualState.surface = -1.0f;
|
||||
posControl.actualState.surfaceVel = 0.0f;
|
||||
posControl.actualState.surfaceMin = -1.0f;
|
||||
|
||||
/* Reset statistics */
|
||||
|
|
@ -2666,12 +2606,12 @@ void navigationInit(void)
|
|||
*-----------------------------------------------------------*/
|
||||
float getEstimatedActualVelocity(int axis)
|
||||
{
|
||||
return posControl.actualState.vel.v[axis];
|
||||
return navGetCurrentActualPositionAndVelocity()->vel.v[axis];
|
||||
}
|
||||
|
||||
float getEstimatedActualPosition(int axis)
|
||||
{
|
||||
return posControl.actualState.pos.v[axis];
|
||||
return navGetCurrentActualPositionAndVelocity()->pos.v[axis];
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue