From ec44b038ec9f8ff7acd73cdd4fde10e7963eeeb0 Mon Sep 17 00:00:00 2001 From: Airwide Date: Tue, 29 Sep 2020 20:59:43 +0200 Subject: [PATCH 01/17] Changed throttle field in osd to always show throttle output --- src/main/io/osd.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9128f571b32..2d60c1e391d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -867,20 +867,17 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) } /** - * Formats throttle position prefixed by its symbol. If autoThr - * is true and the navigation system is controlling THR, it - * uses the THR value applied by the system rather than the - * input value received by the sticks. + * Formats throttle position prefixed by its symbol. + * Shows output to motor, not stick position **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = rxGetChannelValue(THROTTLE); + int16_t thr = rcCommand[THROTTLE]; if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; - thr = rcCommand[THROTTLE]; if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } From 6e272e07119f4d7d02ac8f73348af5214c529c70 Mon Sep 17 00:00:00 2001 From: Airwide Date: Thu, 1 Oct 2020 21:08:40 +0200 Subject: [PATCH 02/17] Rescaled manual throttle in OSD from 0 to 100% --- src/main/io/osd.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2d60c1e391d..38fd5e96714 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -874,14 +874,15 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t { buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = rcCommand[THROTTLE]; + int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - motorConfig()->minthrottle) * 100 / (motorConfig()->maxthrottle - motorConfig()->minthrottle); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; + thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } - tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + tfp_sprintf(buff + 2, "%3d", thr); } #if defined(USE_ESC_SENSOR) From b76c354dc5dab93b5f61bf5c98048af705486a6b Mon Sep 17 00:00:00 2001 From: Airwide Date: Fri, 2 Oct 2020 06:46:50 +0200 Subject: [PATCH 03/17] Fixed minThrottle value --- src/main/io/osd.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 38fd5e96714..68723b0c307 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -872,9 +872,10 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { + const int minThrottle = getThrottleIdleValue(); buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - motorConfig()->minthrottle) * 100 / (motorConfig()->maxthrottle - motorConfig()->minthrottle); + int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; From e965eb2db06d2efda337ddbbe0c731f90dc83964 Mon Sep 17 00:00:00 2001 From: scavanger Date: Wed, 28 Oct 2020 23:46:36 +0100 Subject: [PATCH 04/17] Add TPA to inflight adjustment (+ OSD element) --- src/main/fc/rc_adjustments.c | 14 ++++++++++++++ src/main/fc/rc_adjustments.h | 4 +++- src/main/io/osd.c | 22 ++++++++++++++++++++++ src/main/io/osd.h | 1 + 4 files changed, 40 insertions(+), 1 deletion(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 644dd19b4ff..13844ac8c29 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -268,6 +268,14 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_VTX_POWER_LEVEL, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_TPA, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 5 }} #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT }, { .adjustmentFunction = ADJUSTMENT_PROFILE, @@ -558,6 +566,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t } break; #endif + case ADJUSTMENT_TPA: + applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX); + break; + case ADJUSTMENT_TPA_BREAKPOINT: + applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); + break; default: break; }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index b51cf390d60..75214d6c687 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -75,8 +75,10 @@ typedef enum { ADJUSTMENT_VEL_Z_D = 47, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, ADJUSTMENT_VTX_POWER_LEVEL = 49, + ADJUSTMENT_TPA = 50, + ADJUSTMENT_TPA_BREAKPOINT = 51, #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 50, + ADJUSTMENT_PROFILE = 52, #endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 300d98fb86a..70c6d7fddf7 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2411,7 +2411,29 @@ static bool osdDrawSingleElement(uint8_t item) return true; } #endif + case OSD_TPA: + { + char buff[4]; + textAttributes_t attr; + + displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA BP"); + + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, attr); + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "BP %4d", currentControlRateProfile->throttle.pa_breakpoint); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, attr); + + return true; + } default: return false; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index dba1d7ec313..64df80b570f 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -215,6 +215,7 @@ typedef enum { OSD_GVAR_1, OSD_GVAR_2, OSD_GVAR_3, + OSD_TPA, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From ab60dfb0e21b889009e3605c742931d1331d4b67 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 15 Nov 2020 12:31:38 +0000 Subject: [PATCH 05/17] [DOC] Cli.md: add missing 2.6 items (bind_rx, safehome), make references clickable. (#6290) --- docs/Cli.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 3b059bb6b67..04332e2fbf0 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -65,7 +65,8 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `1wire ` | passthrough 1wire to the specified esc | | `adjrange` | show/set adjustment ranges settings | | `aux` | show/set aux settings | -| `beeper` | show/set beeper (buzzer) usage (see docs/Buzzer.md) | +| `beeper` | show/set beeper (buzzer) [usage](Buzzer.md) | +| `bind_rx` | Initiate binding for RX_SPI or SRXL2 receivers | | `mmix` | design custom motor mixer | | `smix` | design custom servo mixer | | `color` | configure colors | @@ -80,18 +81,19 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `led` | configure leds | | `map` | mapping of rc channel order | | `motor` | get/set motor output value | -| `msc` | Enter USB Mass storage mode. See docs/USB_Mass_Storage_(MSC)_mode.md for usage information.| +| `msc` | Enter USB Mass storage mode. See [USB MSC documentation](USB_Mass_Storage_(MSC)_mode.md) for usage information.| | `play_sound` | index, or none for next | | `profile` | index (0 to 2) | | `rxrange` | configure rx channel ranges (end-points) | +| `safehome` | Define safe home locations. See the [safehome documentation](Safehomes.md) for usage information. | | `save` | save and reboot | | `serial` | Configure serial ports | | `serialpassthrough `| where `id` is the zero based port index, `baud` is a standard baud rate, and mode is `rx`, `tx`, or both (`rxtx`) | | `set` | name=value or blank or * for list | | `status` | show system status | -| `temp_sensor` | list or configure temperature sensor(s). See docs/Temperature sensors.md | -| `wp` | list or configure waypoints. See more in docs/Navigation.md section NAV WP | -| `version` | | +| `temp_sensor` | list or configure temperature sensor(s). See [temperature sensors documentation](Temperature sensors.md) for more information. | +| `wp` | list or configure waypoints. See more in the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). | +| `version` | Displays version information, | ### serial From 3747b1e434f25b05fdfe5045c0a596c7893f5298 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sun, 15 Nov 2020 17:01:49 +0100 Subject: [PATCH 06/17] Fix nav_overrides_motor_stop default docs value --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 447381166a6..077c22e5295 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2108,7 +2108,7 @@ groups: max: 5000 - name: nav_overrides_motor_stop description: "When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV" - default_value: "NOMS_ALL_NAV" + default_value: "ALL_NAV" field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop - name: nav_rth_climb_first From 6285b7a92984b483d60dcac4442fc2f762742f84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sun, 15 Nov 2020 17:02:37 +0100 Subject: [PATCH 07/17] Update Settings.md --- docs/Settings.md | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d82e08a80f4..c2f09410cd6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -61,6 +61,9 @@ | debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) | | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | +| dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | +| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | +| dji_workarounds | | Enables workarounds for different versions of MSP protocol used | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | | dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | @@ -125,6 +128,7 @@ | fw_p_yaw | 6 | Fixed-wing rate stabilisation P-gain for YAW | | fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | | fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | +| fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | | gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | @@ -187,9 +191,9 @@ | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | | mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | mag_to_use | | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | -| maggain_x | 0 | Magnetometer calibration X gain. If 0, no calibration or calibration failed | -| maggain_y | 0 | Magnetometer calibration Y gain. If 0, no calibration or calibration failed | -| maggain_z | 0 | Magnetometer calibration Z gain. If 0, no calibration or calibration failed | +| maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | +| maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | +| maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | | magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | | magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | | magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | @@ -201,6 +205,7 @@ | mavlink_ext_status_rate | | | | mavlink_extra1_rate | | | | mavlink_extra2_rate | | | +| mavlink_extra3_rate | | | | mavlink_pos_rate | | | | mavlink_rc_chan_rate | | | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | @@ -258,13 +263,13 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | +| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | 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 | 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 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | | nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | | nav_fw_launch_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) | | nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | -| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | @@ -272,7 +277,7 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by nav_fw_pitch2thr_threshold | +| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | | nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | @@ -319,7 +324,7 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ON | Setting to OFF combined with MOTOR_STOP feature will allow user to stop motor when in autonomous modes. On most places this setting is likely to cause a stall. | +| nav_overrides_motor_stop | ALL_NAV | When set OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | 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] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | @@ -351,6 +356,7 @@ | osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | | osd_coordinate_digits | | | | osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | +| osd_crsf_lq_format | TYPE1 | To select LQ format | | osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | | osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | | osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | @@ -375,15 +381,17 @@ | osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_left_sidebar_scroll | | | | osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. | +| osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | -| osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_plus_code_digits | | | | osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | -| osd_rssi_alarm | 20 | Value bellow which to make the OSD RSSI indicator blink | +| osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | | osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | | osd_sidebar_scroll_arrows | | | +| osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | @@ -451,6 +459,8 @@ | smartport_master_halfduplex | | | | smartport_master_inverted | | | | spektrum_sat_bind | 0 | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | | | +| srxl2_unit_id | | | | stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | | stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | | stats_total_energy | | | @@ -488,4 +498,4 @@ | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -> Note: this table is autogenerated. Do not edit it manually. +> Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file From a3ada125d62663767f5013694e6b18dd08d8dc9f Mon Sep 17 00:00:00 2001 From: Airwide Date: Sun, 15 Nov 2020 17:51:25 +0100 Subject: [PATCH 08/17] Fix for control smoothness bug introduced in PR #6104 --- src/main/navigation/navigation_fixedwing.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index dffa74bc02c..51c25d77cce 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -76,16 +76,16 @@ static int8_t loiterDirYaw = 1; static float getSmoothnessCutoffFreq(float baseFreq) { uint16_t smoothness = 10 - navConfig()->fw.control_smoothness; - return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f; + return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; } // Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection // pitch_to_throttle_smooth valid range from 0 to 9 -// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz +// resulting cutoff_freq ranging from baseFreq downwards to ~0.01Hz static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq) { uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth; - return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f; + return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f; } /*----------------------------------------------------------- From 27ed750e736c5050bfdd1f92ca869b71d0912451 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sun, 15 Nov 2020 18:36:25 +0100 Subject: [PATCH 09/17] [DJI] Fix dji_worksrounds min/max range --- src/main/fc/settings.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 447381166a6..fa69bc62931 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3180,6 +3180,8 @@ groups: - name: dji_workarounds description: "Enables workarounds for different versions of MSP protocol used" field: proto_workarounds + min: 0 + max: UINT8_MAX - name: dji_use_name_for_messages description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance" default_value: "ON" From e07cbeb49be5b03332eeadfe6075d9ab35d53bb7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Mon, 16 Nov 2020 11:37:13 +0100 Subject: [PATCH 10/17] Migrate from set-env to env files --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f8f2bdd8dad..63362e64682 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,8 +27,8 @@ jobs: COMMIT_ID=${COMMIT_ID:-${{ github.sha }}} BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID}) VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') - echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}" - echo "::set-env name=BUILD_NAME::inav-${VERSION}-${BUILD_SUFFIX}" + echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - uses: actions/cache@v1 with: path: downloads From cf4e2dced46900975e3d7dc514801f1ca71ea6da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Mon, 16 Nov 2020 12:15:08 +0100 Subject: [PATCH 11/17] Make CI check if Settings docs need to be updated --- .github/workflows/docs.yml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 .github/workflows/docs.yml diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml new file mode 100644 index 00000000000..674155daccf --- /dev/null +++ b/.github/workflows/docs.yml @@ -0,0 +1,23 @@ +name: Make sure docs are updated +on: + push: + paths: + - src/main/fc/settings.yaml + - docs/Settings.md + +jobs: + settings_md: + runs-on: ubuntu-18.04 + + steps: + - uses: actions/checkout@v2 + - name: Install dependencies + run: sudo apt-get update && sudo apt-get -y install python3-yaml + - name: Check that Settings.md is up to date + run: | + cp docs/Settings.md{,.ci} + python3 src/utils/update_cli_docs.py -q + if ! diff -q docs/Settings.md{,.ci} >/dev/null; then + echo "::error ::\"docs/Settings.md\" is not up to date, please run \"src/utils/update_cli_docs.py\"" + exit 1 + fi From d028b19ec7efc7e91bf76bb4ac4063fef1e6b936 Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Mon, 16 Nov 2020 14:04:13 +0100 Subject: [PATCH 12/17] Revert "Autolaunch - Added a smooth end launch feature and code refactor" --- docs/Cli.md | 1 + src/main/cms/cms_menu_navigation.c | 1 - src/main/fc/settings.yaml | 6 - src/main/io/osd.c | 4 - src/main/navigation/navigation.c | 1 - src/main/navigation/navigation.h | 4 +- src/main/navigation/navigation_fw_launch.c | 485 +++++++-------------- 7 files changed, 148 insertions(+), 354 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 04332e2fbf0..d177c717c7d 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 2a70ce6fe69..5e09da10660 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 62c5496df7d..c432341cf32 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 b951ec7ebce..80c10d85fb0 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 ba319442ca2..a2871621748 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 f5f0aa59908..36fbd2b29ee 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 d87454baa1b..309bef0cf01 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,388 +57,186 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" -#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 */ +typedef struct FixedWingLaunchState_s { + /* Launch detection */ + timeUs_t launchDetectorPreviousUpdate; + timeUs_t launchDetectionTimeAccum; + bool launchDetected; -static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { - return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); -} + /* Launch progress */ + timeUs_t launchStartedTime; + bool launchFinished; + bool motorControlAllowed; +} FixedWingLaunchState_t; -static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { - fwLaunch.currentState = nextState; - fwLaunch.currentStateTimeUs = currentTimeUs; -} +static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; -/* Wing control Helpers */ +#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 +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))); -static bool isThrottleIdleEnabled(void) { - return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); -} + const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; + const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); -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 + if (isBungeeLaunched || isSwingLaunched) { + launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); + launchState.launchDetectorPreviousUpdate = currentTimeUs; + if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { + launchState.launchDetected = true; + } + } + else { + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; } } -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); +void resetFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchDetectorPreviousUpdate = currentTimeUs; + launchState.launchDetectionTimeAccum = 0; + launchState.launchStartedTime = 0; + launchState.launchDetected = false; + launchState.launchFinished = false; + launchState.motorControlAllowed = false; } -static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { - return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); +bool isFixedWingLaunchDetected(void) +{ + return launchState.launchDetected; } -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(); - } +void enableFixedWingLaunchController(timeUs_t currentTimeUs) +{ + launchState.launchStartedTime = currentTimeUs; + launchState.motorControlAllowed = true; } -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; +bool isFixedWingLaunchFinishedOrAborted(void) +{ + return launchState.launchFinished; } -/* onEntry state handlers */ - -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - - return FW_LAUNCH_EVENT_NONE; +void abortFixedWingLaunch(void) +{ + launchState.launchFinished = true; } -static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); +#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms - 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; +static void applyFixedWingLaunchIdleLogic(void) +{ + // Until motors are started don't use PID I-term + pidResetErrorAccumulators(); - 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; -} + // We're not flying yet, reset TPA filter + pidResetTPAFilter(); -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 + // 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 } - - 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))); - - 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 + 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); } - } else { - fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter } - - return FW_LAUNCH_EVENT_NONE; } -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; +static inline bool isFixedWingLaunchMaxAltitudeReached(void) +{ + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); } - -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; +static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs) +{ + return timeSinceLaunchMs > navConfig()->fw.launch_min_time; } -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; +static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) +{ + return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; } -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; +static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) +{ + return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); } -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 applyFixedWingLaunchController(timeUs_t currentTimeUs) +{ + // Called at PID rate -// Public methods --------------------------------------------------------------- + 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; + } + } + } -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; + if (applyLaunchIdleLogic) { + // Launch idle logic - low throttle and zero out PIDs + applyFixedWingLaunchIdleLogic(); } - setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs); } + else { + // We are waiting for launch - update launch detector + updateFixedWingLaunchDetector(currentTimeUs); - resetPidsIfNeeded(); - updateRcCommand(); + // 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 From 41e721fed34f0701096f58f20f5f1f77f8e7f3ae Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Mon, 16 Nov 2020 15:06:10 +0100 Subject: [PATCH 13/17] Update CLI docs --- docs/Settings.md | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index c2f09410cd6..a816b048418 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -263,7 +263,6 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | -| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | 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 | 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 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | From c1c5ed15001798560ca3c4abcb771ebd296489bd Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Mon, 16 Nov 2020 20:12:48 +0100 Subject: [PATCH 14/17] Revert "Revert "Autolaunch - Added a smooth end launch feature and code refactor"" --- docs/Cli.md | 1 - src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 6 + src/main/io/osd.c | 4 + src/main/navigation/navigation.c | 1 + src/main/navigation/navigation.h | 4 +- src/main/navigation/navigation_fw_launch.c | 485 ++++++++++++++------- 7 files changed, 354 insertions(+), 148 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index d177c717c7d..04332e2fbf0 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -147,5 +147,4 @@ 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 5e09da10660..2a70ce6fe69 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -134,6 +134,7 @@ 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 c432341cf32..62c5496df7d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2371,6 +2371,12 @@ 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 80c10d85fb0..b951ec7ebce 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3327,6 +3327,10 @@ 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 a2871621748..ba319442ca2 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -171,6 +171,7 @@ 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 36fbd2b29ee..f5f0aa59908 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,7 +239,8 @@ 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_min_time; // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early + 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_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) @@ -524,6 +525,7 @@ 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 309bef0cf01..d87454baa1b 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -33,15 +33,6 @@ #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" @@ -57,186 +48,388 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#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 + } +}; -typedef struct FixedWingLaunchState_s { - /* Launch detection */ - timeUs_t launchDetectorPreviousUpdate; - timeUs_t launchDetectionTimeAccum; - bool launchDetected; +/* Current State Handlers */ - /* Launch progress */ - timeUs_t launchStartedTime; - bool launchFinished; - bool motorControlAllowed; -} FixedWingLaunchState_t; +static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { + return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); +} -static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; +static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) { + fwLaunch.currentState = nextState; + fwLaunch.currentStateTimeUs = currentTimeUs; +} -#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 -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))); +/* Wing control Helpers */ - const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; - const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); +static bool isThrottleIdleEnabled(void) { + return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); +} - if (isBungeeLaunched || isSwingLaunched) { - launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate); - launchState.launchDetectorPreviousUpdate = currentTimeUs; - if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { - launchState.launchDetected = true; - } - } - else { - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; +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 } } -void resetFixedWingLaunchController(timeUs_t currentTimeUs) -{ - launchState.launchDetectorPreviousUpdate = currentTimeUs; - launchState.launchDetectionTimeAccum = 0; - launchState.launchStartedTime = 0; - launchState.launchDetected = false; - launchState.launchFinished = false; - launchState.motorControlAllowed = false; +static inline bool isThrottleLow(void) { + return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; } -bool isFixedWingLaunchDetected(void) -{ - return launchState.launchDetected; +static inline bool isLaunchMaxAltitudeReached(void) { + return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); } -void enableFixedWingLaunchController(timeUs_t currentTimeUs) -{ - launchState.launchStartedTime = currentTimeUs; - launchState.motorControlAllowed = true; +static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { + return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); } -bool isFixedWingLaunchFinishedOrAborted(void) -{ - return launchState.launchFinished; +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(); + } } -void abortFixedWingLaunch(void) -{ - launchState.launchFinished = true; +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; } -#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms +/* onEntry state handlers */ -static void applyFixedWingLaunchIdleLogic(void) -{ - // Until motors are started don't use PID I-term - pidResetErrorAccumulators(); +static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); - // We're not flying yet, reset TPA filter - pidResetTPAFilter(); + return FW_LAUNCH_EVENT_NONE; +} - // 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(); +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; } - 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); + } + 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 + } + + 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))); + + 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 } + } else { + fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter } + + return FW_LAUNCH_EVENT_NONE; } -static inline bool isFixedWingLaunchMaxAltitudeReached(void) -{ - return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude); +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; } -static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs) -{ - return timeSinceLaunchMs > navConfig()->fw.launch_min_time; + +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; } -static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) -{ - return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; +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; } -static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) -{ - return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); +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; } -void applyFixedWingLaunchController(timeUs_t currentTimeUs) -{ - // Called at PID rate +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 (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 (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]); + } - if (applyLaunchIdleLogic) { - // Launch idle logic - low throttle and zero out PIDs - applyFixedWingLaunchIdleLogic(); + return FW_LAUNCH_EVENT_NONE; +} + +// Public methods --------------------------------------------------------------- + +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); } - else { - // We are waiting for launch - update launch detector - updateFixedWingLaunchDetector(currentTimeUs); - // Launch idle logic - low throttle and zero out PIDs - applyFixedWingLaunchIdleLogic(); - } + resetPidsIfNeeded(); + updateRcCommand(); // Control beeper - if (!launchState.launchFinished) { - if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) { - beeper(BEEPER_LAUNCH_MODE_ENABLED); - } else { - beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); - } + if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { + beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); + } else { + beeper(BEEPER_LAUNCH_MODE_ENABLED); } +} - // 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; +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; + } } #endif From 94332bc2864af99a1e01c6415fd48e1066b603e2 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Mon, 16 Nov 2020 20:29:11 +0100 Subject: [PATCH 15/17] [NAV/MIXER] Refactor getMotorStop() code; Avoid recursion and crash when nav_overrides_motor_stop is set to NOMS_ALL_NAV --- docs/Settings.md | 1 + src/main/flight/mixer.c | 29 +++++++++++++++++++++++------ src/main/navigation/navigation.c | 10 ++++++++-- src/main/navigation/navigation.h | 1 + 4 files changed, 33 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index a816b048418..c2f09410cd6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -263,6 +263,7 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | +| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | 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 | 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 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cc65c2b2588..346de5cb07a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -574,15 +574,32 @@ void FAST_CODE mixTable(const float dT) motorStatus_e getMotorStatus(void) { - if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE))) { + if (failsafeRequiresMotorStop()) { return MOTOR_STOPPED_AUTO; } - if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) { - navOverridesMotorStop_e motorStopOverride = navConfig()->general.flags.nav_overrides_motor_stop; - if (!failsafeIsActive() && (STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && - ((motorStopOverride == NOMS_OFF) || ((motorStopOverride == NOMS_ALL_NAV) && !navigationIsControllingThrottle()) || ((motorStopOverride == NOMS_AUTO_ONLY) && !navigationIsFlyingAutonomousMode()))) { - return MOTOR_STOPPED_USER; + if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) { + return MOTOR_STOPPED_AUTO; + } + + const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE); + const bool throttleStickLow = + (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW); + + if (throttleStickLow && fixedWingOrAirmodeNotActive && !failsafeIsActive()) { + // If user is holding stick low, we are not in failsafe and either on a plane or on a quad with inactive + // airmode - we need to check if we are allowing navigation to override MOTOR_STOP + + switch (navConfig()->general.flags.nav_overrides_motor_stop) { + case NOMS_ALL_NAV: + return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; + + case NOMS_AUTO_ONLY: + return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER; + + case NOMS_OFF: + default: + return MOTOR_STOPPED_USER; } } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ba319442ca2..da53648ab70 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3649,10 +3649,16 @@ bool navigationIsExecutingAnEmergencyLanding(void) return navGetCurrentStateFlags() & NAV_CTL_EMERG; } -bool navigationIsControllingThrottle(void) +bool navigationInAutomaticThrottleMode(void) { navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); - return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER)); + return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)); +} + +bool navigationIsControllingThrottle(void) +{ + // Note that this makes a detour into mixer code to evaluate actual motor status + return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER); } bool navigationIsFlyingAutonomousMode(void) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index f5f0aa59908..c50f03b86b2 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -513,6 +513,7 @@ void abortForcedRTH(void); rthState_e getStateOfForcedRTH(void); /* Getter functions which return data about the state of the navigation system */ +bool navigationInAutomaticThrottleMode(void); bool navigationIsControllingThrottle(void); bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); From 7ca1dda83e08b02e4edda9379aa8f9299bcc014c Mon Sep 17 00:00:00 2001 From: dragnea Date: Mon, 16 Nov 2020 23:31:01 +0200 Subject: [PATCH 16/17] Add Allow Manual Throttle Increase in the Fixed Wing/Cruise CMS menu --- src/main/cms/cms_menu_navigation.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 2a70ce6fe69..d17d3df2901 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -111,6 +111,7 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] = OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS), OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING), OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD), + OSD_SETTING_ENTRY("MANUAL THR INCREASE", SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE), OSD_BACK_AND_END_ENTRY, }; From 62374122f0d3a8f4c6cda7d2bb28a56c9c1e5262 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Tue, 17 Nov 2020 08:13:54 +0100 Subject: [PATCH 17/17] [RC] Remove ADJUSTMENT_PROFILE which was never functional --- src/main/fc/rc_adjustments.c | 6 ------ src/main/fc/rc_adjustments.h | 3 --- 2 files changed, 9 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 13844ac8c29..415032f758d 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -276,12 +276,6 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_TPA_BREAKPOINT, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 5 }} -#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - }, { - .adjustmentFunction = ADJUSTMENT_PROFILE, - .mode = ADJUSTMENT_MODE_SELECT, - .data = { .selectConfig = { .switchPositions = 3 }} -#endif } }; diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 75214d6c687..625f152ff1d 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -77,9 +77,6 @@ typedef enum { ADJUSTMENT_VTX_POWER_LEVEL = 49, ADJUSTMENT_TPA = 50, ADJUSTMENT_TPA_BREAKPOINT = 51, -#ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 52, -#endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e;