From 8130be4772870007c57629f9e6017ae7d9422744 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 2 Dec 2015 21:46:38 +1000 Subject: [PATCH] Fixes and optimisations to FSM. Smoother transition from PH to RTH. Option to execute RTH tail first --- src/main/config/config.c | 1 + src/main/flight/navigation_rewrite.c | 89 ++++++++++++++++++++-------- src/main/flight/navigation_rewrite.h | 1 + src/main/flight/navigation_rewrite_private.h | 11 ++-- src/main/io/serial_cli.c | 1 + 5 files changed, 73 insertions(+), 30 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 373e02678..b7d19a00e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -217,6 +217,7 @@ void resetNavConfig(navConfig_t * navConfig) navConfig->flags.extra_arming_safety = 1; navConfig->flags.user_control_mode = NAV_GPS_ATTI; navConfig->flags.rth_alt_control_style = NAV_RTH_AT_LEAST_ALT; + navConfig->flags.rth_tail_first = 0; // Inertial position estimator parameters #if defined(INAV_ENABLE_AUTO_MAG_DECLINATION) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 6b41316e0..d98c332a7 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -288,6 +288,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_GPS_FAILING, // re-process the state [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_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_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -634,10 +635,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_INITIALIZE(n { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + if ((prevFlags & NAV_CTL_POS) == 0) { + resetPositionController(); + } + if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) { t_fp_vector targetHoldPos; - - resetPositionController(); calculateInitialHoldPosition(&targetHoldPos); setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); } @@ -655,10 +658,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) { + if ((prevFlags & NAV_CTL_POS) == 0) { + resetPositionController(); + } + + if ((prevFlags & NAV_CTL_ALT) == 0) { resetAltitudeController(); setupAltitudeController(); + } + if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) { // If low enough and surface offset valid - enter surface tracking setDesiredSurfaceOffset(posControl.actualState.surface); setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); @@ -666,8 +675,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) { t_fp_vector targetHoldPos; - - resetPositionController(); calculateInitialHoldPosition(&targetHoldPos); setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); } @@ -706,13 +713,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState) { - UNUSED(previousState); + navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + + if ((prevFlags & NAV_CTL_POS) == 0) { + resetPositionController(); + } + // If close to home - reset home position if (posControl.homeDistance < posControl.navConfig->min_rth_distance) { setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw); } - resetPositionController(); return NAV_FSM_EVENT_SUCCESS; } @@ -722,16 +733,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga // If no position sensor available - switch to NAV_STATE_RTH_2D_GPS_FAILING if (!posControl.flags.hasValidPositionSensor) { - return NAV_FSM_EVENT_ERROR; + return NAV_FSM_EVENT_ERROR; // NAV_STATE_RTH_2D_GPS_FAILING } if (isWaypointReached(&posControl.homeWaypointAbove)) { // Successfully reached position target - return NAV_FSM_EVENT_SUCCESS; + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_FINISHING } else { // Update XY-position target - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + if (posControl.navConfig->flags.rth_tail_first) { + 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); + } + return NAV_FSM_EVENT_NONE; } } @@ -742,7 +759,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navi /* Wait for GPS to be online again */ if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) { - return NAV_FSM_EVENT_SUCCESS; + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_HEAD_HOME } /* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */ else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) { @@ -757,7 +774,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState) { UNUSED(previousState); - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SUCCESS; } @@ -769,23 +786,31 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigat static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState) { - UNUSED(previousState); + navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + + if ((prevFlags & NAV_CTL_POS) == 0) { + resetPositionController(); + } + + if ((prevFlags & NAV_CTL_ALT) == 0) { + resetAltitudeController(); + setupAltitudeController(); + } t_fp_vector targetHoldPos; // If close to home - reset home position if (posControl.homeDistance < posControl.navConfig->min_rth_distance) { setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw); + targetHoldPos = posControl.actualState.pos; + } + else { + calculateInitialHoldPosition(&targetHoldPos); } - resetPositionController(); - resetAltitudeController(); - setupAltitudeController(); - - calculateInitialHoldPosition(&targetHoldPos); - setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); + setDesiredPosition(&targetHoldPos, 0, NAV_POS_UPDATE_XY); - return NAV_FSM_EVENT_SUCCESS; + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) @@ -798,10 +823,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL } if ((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) { - return NAV_FSM_EVENT_SUCCESS; + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME } - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z); + if (posControl.navConfig->flags.rth_tail_first) { + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } + else { + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } return NAV_FSM_EVENT_NONE; } @@ -812,16 +842,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga // If no position sensor available - land immediately if (!posControl.flags.hasValidPositionSensor) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + return NAV_FSM_EVENT_ERROR; // NAV_STATE_RTH_3D_GPS_FAILING } if (isWaypointReached(&posControl.homeWaypointAbove)) { // Successfully reached position target - return NAV_FSM_EVENT_SUCCESS; + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING } else { // Update XYZ-position target - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + if (posControl.navConfig->flags.rth_tail_first) { + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } + else { + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } return NAV_FSM_EVENT_NONE; } } @@ -844,6 +879,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_L // Update XYZ-position target setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); resetLandingDetector(); + return NAV_FSM_EVENT_NONE; } @@ -1448,6 +1484,9 @@ void setDesiredPosition(t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t us else if ((useMask & NAV_POS_UPDATE_BEARING) != 0) { posControl.desiredState.yaw = calculateBearingToDestination(pos); } + else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) { + posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000); + } #if defined(NAV_BLACKBOX) navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767); diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index fd37e2434..4d06daac3 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -63,6 +63,7 @@ typedef struct navConfig_s { uint8_t extra_arming_safety; // Forcibly apply 100% throttle tilt compensation uint8_t user_control_mode; // NAV_GPS_ATTI or NAV_GPS_CRUISE uint8_t rth_alt_control_style; // Controls how RTH controls altitude + uint8_t rth_tail_first; // Return to home tail first } flags; struct { diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 8769cd485..8015aa285 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -46,11 +46,12 @@ #define minFlyableThrottle (posControl.escAndServoConfig->minthrottle + (posControl.escAndServoConfig->maxthrottle - posControl.escAndServoConfig->minthrottle) * 5 / 100) typedef enum { - NAV_POS_UPDATE_NONE = 0, - NAV_POS_UPDATE_XY = 1 << 0, - NAV_POS_UPDATE_Z = 1 << 1, - NAV_POS_UPDATE_HEADING = 1 << 2, - NAV_POS_UPDATE_BEARING = 1 << 3, + NAV_POS_UPDATE_NONE = 0, + NAV_POS_UPDATE_XY = 1 << 0, + NAV_POS_UPDATE_Z = 1 << 1, + NAV_POS_UPDATE_HEADING = 1 << 2, + NAV_POS_UPDATE_BEARING = 1 << 3, + NAV_POS_UPDATE_BEARING_TAIL_FIRST = 1 << 4, } navSetWaypointFlags_t; typedef struct navigationFlags_s { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ad3c55e06..55e659373 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -562,6 +562,7 @@ const clivalue_t valueTable[] = { { "pos_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.pos_hold_deadband, .config.minmax = { 10, 250 }, 0 }, { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.alt_hold_deadband, .config.minmax = { 10, 250 }, 0 }, { "nav_min_rth_distance", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0, 5000 }, 0 }, + { "nav_rth_tail_first", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.rth_tail_first, .config.lookup = { TABLE_OFF_ON }, 0 }, { "nav_rth_alt_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.rth_alt_control_style, .config.lookup = { TABLE_NAV_RTH_ALT_MODE }, 0 }, { "nav_rth_altitude", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.rth_altitude, .config.minmax = { 100, 10000 }, 0 }, { "nav_mc_hover_thr", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.mc_hover_throttle, .config.minmax = { 1000, 2000 }, 0 }, -- 2.11.4.GIT