diff --git a/docs/Settings.md b/docs/Settings.md index 22ad622c92..63ea9a1a07 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -306,6 +306,7 @@ | nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] | | nav_fw_launch_end_time | 3000 | 0 | 5000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | +| nav_fw_launch_idle_motor_delay | 0 | 0 | 20000 | Delay between raising throttle and motor starting at idle throttle (ms) | | nav_fw_launch_idle_thr | 1000 | 1000 | 2000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | | nav_fw_launch_max_altitude | 0 | 0 | 60000 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_launch_max_angle | 45 | 5 | 180 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a7156d480d..8c1b417a72 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -77,7 +77,7 @@ tables: enum: videoSystem_e - name: osd_telemetry values: ["OFF", "ON","TEST"] - enum: osd_telemetry_e + enum: osd_telemetry_e - name: osd_alignment values: ["LEFT", "RIGHT"] enum: osd_alignment_e @@ -2670,6 +2670,12 @@ groups: field: fw.launch_idle_throttle min: 1000 max: 2000 + - name: nav_fw_launch_idle_motor_delay + description: "Delay between raising throttle and motor starting at idle throttle (ms)" + default_value: 0 + field: fw.launch_idle_motor_timer + min: 0 + max: 20000 - name: nav_fw_launch_motor_delay description: "Delay between detected launch and launch sequence start and throttling up (ms)" default_value: 500 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fd7e0c90d3..a258fa58cc 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -185,6 +185,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT, .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms + .launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch .launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode @@ -2578,11 +2579,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti } else { - /* + /* * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 * In other words, when altitude is reached, allow it only to shrink */ - if (navConfig()->general.max_altitude > 0 && + if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0 ) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1809226452..98a88e23a7 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -263,6 +263,7 @@ typedef struct navConfig_s { uint16_t launch_idle_throttle; // Throttle to keep at launch idle uint16_t launch_throttle; // Launch throttle uint16_t launch_motor_timer; // Time to wait before setting launch_throttle (ms) + uint16_t launch_idle_motor_timer; // Time to wait before motor starts at_idle 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 diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 086d9a378d..9a3de29a50 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -54,6 +54,7 @@ #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_IDLE "WAITING FOR IDLE" #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" @@ -61,6 +62,7 @@ typedef enum { FW_LAUNCH_MESSAGE_TYPE_NONE = 0, FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE, + FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE, FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION, FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS, FW_LAUNCH_MESSAGE_TYPE_FINISHING @@ -78,6 +80,7 @@ typedef enum { typedef enum { FW_LAUNCH_STATE_IDLE = 0, FW_LAUNCH_STATE_WAIT_THROTTLE, + FW_LAUNCH_STATE_IDLE_MOTOR_DELAY, FW_LAUNCH_STATE_MOTOR_IDLE, FW_LAUNCH_STATE_WAIT_DETECTION, FW_LAUNCH_STATE_DETECTED, @@ -90,6 +93,7 @@ typedef enum { 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_IDLE_MOTOR_DELAY(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); @@ -125,19 +129,28 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE [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_SUCCESS] = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY, [FW_LAUNCH_EVENT_GOTO_DETECTION] = FW_LAUNCH_STATE_WAIT_DETECTION }, .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE }, + [FW_LAUNCH_STATE_IDLE_MOTOR_DELAY] = { + .onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY, + .onEvent = { + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_IDLE, + [FW_LAUNCH_EVENT_THROTTLE_LOW] = FW_LAUNCH_STATE_WAIT_THROTTLE + }, + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE + }, + [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 + .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE }, [FW_LAUNCH_STATE_WAIT_DETECTION] = { @@ -287,6 +300,21 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs return FW_LAUNCH_EVENT_NONE; } +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs) +{ + if (isThrottleLow()) { + return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE + } + + applyThrottleIdleLogic(true); + + if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) { + return FW_LAUNCH_EVENT_SUCCESS; + } + + return FW_LAUNCH_EVENT_NONE; +} + static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { if (isThrottleLow()) { @@ -476,6 +504,9 @@ const char * fixedWingLaunchStateMessage(void) case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE: return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE; + case FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE: + return FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE; + case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION: return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;