From 8b7a14aea70e37284e0ff3186f44d485637b8cf0 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 13 Mar 2017 21:14:42 +1000 Subject: [PATCH 1/4] Split auto/manual speed and climb rate limitation; Remove 2D RTH; RTH safety refactoring --- docs/Cli.md | 4 +- src/main/fc/cli.c | 4 +- src/main/fc/fc_msp.c | 8 +- src/main/navigation/navigation.c | 417 ++++++------------- src/main/navigation/navigation.h | 4 +- src/main/navigation/navigation_multicopter.c | 7 +- src/main/navigation/navigation_private.h | 59 ++- src/main/target/NAZE/hardware_revision.c | 7 - 8 files changed, 161 insertions(+), 349 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index d53c5a3e19c..f8ba530c4a9 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -160,8 +160,8 @@ Re-apply any new defaults as desired. | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | | nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value (cm) | -| nav_max_speed | 300 | Maximum velocity firmware is allowed in full auto modes (POSHOLD, RTH, WP) [cm/s] [Multirotor only] | -| nav_max_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s | +| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (POSHOLD, RTH, WP) [cm/s] [Multirotor only] | +| nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s | | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | | nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] | diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5ade430c537..f35dec22824 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -807,8 +807,8 @@ static const clivalue_t valueTable[] = { { "nav_position_timeout", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.pos_failure_timeout) }, { "nav_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 10000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.waypoint_radius) }, { "nav_wp_safe_distance", VAR_UINT16 | MASTER_VALUE | MODE_MAX, .config.max = { 65000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.waypoint_safe_distance) }, - { "nav_max_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_speed) }, - { "nav_max_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_climb_rate) }, + { "nav_auto_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_auto_speed) }, + { "nav_auto_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_auto_climb_rate) }, { "nav_manual_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_manual_speed) }, { "nav_manual_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_manual_climb_rate ) }, { "nav_landing_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.land_descent_rate) }, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ea90d7f150c..a16b8e2ef1d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1163,8 +1163,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #ifdef NAV case MSP_NAV_POSHOLD: sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); - sbufWriteU16(dst, navConfig()->general.max_speed); - sbufWriteU16(dst, navConfig()->general.max_climb_rate); + sbufWriteU16(dst, navConfig()->general.max_auto_speed); + sbufWriteU16(dst, navConfig()->general.max_auto_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_speed); sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); @@ -1640,8 +1640,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifdef NAV case MSP_SET_NAV_POSHOLD: navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); - navConfigMutable()->general.max_speed = sbufReadU16(src); - navConfigMutable()->general.max_climb_rate = sbufReadU16(src); + navConfigMutable()->general.max_auto_speed = sbufReadU16(src); + navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src); navConfigMutable()->general.max_manual_speed = sbufReadU16(src); navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5ed39d456af..12ac4f6f86d 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -86,8 +86,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .pos_failure_timeout = 5, // 5 sec .waypoint_radius = 100, // 2m diameter .waypoint_safe_distance = 10000, // 100m - first waypoint should be closer than this - .max_speed = 300, // 3 m/s = 10.8 km/h - .max_climb_rate = 500, // 5 m/s + .max_auto_speed = 300, // 3 m/s = 10.8 km/h + .max_auto_climb_rate = 500, // 5 m/s .max_manual_speed = 500, .max_manual_climb_rate = 200, .land_descent_rate = 200, // 2 m/s @@ -175,17 +175,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS( 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); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState); @@ -319,113 +314,33 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - /** RTH mode entry point ************************************************/ + /** RTH_3D mode ************************************************/ [NAV_STATE_RTH_INITIALIZE] = { .onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE, .timeoutMs = 10, - .stateFlags = NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE, - .mwState = MW_NAV_STATE_RTH_START, - .mwError = MW_NAV_ERROR_SPOILED_GPS, // we are stuck in this state only if GPS is compromised - .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_RTH_2D] = NAV_STATE_RTH_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_3D] = NAV_STATE_RTH_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - } - }, - - /** RTH_2D mode ************************************************/ - [NAV_STATE_RTH_2D_INITIALIZE] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE, - .timeoutMs = 10, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH, - .mapToFlightModes = NAV_RTH_MODE, - .mwState = MW_NAV_STATE_RTH_START, - .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_INITIALIZE, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_HEAD_HOME, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - } - }, - - [NAV_STATE_RTH_2D_HEAD_HOME] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME, - .timeoutMs = 10, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_RTH_MODE, - .mwState = MW_NAV_STATE_RTH_ENROUTE, - .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_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, - } - }, - - [NAV_STATE_RTH_2D_FINISHING] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_FINISHING, - .timeoutMs = 0, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_RTH_MODE, - .mwState = MW_NAV_STATE_RTH_ENROUTE, - .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_FINISHED, - } - }, - - [NAV_STATE_RTH_2D_FINISHED] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_2D_FINISHED, - .timeoutMs = 10, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_RTH_MODE, - .mwState = MW_NAV_STATE_RTH_ENROUTE, - .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_FINISHED, // 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_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - } - }, - - /** RTH_3D mode ************************************************/ - [NAV_STATE_RTH_3D_INITIALIZE] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE, - .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_START, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_INITIALIZE, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - [NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, + [NAV_STATE_RTH_CLIMB_TO_SAFE_ALT] = { + .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HEAD_HOME, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state + [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, @@ -433,16 +348,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_RTH_3D_HEAD_HOME] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME, + [NAV_STATE_RTH_HEAD_HOME] = { + .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME, .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [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, @@ -451,16 +366,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, + [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = { + .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, .timeoutMs = 500, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_SETTLE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_LANDING, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [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, @@ -469,16 +384,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_RTH_3D_LANDING] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_LANDING, + [NAV_STATE_RTH_LANDING] = { + .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING, .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_LANDING, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_FINISHING, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state + [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, @@ -487,28 +402,28 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_RTH_3D_FINISHING] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_FINISHING, + [NAV_STATE_RTH_FINISHING] = { + .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING, .timeoutMs = 0, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_FINISHED, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - [NAV_STATE_RTH_3D_FINISHED] = { - .onEntry = navOnEnteringState_NAV_STATE_RTH_3D_FINISHED, + [NAV_STATE_RTH_FINISHED] = { + .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED, .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_FINISHED, // re-process state + [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, @@ -870,102 +785,55 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { - if (STATE(GPS_FIX_HOME)) { - if (posControl.flags.hasValidHeadingSensor) { - navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - - if ((prevFlags & NAV_CTL_POS) == 0) { - resetPositionController(); - } + navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - if ((prevFlags & NAV_CTL_ALT) == 0) { - resetAltitudeController(); - setupAltitudeController(); - } - - // Switch between 2D and 3D RTH depending on altitude sensor availability - if (posControl.flags.hasValidAltitudeSensor) { - // We might have GPS unavailable - in case of 3D RTH set current altitude target - setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_Z); - - return NAV_FSM_EVENT_SWITCH_TO_RTH_3D; - } - else { - return NAV_FSM_EVENT_SWITCH_TO_RTH_2D; - } - } - /* Position sensor failure timeout - land */ - else if (checkForPositionSensorTimeout()) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - /* No valid POS sensor but still within valid timeout - wait */ - else { - return NAV_FSM_EVENT_NONE; - } - } - else { - // No home position set or no heading sensor + if (!posControl.flags.hasValidHeadingSensor || !STATE(GPS_FIX_HOME)) { + // Heading sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } -} -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState) -{ - UNUSED(previousState); - - if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance)) { - // Prevent RTH from activating on airplanes if too close to home + if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { + // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - else { - if (posControl.flags.hasValidPositionSensor) { - // If close to home - reset home position - if (posControl.homeDistance < navConfig()->general.min_rth_distance) { - setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); - } - // Initialize RTH sanity check to prevent fly-aways on RTH - initializeRTHSanityChecker(&posControl.actualState.pos); + // Reset altitude and position controllers if necessary + if ((prevFlags & NAV_CTL_POS) == 0) { + resetPositionController(); + } - return NAV_FSM_EVENT_SUCCESS; - } - /* Position sensor failure timeout - land */ - else if (checkForPositionSensorTimeout()) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - /* No valid POS sensor but still within valid timeout - wait */ - else { - return NAV_FSM_EVENT_NONE; - } + if ((prevFlags & NAV_CTL_ALT) == 0) { + resetAltitudeController(); + setupAltitudeController(); } -} -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState) -{ - UNUSED(previousState); + // If we have valid position sensor or configured to ignore it's loss at initial stage - continue + if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) { + // 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); - if (!posControl.flags.hasValidHeadingSensor) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - else if (posControl.flags.hasValidPositionSensor) { - if (isWaypointReached(&posControl.homeWaypointAbove, true)) { - // Successfully reached position target - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_FINISHING - } - else if (!validateRTHSanityChecker()) { - // Sanity check of RTH - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING } else { - // Update XY-position target - if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } - else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + t_fp_vector targetHoldPos; + + if (STATE(FIXED_WING)) { + // Airplane - climbout before turning around + calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away + } else { + // Multicopter, hover and climb + calculateInitialHoldPosition(&targetHoldPos); + + // Initialize RTH sanity check to prevent fly-aways on RTH + // For airplanes this is delayed until climb-out is finished + initializeRTHSanityChecker(&targetHoldPos); } - return NAV_FSM_EVENT_NONE; + setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); + + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT } } /* Position sensor failure timeout - land */ @@ -978,71 +846,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState) -{ - UNUSED(previousState); - setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_SUCCESS; -} - -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigationFSMState_t previousState) -{ - // Same logic as PH_2D - return navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(previousState); -} - -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) { UNUSED(previousState); - if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance)) { - // Prevent RTH from activating on airplanes if too close to home - return NAV_FSM_EVENT_SWITCH_TO_IDLE; - } - else { - if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) { - // 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); - - return NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING - } - else { - t_fp_vector targetHoldPos; - - if (STATE(FIXED_WING)) { - // Airplane - climbout before turning around - calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away - } else { - // Multicopter, hover and climb - calculateInitialHoldPosition(&targetHoldPos); - - // Initialize RTH sanity check to prevent fly-aways on RTH - // For airplanes this is delayed until climb-out is finished - initializeRTHSanityChecker(&targetHoldPos); - } - - setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); - - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT - } - } - /* Position sensor failure timeout - land */ - else if (checkForPositionSensorTimeout()) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - /* No valid POS sensor but still within valid timeout - wait */ - else { - return NAV_FSM_EVENT_NONE; - } + if (!posControl.flags.hasValidHeadingSensor) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } -} - -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) -{ - UNUSED(previousState); - + // If we have valid pos sensor OR we are configured to ignore GPS loss if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) || (!navConfig()->general.flags.rth_climb_first)) { @@ -1051,7 +862,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL initializeRTHSanityChecker(&posControl.actualState.pos); } - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME } else { /* For multi-rotors execute sanity check during initial ascent as well */ @@ -1083,18 +894,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState) { UNUSED(previousState); if (!posControl.flags.hasValidHeadingSensor) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - else if (posControl.flags.hasValidPositionSensor) { + + // If we have position sensor - continue home + if (posControl.flags.hasValidPositionSensor) { 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); - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING } else if (!validateRTHSanityChecker()) { // Sanity check of RTH @@ -1121,37 +934,42 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) { UNUSED(previousState); - // If no position sensor available - land immediately - if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) && checkForPositionSensorTimeout()) { + if (!posControl.flags.hasValidHeadingSensor) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - // Wait until target heading is reached (with 15 deg margin for error) - if (STATE(FIXED_WING)) { - resetLandingDetector(); - return NAV_FSM_EVENT_SUCCESS; - } - else { - if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { + // If position ok OR within valid timeout - continue + if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout()) { + // Wait until target heading is reached (with 15 deg margin for error) + if (STATE(FIXED_WING)) { resetLandingDetector(); return NAV_FSM_EVENT_SUCCESS; } - else if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during pre-landing hover - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_NONE; + if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { + resetLandingDetector(); + return NAV_FSM_EVENT_SUCCESS; + } + else if (!validateRTHSanityChecker()) { + // Continue to check for RTH sanity during pre-landing hover + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } + else { + setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_NONE; + } } } + else { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1163,7 +981,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati } else { if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during pre-landing hover + // Continue to check for RTH sanity during landing return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1192,7 +1010,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1203,7 +1021,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(naviga return NAV_FSM_EVENT_SUCCESS; } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState) { // Stay in this state UNUSED(previousState); @@ -1302,10 +1120,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig { UNUSED(previousState); - const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_3D_LANDING(previousState); + const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); if (landEvent == NAV_FSM_EVENT_SUCCESS) { // Landing controller returned success - invoke RTH finishing state and finish the waypoint - navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(previousState); + navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState); return NAV_FSM_EVENT_SUCCESS; } else { @@ -2355,19 +2173,24 @@ bool isApproachingLastWaypoint(void) float getActiveWaypointSpeed(void) { - uint16_t waypointSpeed = navConfig()->general.max_speed; - - if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { - if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) { - waypointSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; + if (posControl.flags.isAdjustingPosition) { + // In manual control mode use different cap for speed + return navConfig()->general.max_manual_speed; + } + else { + uint16_t waypointSpeed = navConfig()->general.max_auto_speed; - if (waypointSpeed < 50 || waypointSpeed > navConfig()->general.max_speed) { - waypointSpeed = navConfig()->general.max_speed; + if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { + if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) { + const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; + if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) { + waypointSpeed = wpSpecificSpeed; + } } } - } - return waypointSpeed; + return waypointSpeed; + } } /*----------------------------------------------------------- @@ -2809,7 +2632,7 @@ rthState_e getStateOfForcedRTH(void) { /* If forced RTH activated and in AUTO_RTH or EMERG state */ if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) { - if (posControl.navState == NAV_STATE_RTH_2D_FINISHED || posControl.navState == NAV_STATE_RTH_3D_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { + if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { return RTH_HAS_LANDED; } else { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d97d91ab894..668ebd9771e 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -114,8 +114,8 @@ typedef struct navConfig_s { uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance - uint16_t max_speed; // autonomous navigation speed cm/sec - uint16_t max_climb_rate; // max vertical speed limitation cm/sec + uint16_t max_auto_speed; // autonomous navigation speed cm/sec + uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint16_t land_descent_rate; // normal RTH landing descent rate diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 15be6899584..2eb1e9ffde1 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -86,7 +86,12 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) float targetVel = altitudeError * posControl.pids.pos[Z].param.kP; // hard limit desired target velocity to max_climb_rate - targetVel = constrainf(targetVel, -navConfig()->general.max_climb_rate, navConfig()->general.max_climb_rate); + if (posControl.flags.isAdjustingAltitude) { + targetVel = constrainf(targetVel, -navConfig()->general.max_manual_climb_rate, navConfig()->general.max_manual_climb_rate); + } + else { + targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate); + } // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity. // if we are decelerating - don't limit (allow better recovery from falling) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 8efb03bc6eb..882b3f35090 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -154,15 +154,13 @@ typedef enum { 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_RTH_2D, - NAV_FSM_EVENT_SWITCH_TO_RTH_3D, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT, NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING, NAV_FSM_EVENT_SWITCH_TO_LAUNCH, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event - NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1, + NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, @@ -183,37 +181,30 @@ typedef enum { NAV_STATE_POSHOLD_3D_INITIALIZE, // 6 NAV_STATE_POSHOLD_3D_IN_PROGRESS, // 7 - NAV_STATE_RTH_INITIALIZE, // 8 - - NAV_STATE_RTH_2D_INITIALIZE, // 9 - NAV_STATE_RTH_2D_HEAD_HOME, // 10 - NAV_STATE_RTH_2D_FINISHING, // 11 - NAV_STATE_RTH_2D_FINISHED, // 12 - - NAV_STATE_RTH_3D_INITIALIZE, // 13 - NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // 14 - NAV_STATE_RTH_3D_HEAD_HOME, // 15 - NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, // 16 - NAV_STATE_RTH_3D_LANDING, // 17 - NAV_STATE_RTH_3D_FINISHING, // 18 - NAV_STATE_RTH_3D_FINISHED, // 19 - - NAV_STATE_WAYPOINT_INITIALIZE, // 20 - NAV_STATE_WAYPOINT_PRE_ACTION, // 21 - NAV_STATE_WAYPOINT_IN_PROGRESS, // 22 - NAV_STATE_WAYPOINT_REACHED, // 23 - NAV_STATE_WAYPOINT_NEXT, // 24 - NAV_STATE_WAYPOINT_FINISHED, // 25 - NAV_STATE_WAYPOINT_RTH_LAND, // 26 - - NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 27 - NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 28 - NAV_STATE_EMERGENCY_LANDING_FINISHED, // 29 - - NAV_STATE_LAUNCH_INITIALIZE, // 30 - NAV_STATE_LAUNCH_WAIT, // 31 - NAV_STATE_LAUNCH_MOTOR_DELAY, // 32 - NAV_STATE_LAUNCH_IN_PROGRESS, // 33 + NAV_STATE_RTH_INITIALIZE, // 8 + NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // 9 + NAV_STATE_RTH_HEAD_HOME, // 10 + NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, // 11 + NAV_STATE_RTH_LANDING, // 12 + NAV_STATE_RTH_FINISHING, // 13 + NAV_STATE_RTH_FINISHED, // 14 + + NAV_STATE_WAYPOINT_INITIALIZE, // 15 + NAV_STATE_WAYPOINT_PRE_ACTION, // 16 + NAV_STATE_WAYPOINT_IN_PROGRESS, // 17 + NAV_STATE_WAYPOINT_REACHED, // 18 + NAV_STATE_WAYPOINT_NEXT, // 19 + NAV_STATE_WAYPOINT_FINISHED, // 20 + NAV_STATE_WAYPOINT_RTH_LAND, // 21 + + NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 22 + NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 23 + NAV_STATE_EMERGENCY_LANDING_FINISHED, // 24 + + NAV_STATE_LAUNCH_INITIALIZE, // 25 + NAV_STATE_LAUNCH_WAIT, // 26 + NAV_STATE_LAUNCH_MOTOR_DELAY, // 27 + NAV_STATE_LAUNCH_IN_PROGRESS, // 28 NAV_STATE_COUNT, } navigationFSMState_t; diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index b41bfe5f474..cc180df3dc8 100755 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -36,13 +36,6 @@ #include "hardware_revision.h" -static const char * const hardwareRevisionNames[] = { - "Unknown", - "Naze 32", - "Naze32 rev.5", - "Naze32 SP" -}; - uint8_t hardwareRevision = UNKNOWN; void detectHardwareRevision(void) From 7ce1872b258946658e76c6759fdfa08aefd87b13 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 13 Mar 2017 21:22:42 +1000 Subject: [PATCH 2/4] Some cleanups --- src/main/navigation/navigation.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 12ac4f6f86d..3eefe8b2f54 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -797,18 +797,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - // Reset altitude and position controllers if necessary - if ((prevFlags & NAV_CTL_POS) == 0) { - resetPositionController(); - } - - if ((prevFlags & NAV_CTL_ALT) == 0) { - resetAltitudeController(); - setupAltitudeController(); - } - // If we have valid position sensor or configured to ignore it's loss at initial stage - continue if (posControl.flags.hasValidPositionSensor || 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(); + 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); @@ -853,7 +853,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n if (!posControl.flags.hasValidHeadingSensor) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - + // If we have valid pos sensor OR we are configured to ignore GPS loss if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) || (!navConfig()->general.flags.rth_climb_first)) { From b38d6e31486b70841b6528d9b541a3774edbfebf Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 13 Mar 2017 23:48:30 +1000 Subject: [PATCH 3/4] Protection from activating RTH w/o altitude sensor --- src/main/navigation/navigation.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 3eefe8b2f54..6ce9d31eef0 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -787,8 +787,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - if (!posControl.flags.hasValidHeadingSensor || !STATE(GPS_FIX_HOME)) { - // Heading sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing + if (!posControl.flags.hasValidHeadingSensor || !posControl.flags.hasValidAltitudeSensor || !STATE(GPS_FIX_HOME)) { + // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -2345,7 +2345,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // RTH/Failsafe_RTH can override PASSTHRU - if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && STATE(GPS_FIX_HOME))) { + if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) { // If we request forced RTH - attempt to activate it no matter what // This might switch to emergency landing controller if GPS is unavailable canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason From 628216e6b48ab06d8b1a65cf06b1d519821bc22e Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 15 Mar 2017 22:15:40 +1000 Subject: [PATCH 4/4] Fallback to FAILSAFE_LANDING in case RTH is aborted during failsafe --- src/main/flight/failsafe.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 98a82e3ae45..b15c9143d6f 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -413,19 +413,24 @@ void failsafeUpdateState(void) if (armed) { beeperMode = BEEPER_RX_LOST_LANDING; } - bool rthIdleOrLanded; + bool rthLanded = false; switch (getStateOfForcedRTH()) { case RTH_IN_PROGRESS: - rthIdleOrLanded = false; break; - default: - case RTH_IDLE: case RTH_HAS_LANDED: - rthIdleOrLanded = true; + rthLanded = true; + break; + + case RTH_IDLE: + default: + // This shouldn't happen. If RTH was somehow aborted during failsafe - fallback to FAILSAFE_LANDING procedure + abortForcedRTH(); + failsafeActivate(FAILSAFE_LANDING); + reprocessState = true; break; } - if (rthIdleOrLanded || !armed) { + if (rthLanded || !armed) { failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData failsafeState.phase = FAILSAFE_LANDED; reprocessState = true;