diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index eee214a26f..020ee369f5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -361,7 +361,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE | IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON | IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST | - IS_ENABLED(FLIGHT_MODE(LAUNCH_MODE)) << BOXNAVLAUNCH | + IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)) << BOXNAVLAUNCH | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET; uint32_t ret = 0; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 47a46e4f40..847667e44a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -39,7 +39,7 @@ typedef enum { NAV_RTH_MODE = (1 << 4), // old GPS_HOME NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD HEADFREE_MODE = (1 << 6), - LAUNCH_MODE = (1 << 7), + NAV_LAUNCH_MODE = (1 << 7), PASSTHRU_MODE = (1 << 8), FAILSAFE_MODE = (1 << 10), UNUSED_MODE = (1 << 11), // old G-Tune diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index b144b48a36..0f4b654f09 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -638,7 +638,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE, .timeoutMs = 0, .stateFlags = NAV_REQUIRE_ANGLE, - .mapToFlightModes = LAUNCH_MODE, + .mapToFlightModes = NAV_LAUNCH_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { @@ -652,10 +652,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT, .timeoutMs = 10, .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE, - .mapToFlightModes = LAUNCH_MODE, + .mapToFlightModes = NAV_LAUNCH_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, @@ -666,10 +667,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS, .timeoutMs = 10, .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE, - .mapToFlightModes = LAUNCH_MODE, + .mapToFlightModes = NAV_LAUNCH_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, @@ -1278,7 +1280,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navig const uint32_t currentTime = micros(); UNUSED(previousState); - resetFixedWingLaunchMode(currentTime); + resetFixedWingLaunchController(currentTime); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_WAIT } @@ -1289,7 +1291,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF UNUSED(previousState); if (isFixedWingLaunchDetected()) { - resetFixedWingLaunchController(currentTime); + enableFixedWingLaunchController(currentTime); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_MOTOR_DELAY } @@ -2232,7 +2234,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void swithNavigationFlightModes(void) { flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE) & (~enabledNavFlightModes); + flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -2264,15 +2266,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // LAUNCH mode has priority over any other NAV mode if (STATE(FIXED_WING)) { if (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH)) { // FIXME: Only available for fixed wing aircrafts now - if (FLIGHT_MODE(LAUNCH_MODE) || canActivateLaunchMode) { + if (canActivateLaunchMode) { canActivateLaunchMode = false; return NAV_FSM_EVENT_SWITCH_TO_LAUNCH; } + else if FLIGHT_MODE(NAV_LAUNCH_MODE) { + // Make sure we don't bail out to IDLE + return NAV_FSM_EVENT_NONE; + } } else { // If we were in LAUNCH mode - force switch to IDLE - if (FLIGHT_MODE(LAUNCH_MODE)) + if (FLIGHT_MODE(NAV_LAUNCH_MODE)) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } } } diff --git a/src/main/flight/navigation_rewrite_fw_launch.c b/src/main/flight/navigation_rewrite_fw_launch.c index c937b00062..9a69882329 100755 --- a/src/main/flight/navigation_rewrite_fw_launch.c +++ b/src/main/flight/navigation_rewrite_fw_launch.c @@ -62,13 +62,18 @@ typedef struct FixedWingLaunchState_s { /* Launch progress */ uint32_t launchStartedTime; bool launchFinished; + bool motorControlAllowed; } FixedWingLaunchState_t; static FixedWingLaunchState_t launchState; +#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe static void updateFixedWingLaunchDetector(const uint32_t currentTime) { - if (imuAccelInBodyFrame.A[X] > posControl.navConfig->fw_launch_accel_thresh) { + const bool isForwardAccelerationHigh = (imuAccelInBodyFrame.A[X] > posControl.navConfig->fw_launch_accel_thresh); + const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= COS_MAX_LAUNCH_ANGLE); + + if (isForwardAccelerationHigh && isAircraftAlmostLevel) { launchState.launchDetectionTimeAccum += (currentTime - launchState.launchDetectorPreviosUpdate); launchState.launchDetectorPreviosUpdate = currentTime; if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)posControl.navConfig->fw_launch_time_thresh)) { @@ -81,13 +86,14 @@ static void updateFixedWingLaunchDetector(const uint32_t currentTime) } } -void resetFixedWingLaunchMode(const uint32_t currentTime) +void resetFixedWingLaunchController(const uint32_t currentTime) { launchState.launchDetectorPreviosUpdate = currentTime; launchState.launchDetectionTimeAccum = 0; launchState.launchStartedTime = 0; launchState.launchDetected = false; launchState.launchFinished = false; + launchState.motorControlAllowed = false; } bool isFixedWingLaunchDetected(void) @@ -95,9 +101,10 @@ bool isFixedWingLaunchDetected(void) return launchState.launchDetected; } -void resetFixedWingLaunchController(const uint32_t currentTime) +void enableFixedWingLaunchController(const uint32_t currentTime) { launchState.launchStartedTime = currentTime; + launchState.motorControlAllowed = true; } bool isFixedWingLaunchFinishedOrAborted(void) @@ -109,7 +116,7 @@ void applyFixedWingLaunchController(const uint32_t currentTime) { // Called at PID rate - if (isFixedWingLaunchDetected()) { + if (launchState.launchDetected) { // If launch detected we are in launch procedure - control airplane const float timeElapsedSinceLaunchMs = US2MS(currentTime - launchState.launchStartedTime); @@ -119,20 +126,15 @@ void applyFixedWingLaunchController(const uint32_t currentTime) } // Control throttle - if (timeElapsedSinceLaunchMs < posControl.navConfig->fw_launch_motor_timer) { + if (timeElapsedSinceLaunchMs >= posControl.navConfig->fw_launch_motor_timer && launchState.motorControlAllowed) { + rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle; + } + else { // Until motors are started don't use PID I-term pidResetErrorAccumulators(); // Throttle control logic - if motor stop is enabled - keep motors stopped - if (feature(FEATURE_MOTOR_STOP)) { - rcCommand[THROTTLE] = posControl.escAndServoConfig->mincommand; - } - else { - rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle; - } - } - else { - rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle; + rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle; } } else { @@ -143,12 +145,7 @@ void applyFixedWingLaunchController(const uint32_t currentTime) pidResetErrorAccumulators(); // Throttle control logic - if (feature(FEATURE_MOTOR_STOP)) { - rcCommand[THROTTLE] = posControl.escAndServoConfig->mincommand; - } - else { - rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle; - } + rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle; } // Lock out controls diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 80b4b6ee79..a9df3fe048 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -365,9 +365,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, void calculateFixedWingInitialHoldPosition(t_fp_vector * pos); /* Fixed-wing launch controller */ -void resetFixedWingLaunchMode(const uint32_t currentTime); -bool isFixedWingLaunchDetected(void); void resetFixedWingLaunchController(const uint32_t currentTime); +bool isFixedWingLaunchDetected(void); +void enableFixedWingLaunchController(const uint32_t currentTime); bool isFixedWingLaunchFinishedOrAborted(void); void applyFixedWingLaunchController(const uint32_t currentTime);