diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 415032f758d..d7ae44f9d83 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 625f152ff1d..ce5470eb460 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 4e0e7ae93b2..4d7e035919c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -785,19 +785,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: @@ -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; } @@ -3343,9 +3346,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 bd912a1b4ff..01670a320e0 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" @@ -216,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; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 574c5710a4b..b59307dff41 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"); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index da53648ab70..ec8047f35eb 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -498,7 +498,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 c50f03b86b2..b6403e937cc 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -386,6 +386,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 } navSystemStatus_State_e; typedef enum { diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index d87454baa1b..0b3c1633bed 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,17 +172,21 @@ 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 = { [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 } @@ -184,39 +194,47 @@ 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) { - if (isThrottleIdleEnabled()) { +static void applyThrottleIdleLogic(bool forceMixerIdle) +{ + if (isThrottleIdleEnabled() && !forceMixerIdle) { 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 { + 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 } } -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(); } @@ -228,7 +246,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]); @@ -237,37 +256,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 { + applyThrottleIdleLogic(true); // Stick low, force mixer idle (motor stop or low rpm) + } + 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(false); 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); } @@ -275,11 +304,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))); @@ -287,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); - applyThrottleIdle(); + applyThrottleIdleLogic(false); if (isBungeeLaunched || isSwingLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { @@ -300,20 +330,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(false); 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(false); 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; } @@ -321,7 +354,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 } @@ -333,7 +367,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); } @@ -341,15 +376,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 } @@ -357,13 +399,18 @@ 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; + 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 { + } + 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]); @@ -374,9 +421,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); @@ -392,41 +440,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; }