diff --git a/docs/Cli.md b/docs/Cli.md index 04332e2fbf..d177c717c7 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -147,4 +147,5 @@ For targets that have a flash data chip, typically used for blackbox logs, the f | `flash_write
` | Writes `data` to `address` | ## CLI Variable Reference + See [Settings.md](Settings.md). diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 2a70ce6fe6..5e09da1066 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -134,7 +134,6 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] = OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR), OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME), OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT), - OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME), OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE), OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE), OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 62c5496df7..c432341cf3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2371,12 +2371,6 @@ groups: field: fw.launch_motor_spinup_time min: 0 max: 1000 - - name: nav_fw_launch_end_time - description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" - default_value: "2000" - field: fw.launch_end_time - min: 0 - max: 5000 - name: nav_fw_launch_min_time description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]." default_value: "0" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b951ec7ebc..80c10d85fb 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3327,10 +3327,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); - const char *launchStateMessage = fixedWingLaunchStateMessage(); - if (launchStateMessage) { - messages[messageCount++] = launchStateMessage; - } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ba319442ca..a287162174 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -171,7 +171,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP .launch_motor_timer = 500, // ms .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch - .launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode .launch_min_time = 0, // ms, min time in launch mode .launch_timeout = 5000, // ms, timeout for launch procedure .launch_max_altitude = 0, // cm, altitude where to consider launch ended diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f5f0aa5990..36fbd2b29e 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,8 +239,7 @@ typedef struct navConfig_s { uint16_t launch_throttle; // Launch throttle uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) uint16_t launch_motor_spinup_time; // Time to speed-up motors from idle to launch_throttle (ESC desync prevention) - uint16_t launch_end_time; // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position - uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early + uint16_t launch_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early uint16_t launch_timeout; // Launch timeout to disable launch mode and swith to normal flight (ms) uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) @@ -525,7 +524,6 @@ bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchFinishedOrAborted(void); -const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index d87454baa1..309bef0cf0 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -33,6 +33,15 @@ #include "config/feature.h" #include "drivers/time.h" +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/rangefinder.h" +#include "sensors/barometer.h" +#include "sensors/acceleration.h" +#include "sensors/boardalignment.h" +#include "sensors/compass.h" #include "io/gps.h" #include "io/beeper.h" @@ -48,238 +57,25 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" + +typedef struct FixedWingLaunchState_s { + /* Launch detection */ + timeUs_t launchDetectorPreviousUpdate; + timeUs_t launchDetectionTimeAccum; + bool launchDetected; + + /* Launch progress */ + timeUs_t launchStartedTime; + bool launchFinished; + bool motorControlAllowed; +} FixedWingLaunchState_t; + +static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; + +#define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate -#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms -#define UNUSED(x) ((void)(x)) -#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE" -#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY" -#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT" -#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING" - -typedef enum { - FW_LAUNCH_MESSAGE_TYPE_NONE = 0, - FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE, - FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION, - FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS, - FW_LAUNCH_MESSAGE_TYPE_FINISHING -} fixedWingLaunchMessage_t; - -typedef enum { - FW_LAUNCH_EVENT_NONE = 0, - FW_LAUNCH_EVENT_SUCCESS, - FW_LAUNCH_EVENT_GOTO_DETECTION, - FW_LAUNCH_EVENT_ABORT, - FW_LAUNCH_EVENT_THROTTLE_LOW, - FW_LAUNCH_EVENT_COUNT -} fixedWingLaunchEvent_t; - -typedef enum { - FW_LAUNCH_STATE_IDLE = 0, - FW_LAUNCH_STATE_WAIT_THROTTLE, - FW_LAUNCH_STATE_MOTOR_IDLE, - FW_LAUNCH_STATE_WAIT_DETECTION, - FW_LAUNCH_STATE_DETECTED, - FW_LAUNCH_STATE_MOTOR_DELAY, - FW_LAUNCH_STATE_MOTOR_SPINUP, - FW_LAUNCH_STATE_IN_PROGRESS, - FW_LAUNCH_STATE_FINISH, - FW_LAUNCH_STATE_COUNT -} fixedWingLaunchState_t; - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs); -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs); - -typedef struct fixedWingLaunchStateDescriptor_s { - fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs); - fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT]; - fixedWingLaunchMessage_t messageType; -} fixedWingLaunchStateDescriptor_t; - -typedef struct fixedWingLaunchData_s { - timeUs_t currentStateTimeUs; - fixedWingLaunchState_t currentState; - uint8_t pitchAngle; // used to smooth the transition of the pitch angle -} fixedWingLaunchData_t; - -static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; - -static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { - - [FW_LAUNCH_STATE_IDLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE, - .onEvent = { - - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE - }, - [FW_LAUNCH_STATE_WAIT_THROTTLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE, - [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE - }, - [FW_LAUNCH_STATE_MOTOR_IDLE] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_WAIT_DETECTION, - [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE - }, - [FW_LAUNCH_STATE_WAIT_DETECTION] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_DETECTED, - [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION - }, - [FW_LAUNCH_STATE_DETECTED] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, - .onEvent = { - // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION - }, - [FW_LAUNCH_STATE_MOTOR_DELAY] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS - }, - [FW_LAUNCH_STATE_MOTOR_SPINUP] = { - .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, - .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS, - [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE - }, - .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, - }, - .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 - }, - .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING - } -}; - -/* Current State Handlers */ - -static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { - return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); -} - -static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { - fwLaunch.currentState = nextState; - fwLaunch.currentStateTimeUs = currentTimeUs; -} - -/* Wing control Helpers */ - -static bool isThrottleIdleEnabled(void) { - return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); -} - -static void applyThrottleIdle(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 - } -} - -static inline bool isThrottleLow(void) { - return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; -} - -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) { - return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); -} - -static void resetPidsIfNeeded(void) { - // Until motors are started don't use PID I-term and reset TPA filter - if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) { - pidResetErrorAccumulators(); - pidResetTPAFilter(); - } -} - -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]); - rcCommand[YAW] = 0; -} - -/* onEntry state handlers */ - -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) { - UNUSED(currentTimeUs); - - if (!isThrottleLow()) { - if (isThrottleIdleEnabled()) { - return FW_LAUNCH_EVENT_SUCCESS; - } else { - fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; - return FW_LAUNCH_EVENT_GOTO_DETECTION; - } - } - fwLaunch.pitchAngle = 0; - - return FW_LAUNCH_EVENT_NONE; -} - -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(); - return FW_LAUNCH_EVENT_SUCCESS; - } 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); - } - - return FW_LAUNCH_EVENT_NONE; -} - -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 - } - +static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) +{ 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,149 +83,160 @@ 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(); - if (isBungeeLaunched || isSwingLaunched) { - if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { - return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED + launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); + launchState.launchDetectorPreviousUpdate = currentTimeUs; + if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { + launchState.launchDetected = true; } - } else { - fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter } - - return FW_LAUNCH_EVENT_NONE; + else { + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; + } } -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(); - - return FW_LAUNCH_EVENT_NONE; +void resetFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; + launchState.launchStartedTime = 0; + launchState.launchDetected = false; + launchState.launchFinished = false; + launchState.motorControlAllowed = false; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) { - applyThrottleIdle(); - - 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; - } - - return FW_LAUNCH_EVENT_NONE; +bool isFixedWingLaunchDetected(void) +{ + return launchState.launchDetected; } -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 - } - - const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); - const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const uint16_t launchThrottle = navConfig()->fw.launch_throttle; - - if (elapsedTimeMs > motorSpinUpMs) { - rcCommand[THROTTLE] = launchThrottle; - return FW_LAUNCH_EVENT_SUCCESS; - } else { - const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); - } - - return FW_LAUNCH_EVENT_NONE; +void enableFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchStartedTime = currentTimeUs; + launchState.motorControlAllowed = true; } -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_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 - } - - return FW_LAUNCH_EVENT_NONE; +bool isFixedWingLaunchFinishedOrAborted(void) +{ + return launchState.launchFinished; } -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 (elapsedTimeMs > endTimeMs) { - return FW_LAUNCH_EVENT_SUCCESS; - } 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]); - } - - return FW_LAUNCH_EVENT_NONE; +void abortFixedWingLaunch(void) +{ + launchState.launchFinished = true; } -// Public methods --------------------------------------------------------------- +#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms -void applyFixedWingLaunchController(timeUs_t currentTimeUs) { +static void applyFixedWingLaunchIdleLogic(void) +{ + // Until motors are started don't use PID I-term + pidResetErrorAccumulators(); + + // We're not flying yet, reset TPA filter + pidResetTPAFilter(); + + // Throttle control logic + if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue()) + { + 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 + { + static float timeThrottleRaisedMs; + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) + { + timeThrottleRaisedMs = millis(); + } + else + { + const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME); + rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, + 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, + getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + } + } +} + +static inline bool isFixedWingLaunchMaxAltitudeReached(void) +{ + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +} +static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs) +{ + return timeSinceLaunchMs > navConfig()->fw.launch_min_time; +} + +static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) +{ + return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; +} + +static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) +{ + return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); +} + +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); - if (newEvent == FW_LAUNCH_EVENT_NONE) { - break; - } - setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs); - } - resetPidsIfNeeded(); - updateRcCommand(); + if (launchState.launchDetected) { + bool applyLaunchIdleLogic = true; + + if (launchState.motorControlAllowed) { + // If launch detected we are in launch procedure - control airplane + const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime); + + launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs); + + // Motor control enabled + if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) { + // Don't apply idle logic anymore + applyLaunchIdleLogic = false; + + // Increase throttle gradually over `launch_motor_spinup_time` milliseconds + if (navConfig()->fw.launch_motor_spinup_time > 0) { + const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time); + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs, + 0.0f, navConfig()->fw.launch_motor_spinup_time, + minIdleThrottle, navConfig()->fw.launch_throttle); + } + else { + rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; + } + } + } + + if (applyLaunchIdleLogic) { + // Launch idle logic - low throttle and zero out PIDs + applyFixedWingLaunchIdleLogic(); + } + } + else { + // We are waiting for launch - update launch detector + updateFixedWingLaunchDetector(currentTimeUs); + + // Launch idle logic - low throttle and zero out PIDs + applyFixedWingLaunchIdleLogic(); + } // Control beeper - if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { - beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); - } else { - beeper(BEEPER_LAUNCH_MODE_ENABLED); + if (!launchState.launchFinished) { + if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { + beeper(BEEPER_LAUNCH_MODE_ENABLED); + } else { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } } -} -void resetFixedWingLaunchController(timeUs_t currentTimeUs) { - setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); -} - -bool isFixedWingLaunchDetected(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; -} - -void enableFixedWingLaunchController(timeUs_t currentTimeUs) { - setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); -} - -bool isFixedWingLaunchFinishedOrAborted(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; -} - -void abortFixedWingLaunch(void) { - setCurrentState(FW_LAUNCH_STATE_IDLE, 0); -} - -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; - } + // Lock out controls + rcCommand[ROLL] = 0; + rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[YAW] = 0; } #endif