From 0b38af6250e97b30937db9b1e36efe582e3a7734 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 13 Oct 2020 22:04:10 +0100 Subject: [PATCH 1/7] OSD Nav Message Update System Messages for: RTH Climb Phase To WP number countdown WP Hold countdown timer --- src/main/io/osd.c | 30 ++++++++++++++++++++++-------- src/main/io/osd.h | 1 + src/main/navigation/navigation.c | 2 +- src/main/navigation/navigation.h | 2 ++ 4 files changed, 26 insertions(+), 9 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4a4f951f1c..4c8700e4f6 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -784,19 +784,19 @@ static const char * navigationStateMessage(void) break; case MW_NAV_STATE_RTH_START: return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); + case MW_NAV_STATE_RTH_CLIMB: + return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); case MW_NAV_STATE_RTH_ENROUTE: - // TODO: Break this up between climb and head home return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); case MW_NAV_STATE_HOLD_INFINIT: // Used by HOLD flight modes. No information to add. break; case MW_NAV_STATE_HOLD_TIMED: - // TODO: Maybe we can display a count down - return OSD_MESSAGE_STR(OSD_MSG_HOLDING_WAYPOINT); + // "HOLDING WP FOR xx S" Countdown added in osdGetSystemMessage break; case MW_NAV_STATE_WP_ENROUTE: - // TODO: Show WP number - return OSD_MESSAGE_STR(OSD_MSG_TO_WP); + // "TO WP" + WP countdown added in osdGetSystemMessage + break; case MW_NAV_STATE_PROCESS_NEXT: return OSD_MESSAGE_STR(OSD_MSG_PREPARE_NEXT_WP); case MW_NAV_STATE_DO_JUMP: @@ -3301,9 +3301,23 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; + if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { + // Countdown display for remaining Waypoints + tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); + messages[messageCount++] = messageBuf; + } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { + // WP hold time countdown in seconds + timeMs_t currentTime = millis(); + int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)((currentTime - posControl.wpReachedTime)/1000); + if (holdTimeRemaining >=0) { + tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); + messages[messageCount++] = messageBuf; + } + } else { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 72576100a3..8530e71c22 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -80,6 +80,7 @@ #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" #define OSD_MSG_STARTING_RTH "STARTING RTH" +#define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" #define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" #define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT" #define OSD_MSG_TO_WP "TO WP" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c16aa048eb..44f5a404ba 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -491,7 +491,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .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, + .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1d8b853902..dff30a56c4 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -377,6 +377,8 @@ typedef enum { MW_NAV_STATE_LAND_START_DESCENT, // Start descent MW_NAV_STATE_HOVER_ABOVE_HOME, // Hover/Loitering above home MW_NAV_STATE_EMERGENCY_LANDING, // Emergency landing + MW_NAV_STATE_RTH_CLIMB, // RTH Climb safe altitude + } navSystemStatus_State_e; typedef enum { From 08866626c63cb1c3e76e3a9a085020f8fe10607a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 14 Oct 2020 08:49:28 +0100 Subject: [PATCH 2/7] Fix Errors --- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 96a350a17b..fd611a8547 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2111,7 +2111,7 @@ groups: description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details" default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode - table: nav_rth_alt_mode + table: nav_rth_alt_mode - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: "50000" diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index dff30a56c4..1f38d97722 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -377,8 +377,7 @@ typedef enum { MW_NAV_STATE_LAND_START_DESCENT, // Start descent MW_NAV_STATE_HOVER_ABOVE_HOME, // Hover/Loitering above home MW_NAV_STATE_EMERGENCY_LANDING, // Emergency landing - MW_NAV_STATE_RTH_CLIMB, // RTH Climb safe altitude - + MW_NAV_STATE_RTH_CLIMB, // RTH Climb safe altitude } navSystemStatus_State_e; typedef enum { From b91acef47e7727c025c003362910c164511cc654 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 17 Nov 2020 22:00:33 +0000 Subject: [PATCH 3/7] Update osd_dji_hd.c --- src/main/io/osd_dji_hd.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 574c5710a4..b59307dff4 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -574,6 +574,8 @@ static const char * navigationStateMessage(void) break; case MW_NAV_STATE_RTH_START: return OSD_MESSAGE_STR("STARTING RTH"); + case MW_NAV_STATE_RTH_CLIMB: + return OSD_MESSAGE_STR("ADJUSTING RTH ALTITUDE"); case MW_NAV_STATE_RTH_ENROUTE: // TODO: Break this up between climb and head home return OSD_MESSAGE_STR("EN ROUTE TO HOME"); From 7df04133b6b4c736ac11ab4408c3485a45edf2c2 Mon Sep 17 00:00:00 2001 From: Mihai Dragnea Date: Thu, 19 Nov 2020 09:47:35 +0200 Subject: [PATCH 4/7] No control during launch FINISHING --- src/main/navigation/navigation_fw_launch.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index d87454baa1..88ca2a2625 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -170,13 +170,15 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING } @@ -344,10 +346,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - if (isLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state - } if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state + } + if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { @@ -361,6 +363,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; + if (areSticksDeflectedMoreThanPosHoldDeadband()) { + return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state + } if (elapsedTimeMs > endTimeMs) { return FW_LAUNCH_EVENT_SUCCESS; } else { From a3e7e3cb15ea85cc38cb946c059410a0cc526490 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Thu, 19 Nov 2020 08:44:30 +0100 Subject: [PATCH 5/7] [LAUNCH] Some readability refactoring; Fix bug with throttling up at zero throttle --- src/main/navigation/navigation_fw_launch.c | 135 +++++++++++++++------ 1 file changed, 97 insertions(+), 38 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 88ca2a2625..4379f6f962 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -119,6 +119,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE }, + [FW_LAUNCH_STATE_WAIT_THROTTLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, .onEvent = { @@ -127,6 +128,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE }, + [FW_LAUNCH_STATE_MOTOR_IDLE] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, .onEvent = { @@ -135,6 +137,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE }, + [FW_LAUNCH_STATE_WAIT_DETECTION] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, .onEvent = { @@ -143,6 +146,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION }, + [FW_LAUNCH_STATE_DETECTED] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, .onEvent = { @@ -150,6 +154,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION }, + [FW_LAUNCH_STATE_MOTOR_DELAY] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEvent = { @@ -158,6 +163,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, + [FW_LAUNCH_STATE_MOTOR_SPINUP] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEvent = { @@ -166,6 +172,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, + [FW_LAUNCH_STATE_IN_PROGRESS] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { @@ -174,6 +181,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, + [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { @@ -186,39 +194,52 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE /* Current State Handlers */ -static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { +static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) +{ return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); } -static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { +static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) +{ fwLaunch.currentState = nextState; fwLaunch.currentStateTimeUs = currentTimeUs; } /* Wing control Helpers */ -static bool isThrottleIdleEnabled(void) { +static bool isThrottleIdleEnabled(void) +{ return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); } -static void applyThrottleIdle(void) { +static void forceMotorStopOrIdle(void) +{ + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle +} + +static void applyThrottleIdleLogic(void) +{ if (isThrottleIdleEnabled()) { rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; - } else { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle + } + else { + forceMotorStopOrIdle(); } } -static inline bool isThrottleLow(void) { +static inline bool isThrottleLow(void) +{ return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; } -static inline bool isLaunchMaxAltitudeReached(void) { +static inline bool isLaunchMaxAltitudeReached(void) +{ return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); } -static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { +static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) +{ return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); } @@ -230,7 +251,8 @@ static void resetPidsIfNeeded(void) { } } -static void updateRcCommand(void) { +static void updateRcCommand(void) +{ // lock roll and yaw and apply needed pitch angle rcCommand[ROLL] = 0; rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); @@ -239,37 +261,47 @@ static void updateRcCommand(void) { /* onEntry state handlers */ -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) +{ UNUSED(currentTimeUs); return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) +{ UNUSED(currentTimeUs); if (!isThrottleLow()) { if (isThrottleIdleEnabled()) { return FW_LAUNCH_EVENT_SUCCESS; - } else { + } + else { fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; return FW_LAUNCH_EVENT_GOTO_DETECTION; } } + else { + forceMotorStopOrIdle(); + } + fwLaunch.pitchAngle = 0; return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) +{ if (isThrottleLow()) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } + const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { - applyThrottleIdle(); + applyThrottleIdleLogic(); return FW_LAUNCH_EVENT_SUCCESS; - } else { + } + else { rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); } @@ -277,11 +309,12 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) +{ if (isThrottleLow()) { return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE } - + const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); @@ -289,7 +322,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - applyThrottleIdle(); + applyThrottleIdleLogic(); if (isBungeeLaunched || isSwingLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { @@ -302,20 +335,23 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) +{ UNUSED(currentTimeUs); // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY - applyThrottleIdle(); + applyThrottleIdleLogic(); return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { - applyThrottleIdle(); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) +{ + applyThrottleIdleLogic(); if (areSticksMoved(0, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE } + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { return FW_LAUNCH_EVENT_SUCCESS; } @@ -323,7 +359,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) +{ if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE } @@ -335,7 +372,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ if (elapsedTimeMs > motorSpinUpMs) { rcCommand[THROTTLE] = launchThrottle; return FW_LAUNCH_EVENT_SUCCESS; - } else { + } + else { const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); } @@ -343,15 +381,22 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) +{ rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; + if (isLaunchMaxAltitudeReached()) { + return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state + } + if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state } + if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH } @@ -359,7 +404,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t return FW_LAUNCH_EVENT_NONE; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) { +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) +{ const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; @@ -368,7 +414,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr } if (elapsedTimeMs > endTimeMs) { return FW_LAUNCH_EVENT_SUCCESS; - } else { + } + else { // make a smooth transition from the launch state to the current state for throttle and the pitch angle rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_throttle, rcCommand[THROTTLE]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); @@ -379,9 +426,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr // Public methods --------------------------------------------------------------- -void applyFixedWingLaunchController(timeUs_t currentTimeUs) { +void applyFixedWingLaunchController(timeUs_t currentTimeUs) +{ // Called at PID rate - + // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE while (launchStateMachine[fwLaunch.currentState].onEntry) { fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs); @@ -397,41 +445,52 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) { // Control beeper if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); - } else { + } + else { beeper(BEEPER_LAUNCH_MODE_ENABLED); } } -void resetFixedWingLaunchController(timeUs_t currentTimeUs) { +void resetFixedWingLaunchController(timeUs_t currentTimeUs) +{ setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); } -bool isFixedWingLaunchDetected(void) { +bool isFixedWingLaunchDetected(void) +{ return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; } -void enableFixedWingLaunchController(timeUs_t currentTimeUs) { +void enableFixedWingLaunchController(timeUs_t currentTimeUs) +{ setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); } -bool isFixedWingLaunchFinishedOrAborted(void) { +bool isFixedWingLaunchFinishedOrAborted(void) +{ return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; } -void abortFixedWingLaunch(void) { +void abortFixedWingLaunch(void) +{ setCurrentState(FW_LAUNCH_STATE_IDLE, 0); } -const char * fixedWingLaunchStateMessage(void) { +const char * fixedWingLaunchStateMessage(void) +{ switch (launchStateMachine[fwLaunch.currentState].messageType) { case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE: return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE; + case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION: return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION; + case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS: return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS; + case FW_LAUNCH_MESSAGE_TYPE_FINISHING: return FW_LAUNCH_MESSAGE_TEXT_FINISHING; + default: return NULL; } From fd73511053ca9edd6502a6a6d60b16d23af7628e Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Thu, 19 Nov 2020 09:55:08 +0100 Subject: [PATCH 6/7] Force mixer idle in launch mode while throttle stick is low --- src/main/navigation/navigation_fw_launch.c | 23 +++++++++------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 4379f6f962..0b3c1633be 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -212,19 +212,14 @@ static bool isThrottleIdleEnabled(void) return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); } -static void forceMotorStopOrIdle(void) +static void applyThrottleIdleLogic(bool forceMixerIdle) { - ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped - rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle -} - -static void applyThrottleIdleLogic(void) -{ - if (isThrottleIdleEnabled()) { + if (isThrottleIdleEnabled() && !forceMixerIdle) { rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; } else { - forceMotorStopOrIdle(); + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped + rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin given throttle value } } @@ -282,7 +277,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs } } else { - forceMotorStopOrIdle(); + applyThrottleIdleLogic(true); // Stick low, force mixer idle (motor stop or low rpm) } fwLaunch.pitchAngle = 0; @@ -298,7 +293,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { - applyThrottleIdleLogic(); + applyThrottleIdleLogic(false); return FW_LAUNCH_EVENT_SUCCESS; } else { @@ -322,7 +317,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - applyThrottleIdleLogic(); + applyThrottleIdleLogic(false); if (isBungeeLaunched || isSwingLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { @@ -339,14 +334,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t cu { UNUSED(currentTimeUs); // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY - applyThrottleIdleLogic(); + applyThrottleIdleLogic(false); return FW_LAUNCH_EVENT_NONE; } static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { - applyThrottleIdleLogic(); + applyThrottleIdleLogic(false); if (areSticksMoved(0, currentTimeUs)) { return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE From 27266e34371b67fa5dbcb3ad26cd16605d8e01ed Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 20 Nov 2020 19:58:56 +0000 Subject: [PATCH 7/7] Add control smoothness adjustment (#6311) Add control smoothness adjustment --- src/main/fc/rc_adjustments.c | 7 +++++++ src/main/fc/rc_adjustments.h | 1 + src/main/io/osd.c | 3 +++ src/main/io/osd.h | 1 + 4 files changed, 12 insertions(+) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 415032f758..d7ae44f9d8 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -276,6 +276,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 5 }} + }, { + .adjustmentFunction = ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} } }; @@ -566,6 +570,9 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_TPA_BREAKPOINT: applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); break; + case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: + applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, 0, 9); + break; default: break; }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 625f152ff1..ce5470eb46 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -77,6 +77,7 @@ typedef enum { ADJUSTMENT_VTX_POWER_LEVEL = 49, ADJUSTMENT_TPA = 50, ADJUSTMENT_TPA_BREAKPOINT = 51, + ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 52, ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dcf7ff8db9..4d7e035919 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2437,6 +2437,9 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_NAV_FW_CONTROL_SMOOTHNESS: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); + return true; default: return false; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 1a6230c676..01670a320e 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -217,6 +217,7 @@ typedef enum { OSD_GVAR_2, OSD_GVAR_3, OSD_TPA, + OSD_NAV_FW_CONTROL_SMOOTHNESS, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e;