diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 8b102d1d75..95b0222477 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -273,7 +273,6 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, - {"navTgtSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, #endif }; @@ -376,7 +375,6 @@ typedef struct blackboxMainState_s { int16_t navHeading; int16_t navTargetHeading; int16_t navSurface; - int16_t navTargetSurface; #endif } blackboxMainState_t; @@ -708,7 +706,6 @@ static void writeIntraframe(void) } blackboxWriteSignedVB(blackboxCurrent->navSurface); - blackboxWriteSignedVB(blackboxCurrent->navTargetSurface); #endif //Rotate our history buffers: @@ -879,7 +876,6 @@ static void writeInterframe(void) } blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface); - blackboxWriteSignedVB(blackboxCurrent->navTargetSurface - blackboxLast->navTargetSurface); #endif //Rotate our history buffers @@ -1192,7 +1188,6 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->navTargetPos[i] = navTargetPosition[i]; } blackboxCurrent->navSurface = navActualSurface; - blackboxCurrent->navTargetSurface = navTargetSurface; #endif } diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 1587c21ea0..4e4724a243 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -35,6 +35,8 @@ #include "sensors/diagnostics.h" #include "sensors/sensors.h" +#include "navigation/navigation.h" + #include "telemetry/telemetry.h" #define BOX_SUFFIX ';' @@ -286,7 +288,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)), BOXSURFACE); #ifdef USE_FLM_FLAPERON CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON); #endif @@ -304,6 +305,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)), BOXOSDALT1); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3); + CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()), BOXSURFACE); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index dc8e5da7f2..92a12aff35 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1213,6 +1213,9 @@ groups: - name: nav_rth_abort_threshold field: general.rth_abort_threshold max: 65000 + - name: nav_max_terrain_follow_alt + field: general.max_terrain_follow_altitude + max: 1000 - name: nav_rth_altitude field: general.rth_altitude max: 65000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 417ea73651..37f747e3db 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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]; } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index da1a82505d..a6cd4f8526 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -134,6 +134,7 @@ typedef struct navConfig_s { uint16_t rth_altitude; // altitude to maintain when RTH is active (depends on rth_alt_control_mode) (cm) uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home + uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode } general; struct { @@ -275,6 +276,7 @@ int8_t navigationGetHeadingControlState(void); bool navigationBlockArming(void); bool navigationPositionEstimateIsHealthy(void); bool navIsCalibrationComplete(void); +bool navigationTerrainFollowingEnabled(void); /* Access to estimated position and velocity */ float getEstimatedActualVelocity(int axis); @@ -335,7 +337,6 @@ extern int16_t navActualVelocity[3]; extern int16_t navDesiredVelocity[3]; extern int16_t navTargetPosition[3]; extern int32_t navLatestActualPosition[3]; -extern int16_t navTargetSurface; extern int16_t navActualSurface; extern uint16_t navFlags; extern uint16_t navEPH; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f60ec37f4d..8244ebe1dd 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -107,7 +107,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS; const float demSKE = 0.0f; - const float estSPE = (posControl.actualState.pos.z / 100.0f) * GRAVITY_MSS; + const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS; const float estSKE = 0.0f; // speedWeight controls balance between potential and kinetic energy used for pitch controller @@ -206,8 +206,8 @@ void resetFixedWingPositionController(void) static void calculateVirtualPositionTarget_FW(float trackingPeriod) { - float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x; - float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y; + float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; + float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); @@ -229,14 +229,14 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle); // We have temporary loiter target. Recalculate distance and position error - posErrorX = loiterTargetX - posControl.actualState.pos.x; - posErrorY = loiterTargetY - posControl.actualState.pos.y; + posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; + posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y; distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); } // Calculate virtual waypoint - virtualDesiredPosition.x = posControl.actualState.pos.x + posErrorX * (trackingDistance / distanceToActualTarget); - virtualDesiredPosition.y = posControl.actualState.pos.y + posErrorY * (trackingDistance / distanceToActualTarget); + virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget); + virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget); // Shift position according to pilot's ROLL input (up to max_manual_speed velocity) if (posControl.flags.isAdjustingPosition) { @@ -330,7 +330,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) } // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode - if ((posControl.flags.estPosStatue >= EST_USABLE)) { + if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; @@ -378,7 +378,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) } // Apply controller only if position source is valid - if ((posControl.flags.estPosStatue >= EST_USABLE)) { + if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; @@ -473,8 +473,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat * TODO refactor conditions in this metod if logic is proven to be correct */ if (navStateFlags & NAV_CTL_LAND) { - if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.z <= navConfig()->general.land_slowdown_minalt)) || - ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surface <= navConfig()->general.land_slowdown_minalt)) ) { + if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) || + ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) { /* * Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled */ @@ -534,7 +534,7 @@ void applyFixedWingEmergencyLandingController(void) void calculateFixedWingInitialHoldPosition(fpVector3_t * pos) { // TODO: stub, this should account for velocity and target loiter radius - *pos = posControl.actualState.pos; + *pos = navGetCurrentActualPositionAndVelocity()->pos; } void resetFixedWingHeadingController(void) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 9f4b157fec..1bb3e332b8 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -61,7 +61,7 @@ static bool prepareForTakeoffOnReset = false; // Position to velocity controller for Z axis static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) { - const float altitudeError = posControl.desiredState.pos.z - posControl.actualState.pos.z; + const float altitudeError = posControl.desiredState.pos.z - navGetCurrentActualPositionAndVelocity()->pos.z; float targetVel = altitudeError * posControl.pids.pos[Z].param.kP; // hard limit desired target velocity to max_climb_rate @@ -93,7 +93,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle; - posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, posControl.actualState.vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); + posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax); @@ -101,32 +101,50 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) bool adjustMulticopterAltitudeFromRCInput(void) { - const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband); - if (rcThrottleAdjustment) { - // set velocity proportional to stick movement - float rcClimbRate; + if (posControl.flags.isTerrainFollowEnabled) { + const float altTarget = scaleRangef(rcCommand[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude); - // Make sure we can satisfy max_manual_climb_rate in both up and down directions - if (rcThrottleAdjustment > 0) { - // Scaling from altHoldThrottleRCZero to maxthrottle - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); + // In terrain follow mode we apply different logic for terrain control + if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { + // We have solid terrain sensor signal - directly map throttle to altitude + updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + posControl.desiredState.pos.z = altTarget; } else { - // Scaling from minthrottle to altHoldThrottleRCZero - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband); + updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL); } - updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); - + // In surface tracking we always indicate that we're adjusting altitude return true; } else { - // Adjusting finished - reset desired position to stay exactly where pilot released the stick - if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - } + const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband); + if (rcThrottleAdjustment) { + // set velocity proportional to stick movement + float rcClimbRate; - return false; + // Make sure we can satisfy max_manual_climb_rate in both up and down directions + if (rcThrottleAdjustment > 0) { + // Scaling from altHoldThrottleRCZero to maxthrottle + rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); + } + else { + // Scaling from minthrottle to altHoldThrottleRCZero + rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband); + } + + updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); + + return true; + } + else { + // Adjusting finished - reset desired position to stay exactly where pilot released the stick + if (posControl.flags.isAdjustingAltitude) { + updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + } + + return false; + } } } @@ -160,6 +178,8 @@ void setupMulticopterAltitudeController(void) void resetMulticopterAltitudeController(void) { + const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity(); + navPidReset(&posControl.pids.vel[Z]); navPidReset(&posControl.pids.surface); posControl.rcAdjustment[THROTTLE] = 0; @@ -167,13 +187,13 @@ void resetMulticopterAltitudeController(void) if (prepareForTakeoffOnReset) { /* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */ posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate; - posControl.desiredState.pos.z = posControl.actualState.pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); + posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); posControl.pids.vel[Z].integrator = -500.0f; pt1FilterReset(&altholdThrottleFilterState, -500.0f); prepareForTakeoffOnReset = false; } else { - posControl.desiredState.vel.z = posControl.actualState.vel.z; // Gradually transition from current climb + posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb pt1FilterReset(&altholdThrottleFilterState, 0.0f); } } @@ -270,8 +290,8 @@ bool adjustMulticopterPositionFromRCInput(void) const float neuVelY = rcVelX * posControl.actualState.sinYaw + rcVelY * posControl.actualState.cosYaw; // Calculate new position target, so Pos-to-Vel P-controller would yield desired velocity - posControl.desiredState.pos.x = posControl.actualState.pos.x + (neuVelX / posControl.pids.pos[X].param.kP); - posControl.desiredState.pos.y = posControl.actualState.pos.y + (neuVelY / posControl.pids.pos[Y].param.kP); + posControl.desiredState.pos.x = navGetCurrentActualPositionAndVelocity()->pos.x + (neuVelX / posControl.pids.pos[X].param.kP); + posControl.desiredState.pos.y = navGetCurrentActualPositionAndVelocity()->pos.y + (neuVelY / posControl.pids.pos[Y].param.kP); } return true; @@ -314,8 +334,8 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax) static void updatePositionVelocityController_MC(void) { - const float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x; - const float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y; + const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; + const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; // Calculate target velocity float newVelX = posErrorX * posControl.pids.pos[X].param.kP; @@ -348,8 +368,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA { // Calculate velocity error - const float velErrorX = posControl.desiredState.vel.x - posControl.actualState.vel.x; - const float velErrorY = posControl.desiredState.vel.y - posControl.actualState.vel.y; + const float velErrorX = posControl.desiredState.vel.x - navGetCurrentActualPositionAndVelocity()->vel.x; + const float velErrorY = posControl.desiredState.vel.y - navGetCurrentActualPositionAndVelocity()->vel.y; // Calculate XY-acceleration limit according to velocity error limit float accelLimitX, accelLimitY; @@ -376,8 +396,8 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration - const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, posControl.actualState.vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0); - const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, posControl.actualState.vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0); + const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0); + const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0); // Save last acceleration target lastAccelTargetX = newAccelX; @@ -422,7 +442,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode // and pilots input would be passed thru to PID controller - if ((posControl.flags.estPosStatue >= EST_USABLE)) { + if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; @@ -484,7 +504,7 @@ bool isMulticopterLandingDetected(void) } // Average climb rate should be low enough - bool verticalMovement = fabsf(posControl.actualState.vel.z) > 25.0f; + bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > 25.0f; // check if we are moving horizontally bool horizontalMovement = posControl.actualState.velXY > 100.0f; @@ -507,11 +527,11 @@ bool isMulticopterLandingDetected(void) DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 2, (currentTimeUs - landingTimer) / 1000); // If we have surface sensor (for example sonar) - use it to detect touchdown - if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surfaceMin >= 0)) { + if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) { // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety. // TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy. // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed - possibleLandingDetected = possibleLandingDetected && (posControl.actualState.surface <= (posControl.actualState.surfaceMin + 5.0f)); + possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f)); } if (!possibleLandingDetected) { @@ -585,11 +605,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) *-----------------------------------------------------------*/ void calculateMulticopterInitialHoldPosition(fpVector3_t * pos) { - const float stoppingDistanceX = posControl.actualState.vel.x * posControl.posDecelerationTime; - const float stoppingDistanceY = posControl.actualState.vel.y * posControl.posDecelerationTime; + const float stoppingDistanceX = navGetCurrentActualPositionAndVelocity()->vel.x * posControl.posDecelerationTime; + const float stoppingDistanceY = navGetCurrentActualPositionAndVelocity()->vel.y * posControl.posDecelerationTime; - pos->x = posControl.actualState.pos.x + stoppingDistanceX; - pos->y = posControl.actualState.pos.y + stoppingDistanceY; + pos->x = navGetCurrentActualPositionAndVelocity()->pos.x + stoppingDistanceX; + pos->y = navGetCurrentActualPositionAndVelocity()->pos.y + stoppingDistanceY; } void resetMulticopterHeadingController(void) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 10c2371636..1a7ece4d59 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -828,8 +828,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) posEstimator.est.epv = newEPV; /* AGL estimation */ -#ifdef USE_RANGEFINDER - if (isSurfaceValid) { // If surface topic is updated in timely manner - do something smart +#if defined(USE_RANGEFINDER) && defined(USE_BARO) + if (isSurfaceValid && isBaroValid) { navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual; bool resetSurfaceEstimate = false; switch (posEstimator.est.aglQual) { @@ -893,7 +893,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { // Correct estimate from rangefinder const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt; - const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f); + const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f); posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt; posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt; @@ -909,7 +909,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt; const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability; const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler); - + posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt; posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt; } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 1effbae97d..0fb2f40560 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -71,9 +71,9 @@ typedef struct navigationFlags_s { bool verticalPositionDataConsumed; navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it) - navigationEstimateStatus_e estPosStatue; // Indicates that GPS is working (or not) + navigationEstimateStatus_e estPosStatus; // Indicates that GPS is working (or not) + navigationEstimateStatus_e estAglStatus; navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane - navigationEstimateStatus_e estSurfaceStatus; bool isAdjustingPosition; bool isAdjustingAltitude; @@ -131,20 +131,25 @@ typedef struct navigationPIDControllers_s { typedef struct { fpVector3_t pos; fpVector3_t vel; - int32_t yaw; - float sinYaw; - float cosYaw; - float surface; - float surfaceVel; - float surfaceMin; - float velXY; +} navEstimatedPosVel_t; + +typedef struct { + // Local estimated states + navEstimatedPosVel_t abs; + navEstimatedPosVel_t agl; + int32_t yaw; + + // Service values + float sinYaw; + float cosYaw; + float surfaceMin; + float velXY; } navigationEstimatedState_t; typedef struct { fpVector3_t pos; fpVector3_t vel; int32_t yaw; - float surface; } navigationDesiredState_t; typedef enum { @@ -156,7 +161,6 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_IDLE, NAV_FSM_EVENT_SWITCH_TO_ALTHOLD, - NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D, NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D, NAV_FSM_EVENT_SWITCH_TO_RTH, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT, @@ -180,9 +184,6 @@ typedef enum { NAV_STATE_ALTHOLD_INITIALIZE, // 2 NAV_STATE_ALTHOLD_IN_PROGRESS, // 3 - NAV_STATE_POSHOLD_2D_INITIALIZE, // 4 - NAV_STATE_POSHOLD_2D_IN_PROGRESS, // 5 - NAV_STATE_POSHOLD_3D_INITIALIZE, // 6 NAV_STATE_POSHOLD_3D_IN_PROGRESS, // 7 @@ -302,6 +303,8 @@ typedef struct { extern navigationPosControl_t posControl; /* Internally used functions */ +const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); + float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler); void navPidReset(pidController_t *pid); diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 5c22a2583d..eb49faaee3 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -52,7 +52,7 @@ * time to send frame: 3ms. */ -#define SBUS_MIN_INTERFRAME_DELAY_US 5000 // According to FrSky interframe is 6.67ms, we go smaller just in case +#define SBUS_MIN_INTERFRAME_DELAY_US 3000 // According to FrSky interframe is 6.67ms, we go smaller just in case #define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2) diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index cc434a2672..4715988826 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -165,7 +165,11 @@ #define USE_MAG_IST8308 #define USE_MAG_MAG3110 +#define USE_OPTICAL_FLOW +#define USE_OPFLOW_MSP + #define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS