diff --git a/.gitattributes b/.gitattributes index 0ec73feee9..4c09735408 100644 --- a/.gitattributes +++ b/.gitattributes @@ -10,4 +10,4 @@ Makefile text *.bat eol=crlf *.txt text -*.sh text +*.sh text eol=lf diff --git a/README.md b/README.md index f34db4d136..cf8eecc7e9 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,6 @@ # INAV - navigation capable flight controller -## F3 based flight controllers - -> STM32 F3 flight controllers like Omnibus F3 or SP Racing F3 are deprecated and soon they will reach the end of support in INAV. If you are still using F3 boards, please migrate to F4 or F7. - ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) -![Travis CI status](https://travis-ci.org/iNavFlight/inav.svg?branch=master) ## Features @@ -16,9 +11,9 @@ * Fully configurable mixer that allows to run any hardware you want: multirotor, fixed wing, rovers, boats and other experimental devices * Multiple sensor support: GPS, Pitot tube, sonar, lidar, temperature, ESC with BlHeli_32 telemetry * SmartAudio and IRC Tramp VTX support -* DSHOT and Multishot ESCs * Blackbox flight recorder logging * On Screen Display (OSD) - both character and pixel style +* DJI OSD integration: all elements, system messages and warnings * Telemetry: SmartPort, FPort, MAVlink, LTM * Multi-color RGB LED Strip support * Advanced gyro filtering: Matrix Filter and RPM filter diff --git a/docs/Board - BetFPVF722.md b/docs/Board - BetFPVF722.md new file mode 100644 index 0000000000..387c9f8de2 --- /dev/null +++ b/docs/Board - BetFPVF722.md @@ -0,0 +1,12 @@ +# BetaFPVF722 + +## I2C bus for magnetometer + +BetFPVF722 does not have I2C pads broken out. I2C is shared with UART3 + +* TX3 - SCL +* RX3 - SDA + +> I2C and UART3 can not be used at the same time! Connect the serial receiver to a different serial port when I2C device like MAG is used. + +![BetaFPVF722 I2C](assets/betafpvf722.png) \ No newline at end of file diff --git a/docs/Board - Matek F411-WSE.md b/docs/Board - Matek F411-WSE.md index f7e4bc88f5..9fca091932 100644 --- a/docs/Board - Matek F411-WSE.md +++ b/docs/Board - Matek F411-WSE.md @@ -6,7 +6,7 @@ * STM32F411 CPU * OSD -* BMP280 barometer +* BMP280 barometer (DSP310 with new FCs from around June 2021) * Integrated PDB for 2 motors * 2 UART ports * 6 servos diff --git a/docs/Controls.md b/docs/Controls.md index d245d92b04..de5d18b45c 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -34,7 +34,8 @@ The stick positions are combined to activate different functions: | Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER | | Trim Acc Backwards | HIGH | CENTER | LOW | CENTER | | Save current waypoint mission | LOW | CENTER | HIGH | LOW | -| Load/unload current waypoint mission | LOW | CENTER | HIGH | HIGH | +| Load current waypoint mission | LOW | CENTER | HIGH | HIGH | +| Unload waypoint mission | LOW | CENTER | LOW | HIGH | | Save setting | LOW | LOW | LOW | HIGH | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index ca5cc65f7f..b6f2780870 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -81,7 +81,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI | Operand Type | Name | Notes | |---- |---- |---- | | 0 | VALUE | Value derived from `value` field | -| 1 | RC_CHANNEL | `value` points to RC channel number, indexed from 1 | +| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 | | 2 | FLIGHT | `value` points to flight parameter table | | 3 | FLIGHT_MODE | `value` points to flight modes table | | 4 | LC | `value` points to other logic condition ID | diff --git a/docs/Settings.md b/docs/Settings.md index 8f5eb28f8d..f61ca5ef51 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1,567 +1,5674 @@ # CLI Variable Reference -| Variable Name | Default Value | Min | Max | Description | -| ------------- | ------------- | --- | --- | ----------- | -| 3d_deadband_high | 1514 | PWM_RANGE_MIN | PWM_RANGE_MAX | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) | -| 3d_deadband_low | 1406 | PWM_RANGE_MIN | PWM_RANGE_MAX | Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead) | -| 3d_deadband_throttle | 50 | 0 | 200 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. | -| 3d_neutral | 1460 | PWM_RANGE_MIN | PWM_RANGE_MAX | Neutral (stop) throttle value for 3D mode | -| acc_event_threshold_high | 0 | 0 | 65535 | Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off. | -| acc_event_threshold_low | 0 | 0 | 900 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. | -| acc_event_threshold_neg_x | 0 | 0 | 65535 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. | -| acc_hardware | AUTO | | | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| acc_lpf_hz | 15 | 0 | 200 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | -| acc_lpf_type | BIQUAD | | | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| acc_notch_cutoff | 1 | 1 | 255 | | -| acc_notch_hz | 0 | 0 | 255 | | -| accgain_x | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| accgain_y | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| accgain_z | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| acczero_x | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| acczero_y | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| acczero_z | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| airmode_throttle_threshold | 1300 | 1000 | 2000 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | -| airmode_type | STICK_CENTER | | | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | -| airspeed_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | -| align_acc | DEFAULT | | | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_board_pitch | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_roll | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_yaw | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_gyro | DEFAULT | | | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_mag | DEFAULT | | | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_mag_pitch | 0 | -1800 | 3600 | Same as align_mag_roll, but for the pitch axis. | -| align_mag_roll | 0 | -1800 | 3600 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. | -| align_mag_yaw | 0 | -1800 | 3600 | Same as align_mag_roll, but for the yaw axis. | -| align_opflow | CW0FLIP | | | Optical flow module alignment (default CW0_DEG_FLIP) | -| alt_hold_deadband | 50 | 10 | 250 | Defines the deadband of throttle during alt_hold [r/c points] | -| antigravity_accelerator | 1 | 1 | 20 | | -| antigravity_cutoff_lpf_hz | 15 | 1 | 30 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain | -| antigravity_gain | 1 | 1 | 20 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements | -| applied_defaults | 0 | 0 | 3 | Internal (configurator) hint. Should not be changed manually | -| baro_cal_tolerance | 150 | 0 | 1000 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | -| baro_hardware | AUTO | | | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| baro_median_filter | ON | | | 3-point median filtering for barometer readouts. No reason to change this setting | -| bat_cells | 0 | 0 | 12 | Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. | -| bat_voltage_src | RAW | | | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | -| battery_capacity | 0 | 0 | 4294967295 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | -| battery_capacity_critical | 0 | 0 | 4294967295 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | -| battery_capacity_unit | MAH | | | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | -| battery_capacity_warning | 0 | 0 | 4294967295 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | -| blackbox_device | _target default_ | | | Selection of where to write blackbox data | -| blackbox_rate_denom | 1 | 1 | 65535 | Blackbox logging rate denominator. See blackbox_rate_num. | -| blackbox_rate_num | 1 | 1 | 65535 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations | -| cpu_underclock | OFF | | | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | -| cruise_power | 0 | 0 | 4294967295 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | -| current_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | -| current_meter_offset | _target default_ | -32768 | 32767 | This sets the output offset voltage of the current sensor in millivolts. | -| current_meter_scale | _target default_ | -10000 | 10000 | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | -| current_meter_type | ADC | | | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | -| d_boost_factor | 1.25 | 1 | 3 | | -| d_boost_gyro_delta_lpf_hz | 80 | 10 | 250 | | -| d_boost_max_at_acceleration | 7500 | 1000 | 16000 | | -| deadband | 5 | 0 | 32 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | -| 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 | 1 | 0 | 255 | Enables workarounds for different versions of MSP protocol used | -| dshot_beeper_enabled | ON | | | Whether using DShot motors as beepers is enabled | -| dshot_beeper_tone | 1 | 1 | 5 | Sets the DShot beeper tone | -| dterm_lpf2_hz | 0 | 0 | 500 | 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 | 0 | 500 | 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 | -| dterm_lpf_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. | -| dynamic_gyro_notch_enabled | OFF | | | Enable/disable dynamic gyro notch also known as Matrix Filter | -| dynamic_gyro_notch_min_hz | 150 | 30 | 1000 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | -| dynamic_gyro_notch_q | 120 | 1 | 1000 | Q factor for dynamic notches | -| dynamic_gyro_notch_range | MEDIUM | | | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | -| eleres_freq | 435 | 415 | 450 | | -| eleres_loc_delay | 240 | 30 | 1800 | | -| eleres_loc_en | OFF | | | | -| eleres_loc_power | 7 | 0 | 7 | | -| eleres_signature | 0 | | 4294967295 | | -| eleres_telemetry_en | OFF | | | | -| eleres_telemetry_power | 7 | 0 | 7 | | -| esc_sensor_listen_only | OFF | | | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | -| failsafe_delay | 5 | 0 | 200 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | -| failsafe_fw_pitch_angle | 100 | -800 | 800 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | -| failsafe_fw_roll_angle | -200 | -800 | 800 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | -| failsafe_fw_yaw_rate | -45 | -1000 | 1000 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | -| failsafe_lights | ON | | | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | -| failsafe_lights_flash_on_time | 100 | 20 | 65535 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | -| failsafe_lights_flash_period | 1000 | 40 | 65535 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | -| failsafe_min_distance | 0 | 0 | 65000 | If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. | -| failsafe_min_distance_procedure | DROP | | | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_mission | ON | | | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | -| failsafe_off_delay | 200 | 0 | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | -| failsafe_procedure | SET-THR | | | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_recovery_delay | 5 | 0 | 200 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | -| failsafe_stick_threshold | 50 | 0 | 500 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. | -| failsafe_throttle | 1000 | PWM_RANGE_MIN | PWM_RANGE_MAX | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_throttle_low_delay | 0 | 0 | 300 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | -| fixed_wing_auto_arm | OFF | | | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | -| flaperon_throw_offset | 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | -| fpv_mix_degrees | 0 | 0 | 50 | | -| frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | -| frsky_default_latitude | 0 | -90 | 90 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_default_longitude | 0 | -180 | 180 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_pitch_roll | OFF | | | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | -| frsky_unit | METRIC | | | Not used? [METRIC/IMPERIAL] | -| frsky_vfas_precision | 0 | FRSKY_VFAS_PRECISION_LOW | FRSKY_VFAS_PRECISION_HIGH | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | -| fw_autotune_ff_to_i_tc | 600 | 100 | 5000 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | -| fw_autotune_ff_to_p_gain | 10 | 0 | 100 | FF to P gain (strength relationship) [%] | -| fw_autotune_max_rate_deflection | 80 | 50 | 100 | The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. | -| fw_autotune_min_stick | 50 | 0 | 100 | Minimum stick input [%] to consider overshoot/undershoot detection | -| fw_autotune_p_to_d_gain | 0 | 0 | 200 | P to D gain (strength relationship) [%] | -| fw_autotune_rate_adjustment | AUTO | | | `AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode. | -| fw_d_level | 75 | 0 | 200 | Fixed-wing attitude stabilisation HORIZON transition point | -| fw_d_pitch | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for PITCH | -| fw_d_roll | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for ROLL | -| fw_d_yaw | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for YAW | -| fw_ff_pitch | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for PITCH | -| fw_ff_roll | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for ROLL | -| fw_ff_yaw | 60 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for YAW | -| fw_i_level | 5 | 0 | 200 | Fixed-wing attitude stabilisation low-pass filter cutoff | -| fw_i_pitch | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for PITCH | -| fw_i_roll | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for ROLL | -| fw_i_yaw | 10 | 0 | 200 | Fixed-wing rate stabilisation I-gain for YAW | -| fw_iterm_limit_stick_position | 0.5 | 0 | 1 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | -| fw_iterm_throw_limit | 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | -| fw_level_pitch_deadband | 5 | 0 | 20 | Deadband for automatic leveling when AUTOLEVEL mode is used. | -| fw_level_pitch_gain | 5 | 0 | 20 | I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations | -| fw_level_pitch_trim | 0 | -10 | 10 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | -| fw_loiter_direction | RIGHT | | | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | -| fw_min_throttle_down_pitch | 0 | 0 | 450 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | -| fw_p_level | 20 | 0 | 200 | Fixed-wing attitude stabilisation P-gain | -| fw_p_pitch | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for PITCH | -| fw_p_roll | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for ROLL | -| fw_p_yaw | 6 | 0 | 200 | Fixed-wing rate stabilisation P-gain for YAW | -| fw_reference_airspeed | 1500 | 300 | 6000 | 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 | 0 | 5000 | 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 | 0 | 2 | 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 | 0 | 2 | 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 | -| fw_yaw_iterm_freeze_bank_angle | 0 | 0 | 90 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. | -| 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. | -| gps_dyn_model | AIR_1G | | | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | -| gps_min_sats | 6 | 5 | 10 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. | -| gps_provider | UBLOX | | | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). | -| gps_sbas_mode | NONE | | | Which SBAS mode to be used | -| gps_ublox_use_galileo | OFF | | | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | -| gyro_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter | -| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter | -| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter | -| gyro_anti_aliasing_lpf_hz | 250 | | 255 | Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz | -| gyro_anti_aliasing_lpf_type | PT1 | | | Specifies the type of the software LPF of the gyro signals. | -| gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | -| gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF | -| gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF | -| gyro_hardware_lpf | 256HZ | | | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | -| gyro_main_lpf_hz | 60 | 0 | 500 | Software based gyro main lowpass filter. Value is cutoff frequency (Hz) | -| gyro_main_lpf_type | BIQUAD | | | Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_notch_cutoff | 1 | 1 | 500 | | -| gyro_notch_hz | 0 | | 500 | | -| gyro_to_use | 0 | 0 | 1 | | -| gyro_use_dyn_lpf | OFF | | | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | -| has_flaps | OFF | | | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | -| heading_hold_rate_limit | 90 | HEADING_HOLD_RATE_LIMIT_MIN | HEADING_HOLD_RATE_LIMIT_MAX | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. | -| hott_alarm_sound_interval | 5 | 0 | 120 | Battery alarm delay in seconds for Hott telemetry | -| i2c_speed | 400KHZ | | | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | -| ibus_telemetry_type | 0 | 0 | 255 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | -| idle_power | 0 | 0 | 65535 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | -| imu2_align_pitch | 0 | -1800 | 3600 | Pitch alignment for Secondary IMU. 1/10 of a degree | -| imu2_align_roll | 0 | -1800 | 3600 | Roll alignment for Secondary IMU. 1/10 of a degree | -| imu2_align_yaw | 0 | -1800 | 3600 | Yaw alignment for Secondary IMU. 1/10 of a degree | -| imu2_gain_acc_x | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | -| imu2_gain_acc_y | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | -| imu2_gain_acc_z | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | -| imu2_gain_mag_x | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | -| imu2_gain_mag_y | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | -| imu2_gain_mag_z | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | -| imu2_hardware | NONE | | | Selection of a Secondary IMU hardware type. NONE disables this functionality | -| imu2_radius_acc | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | -| imu2_radius_mag | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | -| imu2_use_for_osd_ahi | OFF | | | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | -| imu2_use_for_osd_heading | OFF | | | If set to ON, Secondary IMU data will be used for Analog OSD heading | -| imu2_use_for_stabilized | OFF | | | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | -| imu_acc_ignore_rate | 0 | 0 | 20 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | -| imu_acc_ignore_slope | 0 | 0 | 5 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | -| imu_dcm_ki | 50 | | 65535 | Inertial Measurement Unit KI Gain for accelerometer measurements | -| imu_dcm_ki_mag | 0 | | 65535 | Inertial Measurement Unit KI Gain for compass measurements | -| imu_dcm_kp | 2500 | | 65535 | Inertial Measurement Unit KP Gain for accelerometer measurements | -| imu_dcm_kp_mag | 10000 | | 65535 | Inertial Measurement Unit KP Gain for compass measurements | -| inav_allow_dead_reckoning | OFF | | | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | -| inav_auto_mag_decl | ON | | | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | -| inav_baro_epv | 100 | 0 | 9999 | Uncertainty value for barometric sensor [cm] | -| inav_gravity_cal_tolerance | 5 | 0 | 255 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | -| inav_max_eph_epv | 1000 | 0 | 9999 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | -| inav_max_surface_altitude | 200 | 0 | 1000 | Max allowed altitude for surface following mode. [cm] | -| inav_reset_altitude | FIRST_ARM | | | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | -| inav_reset_home | FIRST_ARM | | | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | -| inav_use_gps_no_baro | OFF | | | | -| inav_use_gps_velned | ON | | | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | -| inav_w_acc_bias | 0.01 | 0 | 1 | Weight for accelerometer drift estimation | -| inav_w_xy_flow_p | 1.0 | 0 | 100 | | -| inav_w_xy_flow_v | 2.0 | 0 | 100 | | -| inav_w_xy_gps_p | 1.0 | 0 | 10 | Weight of GPS coordinates in estimated UAV position and speed. | -| inav_w_xy_gps_v | 2.0 | 0 | 10 | Weight of GPS velocity data in estimated UAV speed | -| inav_w_xy_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated velocity when GPS reference for position is lost | -| inav_w_xyz_acc_p | 1.0 | 0 | 1 | | -| inav_w_z_baro_p | 0.35 | 0 | 10 | Weight of barometer measurements in estimated altitude and climb rate | -| inav_w_z_gps_p | 0.2 | 0 | 10 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | -| inav_w_z_gps_v | 0.1 | 0 | 10 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero | -| inav_w_z_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | -| inav_w_z_surface_p | 3.5 | 0 | 100 | | -| inav_w_z_surface_v | 6.1 | 0 | 100 | | -| iterm_windup | 50 | 0 | 90 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | -| ledstrip_visual_beeper | OFF | | | | -| limit_attn_filter_cutoff | 1.2 | | 100 | Throttle attenuation PI control output filter cutoff frequency | -| limit_burst_current | 0 | | 4000 | Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable | -| limit_burst_current_falldown_time | 0 | | 3000 | Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current` | -| limit_burst_current_time | 0 | | 3000 | Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced | -| limit_burst_power | 0 | | 40000 | Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable | -| limit_burst_power_falldown_time | 0 | | 3000 | Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power` | -| limit_burst_power_time | 0 | | 3000 | Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced | -| limit_cont_current | 0 | | 4000 | Continous current limit (dA), set to 0 to disable | -| limit_cont_power | 0 | | 40000 | Continous power limit (dW), set to 0 to disable | -| limit_pi_i | 100 | | 10000 | Throttle attenuation PI control I term | -| limit_pi_p | 100 | | 10000 | Throttle attenuation PI control P term | -| log_level | ERROR | | | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | -| log_topics | 0 | 0 | 4294967295 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | -| looptime | 1000 | | 9000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | -| ltm_update_rate | NORMAL | | | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. | -| mag_calibration_time | 30 | 20 | 120 | Adjust how long time the Calibration of mag will last. | -| mag_declination | 0 | -18000 | 18000 | 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 | 0 | 0 | 1 | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | -| maggain_x | 1024 | -32768 | 32767 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | -| maggain_y | 1024 | -32768 | 32767 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | -| maggain_z | 1024 | -32768 | 32767 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | -| magzero_x | 0 | -32768 | 32767 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_y | 0 | -32768 | 32767 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_z | 0 | -32768 | 32767 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | -| manual_pitch_rate | 100 | 0 | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | -| manual_rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | -| manual_rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | -| manual_roll_rate | 100 | 0 | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | -| manual_yaw_rate | 100 | 0 | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | -| mavlink_ext_status_rate | 2 | 0 | 255 | | -| mavlink_extra1_rate | 10 | 0 | 255 | | -| mavlink_extra2_rate | 2 | 0 | 255 | | -| mavlink_extra3_rate | 1 | 0 | 255 | | -| mavlink_pos_rate | 2 | 0 | 255 | | -| mavlink_rc_chan_rate | 5 | 0 | 255 | | -| mavlink_version | 2 | 1 | 2 | Version of MAVLink to use | -| max_angle_inclination_pit | 300 | 100 | 900 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | -| max_angle_inclination_rll | 300 | 100 | 900 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | -| max_check | 1900 | PWM_RANGE_MIN | PWM_RANGE_MAX | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | -| max_throttle | 1850 | PWM_RANGE_MIN | PWM_RANGE_MAX | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. | -| mc_cd_lpf_hz | 30 | 0 | 200 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky | -| mc_cd_pitch | 60 | 0 | 200 | Multicopter Control Derivative gain for PITCH | -| mc_cd_roll | 60 | 0 | 200 | Multicopter Control Derivative gain for ROLL | -| mc_cd_yaw | 60 | 0 | 200 | Multicopter Control Derivative gain for YAW | -| mc_d_level | 75 | 0 | 200 | Multicopter attitude stabilisation HORIZON transition point | -| mc_d_pitch | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for PITCH | -| mc_d_roll | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for ROLL | -| mc_d_yaw | 0 | 0 | 200 | Multicopter rate stabilisation D-gain for YAW | -| mc_i_level | 15 | 0 | 200 | Multicopter attitude stabilisation low-pass filter cutoff | -| mc_i_pitch | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for PITCH | -| mc_i_roll | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for ROLL | -| mc_i_yaw | 45 | 0 | 200 | Multicopter rate stabilisation I-gain for YAW | -| mc_iterm_relax | RP | | | | -| mc_iterm_relax_cutoff | 15 | 1 | 100 | | -| mc_p_level | 20 | 0 | 200 | Multicopter attitude stabilisation P-gain | -| mc_p_pitch | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for PITCH | -| mc_p_roll | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for ROLL | -| mc_p_yaw | 85 | 0 | 200 | Multicopter rate stabilisation P-gain for YAW | -| min_check | 1100 | PWM_RANGE_MIN | PWM_RANGE_MAX | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | -| min_command | 1000 | 0 | PWM_RANGE_MAX | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | -| mode_range_logic_operator | OR | | | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | -| model_preview_type | -1 | -1 | 32767 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | -| moron_threshold | 32 | | 128 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. | -| motor_accel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | -| motor_decel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | -| motor_direction_inverted | OFF | | | Use if you need to inverse yaw motor direction. | -| motor_poles | 14 | 4 | 255 | The number of motor poles. Required to compute motor RPM | -| motor_pwm_protocol | ONESHOT125 | | | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | -| motor_pwm_rate | 400 | 50 | 32000 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | -| msp_override_channels | 0 | 0 | 65535 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | -| name | _empty_ | | | Craft name | -| nav_auto_climb_rate | 500 | 10 | 2000 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | -| nav_auto_speed | 300 | 10 | 2000 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | -| nav_disarm_on_landing | OFF | | | If set to ON, iNav disarms the FC after landing | -| nav_emerg_landing_speed | 500 | 100 | 2000 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] | -| nav_extra_arming_safety | ON | | | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | -| nav_fw_allow_manual_thr_increase | OFF | | | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | -| nav_fw_auto_disarm_delay | 2000 | 100 | 10000 | Delay before plane disarms when `nav_disarm_on_landing` is set (ms) | -| nav_fw_bank_angle | 35 | 5 | 80 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | -| nav_fw_climb_angle | 20 | 5 | 80 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_control_smoothness | 0 | 0 | 9 | How smoothly the autopilot controls the airplane to correct the navigation error | -| nav_fw_cruise_speed | 0 | 0 | 65535 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | -| nav_fw_cruise_thr | 1400 | 1000 | 2000 | Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded ) | -| nav_fw_cruise_yaw_rate | 20 | 0 | 60 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] | -| nav_fw_dive_angle | 15 | 5 | 80 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) | -| nav_fw_land_dive_angle | 2 | -20 | 20 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees | -| nav_fw_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | -| 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_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] | -| nav_fw_launch_min_time | 0 | 0 | 60000 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | -| nav_fw_launch_motor_delay | 500 | 0 | 5000 | Delay between detected launch and launch sequence start and throttling up (ms) | -| nav_fw_launch_spinup_time | 100 | 0 | 1000 | 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_thr | 1700 | 1000 | 2000 | Launch throttle - throttle to be set during launch sequence (pwm units) | -| nav_fw_launch_timeout | 5000 | | 60000 | 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 | 100 | 10000 | Forward velocity threshold for swing-launch detection [cm/s] | -| nav_fw_loiter_radius | 7500 | 0 | 30000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | -| nav_fw_max_thr | 1700 | 1000 | 2000 | Maximum throttle for flying wing in GPS assisted modes | -| nav_fw_min_thr | 1200 | 1000 | 2000 | Minimum throttle for flying wing in GPS assisted modes | -| nav_fw_pitch2thr | 10 | 0 | 100 | 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 | 6 | 0 | 9 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 50 | 0 | 900 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | -| nav_fw_pos_hdg_d | 0 | 0 | 255 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_i | 2 | 0 | 255 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_p | 30 | 0 | 255 | P gain of heading PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_pidsum_limit | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_xy_d | 8 | 0 | 255 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | -| nav_fw_pos_xy_i | 5 | 0 | 255 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | -| nav_fw_pos_xy_p | 75 | 0 | 255 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH | -| nav_fw_pos_z_d | 10 | 0 | 255 | D gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_i | 5 | 0 | 255 | I gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_p | 40 | 0 | 255 | P gain of altitude PID controller (Fixedwing) | -| nav_fw_yaw_deadband | 0 | 0 | 90 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | -| nav_land_maxalt_vspd | 200 | 100 | 2000 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] | -| nav_land_minalt_vspd | 50 | 50 | 500 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] | -| nav_land_slowdown_maxalt | 2000 | 500 | 4000 | Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] | -| nav_land_slowdown_minalt | 500 | 50 | 1000 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] | -| nav_manual_climb_rate | 200 | 10 | 2000 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | -| nav_manual_speed | 500 | 10 | 2000 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | -| nav_max_altitude | 0 | 0 | 65000 | Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled | -| nav_max_terrain_follow_alt | 100 | | 1000 | Max allowed above the ground altitude for terrain following mode | -| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms) | -| nav_mc_bank_angle | 30 | 15 | 45 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | -| nav_mc_braking_bank_angle | 40 | 15 | 60 | max angle that MR is allowed to bank in BOOST mode | -| nav_mc_braking_boost_disengage_speed | 100 | 0 | 1000 | BOOST will be disabled when speed goes below this value | -| nav_mc_braking_boost_factor | 100 | 0 | 200 | acceleration factor for BOOST phase | -| nav_mc_braking_boost_speed_threshold | 150 | 100 | 1000 | BOOST can be enabled when speed is above this value | -| nav_mc_braking_boost_timeout | 750 | 0 | 5000 | how long in ms BOOST phase can happen | -| nav_mc_braking_disengage_speed | 75 | 0 | 1000 | braking is disabled when speed goes below this value | -| nav_mc_braking_speed_threshold | 100 | 0 | 1000 | min speed in cm/s above which braking can happen | -| nav_mc_braking_timeout | 2000 | 100 | 5000 | timeout in ms for braking | -| nav_mc_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Multirotor) | -| nav_mc_hover_thr | 1500 | 1000 | 2000 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | -| nav_mc_pos_deceleration_time | 120 | 0 | 255 | Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting | -| nav_mc_pos_expo | 10 | 0 | 255 | Expo for PosHold control | -| nav_mc_pos_xy_p | 65 | 0 | 255 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity | -| nav_mc_pos_z_p | 50 | 0 | 255 | P gain of altitude PID controller (Multirotor) | -| nav_mc_vel_xy_d | 100 | 0 | 255 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. | -| nav_mc_vel_xy_dterm_attenuation | 90 | 0 | 100 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. | -| nav_mc_vel_xy_dterm_attenuation_end | 60 | 0 | 100 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum | -| nav_mc_vel_xy_dterm_attenuation_start | 10 | 0 | 100 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins | -| nav_mc_vel_xy_dterm_lpf_hz | 2 | 0 | 100 | | -| nav_mc_vel_xy_ff | 40 | 0 | 255 | | -| nav_mc_vel_xy_i | 15 | 0 | 255 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot | -| nav_mc_vel_xy_p | 40 | 0 | 255 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations | -| nav_mc_vel_z_d | 10 | 0 | 255 | D gain of velocity PID controller | -| nav_mc_vel_z_i | 50 | 0 | 255 | I gain of velocity PID controller | -| nav_mc_vel_z_p | 100 | 0 | 255 | P gain of velocity PID controller | -| nav_mc_wp_slowdown | ON | | | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | -| nav_min_rth_distance | 500 | 0 | 5000 | 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 | ALL_NAV | | | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. 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 on NAV modes which take control of the throttle when 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 | 0 | 10 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | -| nav_rth_abort_threshold | 50000 | | 65000 | 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. | -| nav_rth_alt_control_override | OFF | | | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing) | -| nav_rth_alt_mode | AT_LEAST | | | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | -| nav_rth_altitude | 1000 | | 65000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | -| nav_rth_climb_first | ON | | | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) | -| nav_rth_climb_ignore_emerg | OFF | | | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | -| nav_rth_home_altitude | 0 | | 65000 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | -| nav_rth_tail_first | OFF | | | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | -| nav_use_fw_yaw_control | OFF | | | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | -| nav_use_midthr_for_althold | OFF | | | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | -| nav_user_control_mode | ATTI | | | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | -| nav_wp_load_on_boot | OFF | | | If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. | -| nav_wp_radius | 100 | 10 | 10000 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | -| nav_wp_safe_distance | 10000 | | 65000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | -| opflow_hardware | NONE | | | Selection of OPFLOW hardware. | -| opflow_scale | 10.5 | 0 | 10000 | | -| osd_ahi_bordered | OFF | | | Shows a border/corners around the AHI region (pixel OSD only) | -| osd_ahi_height | 162 | | 255 | AHI height in pixels (pixel OSD only) | -| osd_ahi_max_pitch | 20 | 10 | 90 | Max pitch, in degrees, for OSD artificial horizon | -| osd_ahi_reverse_roll | OFF | | | | -| osd_ahi_style | DEFAULT | | | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | -| osd_ahi_vertical_offset | -18 | -128 | 127 | AHI vertical offset from center (pixel OSD only) | -| osd_ahi_width | 132 | | 255 | AHI width in pixels (pixel OSD only) | -| osd_alt_alarm | 100 | 0 | 10000 | Value above which to make the OSD relative altitude indicator blink (meters) | -| osd_baro_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | -| osd_baro_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | -| osd_camera_fov_h | 135 | 60 | 150 | Horizontal field of view for the camera in degres | -| osd_camera_fov_v | 85 | 30 | 120 | Vertical field of view for the camera in degres | -| osd_camera_uptilt | 0 | -40 | 80 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | -| osd_coordinate_digits | 9 | 8 | 11 | | -| 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 | 0 | 255 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | -| osd_dist_alarm | 1000 | 0 | 50000 | Value above which to make the OSD distance from home indicator blink (meters) | -| osd_esc_temp_alarm_max | 900 | -550 | 1500 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_esc_temp_alarm_min | -200 | -550 | 1500 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_estimations_wind_compensation | ON | | | Use wind estimation for remaining flight time/distance estimation | -| osd_failsafe_switch_layout | OFF | | | If enabled the OSD automatically switches to the first layout during failsafe | -| osd_force_grid | OFF | | | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | -| osd_gforce_alarm | 5 | 0 | 20 | Value above which the OSD g force indicator will blink (g) | -| osd_gforce_axis_alarm_max | 5 | -20 | 20 | Value above which the OSD axis g force indicators will blink (g) | -| osd_gforce_axis_alarm_min | -5 | -20 | 20 | Value under which the OSD axis g force indicators will blink (g) | -| osd_home_position_arm_screen | ON | | | Should home position coordinates be displayed on the arming screen. | -| osd_horizon_offset | 0 | -2 | 2 | To vertically adjust the whole OSD and AHI and scrolling bars | -| osd_hud_homepoint | OFF | | | To 3D-display the home point location in the hud | -| osd_hud_homing | OFF | | | To display little arrows around the crossair showing where the home point is in the hud | -| osd_hud_margin_h | 3 | 0 | 4 | Left and right margins for the hud area | -| osd_hud_margin_v | 3 | 1 | 3 | Top and bottom margins for the hud area | -| osd_hud_radar_disp | 0 | 0 | 4 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc | -| osd_hud_radar_nearest | 0 | 0 | 4000 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. | -| osd_hud_radar_range_max | 4000 | 100 | 9990 | In meters, radar aircrafts further away than this will not be displayed in the hud | -| osd_hud_radar_range_min | 3 | 1 | 30 | In meters, radar aircrafts closer than this will not be displayed in the hud | -| osd_hud_wp_disp | 0 | 0 | 3 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) | -| osd_imu_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_imu_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_left_sidebar_scroll | NONE | | | | -| osd_left_sidebar_scroll_step | 0 | | 255 | How many units each sidebar step represents. 0 means the default value for the scroll type. | -| osd_link_quality_alarm | 70 | 0 | 100 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | -| osd_main_voltage_decimals | 1 | 1 | 2 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | -| osd_neg_alt_alarm | 5 | 0 | 10000 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | -| osd_pan_servo_index | 0 | 0 | 10 | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | -| osd_pan_servo_pwm2centideg | 0 | -36 | 36 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | -| osd_plus_code_digits | 11 | 10 | 13 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | -| osd_plus_code_short | 0 | | | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | -| osd_right_sidebar_scroll | NONE | | | | -| osd_right_sidebar_scroll_step | 0 | | 255 | Same as left_sidebar_scroll_step, but for the right sidebar | -| osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) | -| osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink | -| osd_sidebar_height | 3 | 0 | 5 | Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) | -| osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | -| osd_sidebar_scroll_arrows | OFF | | | | -| osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) | -| osd_speed_source | GROUND | | | Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR | -| osd_stats_energy_unit | MAH | | | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | -| osd_stats_min_voltage_unit | BATTERY | | | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. | -| osd_telemetry | OFF | | | To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` | -| 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 | 0 | 600 | Value above which to make the OSD flight time indicator blink (minutes) | -| osd_units | METRIC | | | IMPERIAL, METRIC, UK | -| osd_video_system | AUTO | | | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | -| pid_type | AUTO | | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | -| pidsum_limit | 500 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pidsum_limit_yaw | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pinio_box1 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | -| pinio_box2 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | -| pinio_box3 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | -| pinio_box4 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | -| pitch_rate | 20 | 6 | 180 | Defines rotation rate on PITCH 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. | -| pitot_hardware | NONE | | | Selection of pitot hardware. | -| pitot_lpf_milli_hz | 350 | 0 | 10000 | | -| pitot_scale | 1.0 | 0 | 100 | | -| platform_type | MULTIROTOR | | | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | -| pos_hold_deadband | 10 | 2 | 250 | Stick deadband in [r/c points], applied after r/c deadband and expo | -| prearm_timeout | 10000 | 0 | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. | -| rangefinder_hardware | NONE | | | Selection of rangefinder hardware. | -| rangefinder_median_filter | OFF | | | 3-point median filtering for rangefinder readouts | -| rate_accel_limit_roll_pitch | 0 | | 500000 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | -| rate_accel_limit_yaw | 10000 | | 500000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. | -| rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | -| rc_filter_frequency | 50 | 0 | 100 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | -| rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | -| reboot_character | 82 | 48 | 126 | Special character used to trigger reboot | -| receiver_type | _target default_ | | | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | -| report_cell_voltage | OFF | | | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | -| roll_rate | 20 | 6 | 180 | Defines rotation rate on ROLL 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. | -| rpm_gyro_filter_enabled | OFF | | | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | -| rpm_gyro_harmonics | 1 | 1 | 3 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | -| rpm_gyro_min_hz | 100 | 30 | 200 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | -| rpm_gyro_q | 500 | 1 | 3000 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | -| rssi_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | -| rssi_channel | 0 | 0 | MAX_SUPPORTED_RC_CHANNEL_COUNT | RX channel containing the RSSI signal | -| rssi_max | 100 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | -| rssi_min | 0 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | -| rssi_source | AUTO | | | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` | -| rth_energy_margin | 5 | 0 | 100 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | -| rx_max_usec | 2115 | PWM_PULSE_MIN | PWM_PULSE_MAX | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | -| rx_min_usec | 885 | PWM_PULSE_MIN | PWM_PULSE_MAX | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | -| rx_spi_id | 0 | 0 | 0 | | -| rx_spi_protocol | _target default_ | | | | -| rx_spi_rf_channel_count | 0 | 0 | 8 | | -| safehome_max_distance | 20000 | 0 | 65000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | -| safehome_usage_mode | RTH | | | Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information. | -| sbus_sync_interval | 3000 | 500 | 10000 | | -| sdcard_detect_inverted | _target default_ | | | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | -| serialrx_halfduplex | AUTO | | | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | -| serialrx_inverted | OFF | | | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | -| serialrx_provider | _target default_ | | | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | -| servo_autotrim_rotation_limit | 15 | 1 | 60 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | -| servo_center_pulse | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | Servo midpoint | -| servo_lpf_hz | 20 | 0 | 400 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | -| servo_protocol | PWM | | | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | -| servo_pwm_rate | 50 | 50 | 498 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | -| setpoint_kalman_enabled | OFF | | | Enable Kalman filter on the PID controller setpoint | -| setpoint_kalman_q | 100 | 1 | 16000 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds | -| setpoint_kalman_sharpness | 100 | 1 | 16000 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | -| setpoint_kalman_w | 4 | 1 | 40 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response | -| sim_ground_station_number | _empty_ | | | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | -| sim_low_altitude | -32767 | -32768 | 32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | -| sim_pin | 0000 | | | PIN code for the SIM module | -| sim_transmit_flags | `SIM_TX_FLAG_FAILSAFE` | | 63 | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` | -| sim_transmit_interval | 60 | SIM_MIN_TRANSMIT_INTERVAL | 65535 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | -| small_angle | 25 | 0 | 180 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | -| smartport_fuel_unit | MAH | | | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | -| smartport_master_halfduplex | ON | | | | -| smartport_master_inverted | OFF | | | | -| smith_predictor_delay | 0 | 0 | 8 | Expected delay of the gyro signal. In milliseconds | -| smith_predictor_lpf_hz | 50 | 1 | 500 | Cutoff frequency for the Smith Predictor Low Pass Filter | -| smith_predictor_strength | 0.5 | 0 | 1 | The strength factor of a Smith Predictor of PID measurement. In percents | -| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | SPEKTRUM_SAT_BIND_DISABLED | SPEKTRUM_SAT_BIND_MAX | 0 = disabled. Used to bind the spektrum satellite to RX | -| srxl2_baud_fast | ON | | | | -| srxl2_unit_id | 1 | 0 | 15 | | -| stats | OFF | | | General switch of the statistics recording feature (a.k.a. odometer) | -| stats_total_dist | 0 | | 2147483647 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | -| stats_total_energy | 0 | | 2147483647 | | -| stats_total_time | 0 | | 2147483647 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | -| switch_disarm_delay | 250 | 0 | 1000 | Delay before disarming when requested by switch (ms) [0-1000] | -| telemetry_halfduplex | ON | | | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | -| telemetry_inverted | OFF | | | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | -| telemetry_switch | OFF | | | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | -| thr_comp_weight | 1 | 0 | 2 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | -| thr_expo | 0 | 0 | 100 | Throttle exposition value | -| thr_mid | 50 | 0 | 100 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | -| throttle_idle | 15 | 0 | 30 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | -| throttle_scale | 1.0 | 0 | 1 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | -| throttle_tilt_comp_str | 0 | 0 | 100 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | -| tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. | -| tpa_rate | 0 | 0 | 100 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | -| tri_unarmed_servo | ON | | | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | -| turtle_mode_power_factor | 55 | 0 | 100 | Turtle mode power factor | -| tz_automatic_dst | OFF | | | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | -| tz_offset | 0 | -1440 | 1440 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | -| vbat_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | -| vbat_cell_detect_voltage | 425 | 100 | 500 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. | -| vbat_max_cell_voltage | 420 | 100 | 500 | Maximum voltage per cell in 0.01V units, default is 4.20V | -| vbat_meter_type | ADC | | | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | -| vbat_min_cell_voltage | 330 | 100 | 500 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | -| vbat_scale | _target default_ | VBAT_SCALE_MIN | VBAT_SCALE_MAX | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | -| vbat_warning_cell_voltage | 350 | 100 | 500 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | -| vtx_band | 4 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | -| vtx_channel | 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | -| vtx_halfduplex | ON | | | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | -| vtx_low_power_disarm | OFF | | | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | -| vtx_max_power_override | 0 | 0 | 10000 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | -| vtx_pit_mode_chan | 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | | -| vtx_power | 1 | VTX_SETTINGS_MIN_POWER | VTX_SETTINGS_MAX_POWER | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | -| vtx_smartaudio_early_akk_workaround | ON | | | Enable workaround for early AKK SAudio-enabled VTX bug. | -| yaw_deadband | 5 | 0 | 100 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | -| yaw_lpf_hz | 0 | 0 | 200 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | -| yaw_rate | 20 | 2 | 180 | 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 document is autogenerated. Do not edit it manually. + +### 3d_deadband_high + +High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) + +| Default | Min | Max | +| --- | --- | --- | +| 1514 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### 3d_deadband_low + +Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead) + +| Default | Min | Max | +| --- | --- | --- | +| 1406 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### 3d_deadband_throttle + +Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 200 | + +--- + +### 3d_neutral + +Neutral (stop) throttle value for 3D mode + +| Default | Min | Max | +| --- | --- | --- | +| 1460 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### acc_event_threshold_high + +Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65535 | + +--- + +### acc_event_threshold_low + +Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 900 | + +--- + +### acc_event_threshold_neg_x + +Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65535 | + +--- + +### acc_hardware + +Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### acc_lpf_hz + +Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 200 | + +--- + +### acc_lpf_type + +Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. + +| Default | Min | Max | +| --- | --- | --- | +| BIQUAD | | | + +--- + +### acc_notch_cutoff + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + +### acc_notch_hz + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### accgain_x + +Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. + +| Default | Min | Max | +| --- | --- | --- | +| 4096 | 1 | 8192 | + +--- + +### accgain_y + +Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. + +| Default | Min | Max | +| --- | --- | --- | +| 4096 | 1 | 8192 | + +--- + +### accgain_z + +Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. + +| Default | Min | Max | +| --- | --- | --- | +| 4096 | 1 | 8192 | + +--- + +### acczero_x + +Calculated value after '6 position avanced calibration'. See Wiki page. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### acczero_y + +Calculated value after '6 position avanced calibration'. See Wiki page. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### acczero_z + +Calculated value after '6 position avanced calibration'. See Wiki page. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### airmode_throttle_threshold + +Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used + +| Default | Min | Max | +| --- | --- | --- | +| 1300 | 1000 | 2000 | + +--- + +### airmode_type + +Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. + +| Default | Min | Max | +| --- | --- | --- | +| STICK_CENTER | | | + +--- + +### airspeed_adc_channel + +ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | + +--- + +### align_acc + +When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. + +| Default | Min | Max | +| --- | --- | --- | +| DEFAULT | | | + +--- + +### align_board_pitch + +Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### align_board_roll + +Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### align_board_yaw + +Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### align_gyro + +When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. + +| Default | Min | Max | +| --- | --- | --- | +| DEFAULT | | | + +--- + +### align_mag + +When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. + +| Default | Min | Max | +| --- | --- | --- | +| DEFAULT | | | + +--- + +### align_mag_pitch + +Same as align_mag_roll, but for the pitch axis. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### align_mag_roll + +Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### align_mag_yaw + +Same as align_mag_roll, but for the yaw axis. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### align_opflow + +Optical flow module alignment (default CW0_DEG_FLIP) + +| Default | Min | Max | +| --- | --- | --- | +| CW0FLIP | | | + +--- + +### alt_hold_deadband + +Defines the deadband of throttle during alt_hold [r/c points] + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 10 | 250 | + +--- + +### antigravity_accelerator + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 20 | + +--- + +### antigravity_cutoff_lpf_hz + +Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 1 | 30 | + +--- + +### antigravity_gain + +Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 20 | + +--- + +### applied_defaults + +Internal (configurator) hint. Should not be changed manually + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 3 | + +--- + +### baro_cal_tolerance + +Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. + +| Default | Min | Max | +| --- | --- | --- | +| 150 | 0 | 1000 | + +--- + +### baro_hardware + +Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### baro_median_filter + +3-point median filtering for barometer readouts. No reason to change this setting + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### bat_cells + +Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 12 | + +--- + +### bat_voltage_src + +Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` + +| Default | Min | Max | +| --- | --- | --- | +| RAW | | | + +--- + +### battery_capacity + +Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4294967295 | + +--- + +### battery_capacity_critical + +If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4294967295 | + +--- + +### battery_capacity_unit + +Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). + +| Default | Min | Max | +| --- | --- | --- | +| MAH | | | + +--- + +### battery_capacity_warning + +If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4294967295 | + +--- + +### beeper_pwm_mode + +Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if the external beeper sound is weak. Do not switch from OFF to ON without checking if the board supports PWM beeper mode + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### blackbox_device + +Selection of where to write blackbox data + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | | | + +--- + +### blackbox_rate_denom + +Blackbox logging rate denominator. See blackbox_rate_num. + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 65535 | + +--- + +### blackbox_rate_num + +Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 65535 | + +--- + +### control_deadband + +Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 2 | 250 | + +--- + +### controlrate_profile + +Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 3 | + +--- + +### cpu_underclock + +This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### cruise_power + +Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4294967295 | + +--- + +### current_adc_channel + +ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | + +--- + +### current_meter_offset + +This sets the output offset voltage of the current sensor in millivolts. + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | -32768 | 32767 | + +--- + +### current_meter_scale + +This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | -10000 | 10000 | + +--- + +### current_meter_type + +ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. + +| Default | Min | Max | +| --- | --- | --- | +| ADC | | | + +--- + +### d_boost_factor + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1.25 | 1 | 3 | + +--- + +### d_boost_gyro_delta_lpf_hz + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 80 | 10 | 250 | + +--- + +### d_boost_max_at_acceleration + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 7500 | 1000 | 16000 | + +--- + +### deadband + +These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 32 | + +--- + +### debug_mode + +Defines debug values exposed in debug variables (developer / debugging setting) + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### disarm_kill_switch + +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. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### display_force_sw_blink + +OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### dji_cn_alternating_duration + +Alternating duration of craft name elements, in tenths of a second + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 1 | 150 | + +--- + +### dji_esc_temp_source + +Re-purpose the ESC temperature field for IMU/BARO temperature + +| Default | Min | Max | +| --- | --- | --- | +| ESC | | | + +--- + +### dji_message_speed_source + +Sets the speed type displayed by the DJI OSD in craft name: GROUND, 3D, AIR + +| Default | Min | Max | +| --- | --- | --- | +| 3D | | | + +--- + +### dji_rssi_source + +Source of the DJI RSSI field: RSSI, CRSF_LQ + +| Default | Min | Max | +| --- | --- | --- | +| RSSI | | | + +--- + +### dji_use_adjustments + +Show inflight adjustments in craft name field + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### dji_use_name_for_messages + +Re-purpose the craft name field for messages. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### dji_workarounds + +Enables workarounds for different versions of MSP protocol used + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### dshot_beeper_enabled + +Whether using DShot motors as beepers is enabled + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### dshot_beeper_tone + +Sets the DShot beeper tone + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 5 | + +--- + +### dterm_lpf2_hz + +Cutoff frequency for stage 2 D-term low pass filter + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 500 | + +--- + +### dterm_lpf2_type + +Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. + +| Default | Min | Max | +| --- | --- | --- | +| BIQUAD | | | + +--- + +### dterm_lpf_hz + +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 + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 500 | + +--- + +### dterm_lpf_type + +Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. + +| Default | Min | Max | +| --- | --- | --- | +| BIQUAD | | | + +--- + +### dynamic_gyro_notch_enabled + +Enable/disable dynamic gyro notch also known as Matrix Filter + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### dynamic_gyro_notch_min_hz + +Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` + +| Default | Min | Max | +| --- | --- | --- | +| 150 | 30 | 1000 | + +--- + +### dynamic_gyro_notch_q + +Q factor for dynamic notches + +| Default | Min | Max | +| --- | --- | --- | +| 120 | 1 | 1000 | + +--- + +### dynamic_gyro_notch_range + +Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers + +| Default | Min | Max | +| --- | --- | --- | +| MEDIUM | | | + +--- + +### eleres_freq + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 435 | 415 | 450 | + +--- + +### eleres_loc_delay + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 240 | 30 | 1800 | + +--- + +### eleres_loc_en + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### eleres_loc_power + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 7 | 0 | 7 | + +--- + +### eleres_signature + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 4294967295 | + +--- + +### eleres_telemetry_en + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### eleres_telemetry_power + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 7 | 0 | 7 | + +--- + +### esc_sensor_listen_only + +Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### failsafe_delay + +Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 200 | + +--- + +### failsafe_fw_pitch_angle + +Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb + +| Default | Min | Max | +| --- | --- | --- | +| 100 | -800 | 800 | + +--- + +### failsafe_fw_roll_angle + +Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll + +| Default | Min | Max | +| --- | --- | --- | +| -200 | -800 | 800 | + +--- + +### failsafe_fw_yaw_rate + +Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn + +| Default | Min | Max | +| --- | --- | --- | +| -45 | -1000 | 1000 | + +--- + +### failsafe_lights + +Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### failsafe_lights_flash_on_time + +Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 20 | 65535 | + +--- + +### failsafe_lights_flash_period + +Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 40 | 65535 | + +--- + +### failsafe_min_distance + +If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65000 | + +--- + +### failsafe_min_distance_procedure + +What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). + +| Default | Min | Max | +| --- | --- | --- | +| DROP | | | + +--- + +### failsafe_mission + +If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### failsafe_off_delay + +Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 0 | 200 | + +--- + +### failsafe_procedure + +What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). + +| Default | Min | Max | +| --- | --- | --- | +| SET-THR | | | + +--- + +### failsafe_recovery_delay + +Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 200 | + +--- + +### failsafe_stick_threshold + +Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 500 | + +--- + +### failsafe_throttle + +Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### failsafe_throttle_low_delay + +If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 300 | + +--- + +### fixed_wing_auto_arm + +Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### flaperon_throw_offset + +Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. + +| Default | Min | Max | +| --- | --- | --- | +| 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | + +--- + +### fpv_mix_degrees + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 50 | + +--- + +### frsky_coordinates_format + +D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | FRSKY_FORMAT_NMEA | + +--- + +### frsky_default_latitude + +D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -90 | 90 | + +--- + +### frsky_default_longitude + +D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -180 | 180 | + +--- + +### frsky_pitch_roll + +S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### frsky_unit + +Not used? [METRIC/IMPERIAL] + +| Default | Min | Max | +| --- | --- | --- | +| METRIC | | | + +--- + +### frsky_vfas_precision + +D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method + +| Default | Min | Max | +| --- | --- | --- | +| 0 | FRSKY_VFAS_PRECISION_LOW | FRSKY_VFAS_PRECISION_HIGH | + +--- + +### fw_autotune_ff_to_i_tc + +FF to I time (defines time for I to reach the same level of response as FF) [ms] + +| Default | Min | Max | +| --- | --- | --- | +| 600 | 100 | 5000 | + +--- + +### fw_autotune_ff_to_p_gain + +FF to P gain (strength relationship) [%] + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 100 | + +--- + +### fw_autotune_max_rate_deflection + +The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. + +| Default | Min | Max | +| --- | --- | --- | +| 80 | 50 | 100 | + +--- + +### fw_autotune_min_stick + +Minimum stick input [%], after applying deadband and expo, to start recording the plane's response to stick input. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### fw_autotune_p_to_d_gain + +P to D gain (strength relationship) [%] + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 200 | + +--- + +### fw_autotune_rate_adjustment + +`AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode. + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### fw_d_level + +Fixed-wing attitude stabilisation HORIZON transition point + +| Default | Min | Max | +| --- | --- | --- | +| 75 | 0 | 255 | + +--- + +### fw_d_pitch + +Fixed wing rate stabilisation D-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### fw_d_roll + +Fixed wing rate stabilisation D-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### fw_d_yaw + +Fixed wing rate stabilisation D-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### fw_ff_pitch + +Fixed-wing rate stabilisation FF-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 255 | + +--- + +### fw_ff_roll + +Fixed-wing rate stabilisation FF-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 255 | + +--- + +### fw_ff_yaw + +Fixed-wing rate stabilisation FF-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 255 | + +--- + +### fw_i_level + +Fixed-wing attitude stabilisation low-pass filter cutoff + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### fw_i_pitch + +Fixed-wing rate stabilisation I-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 7 | 0 | 255 | + +--- + +### fw_i_roll + +Fixed-wing rate stabilisation I-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 7 | 0 | 255 | + +--- + +### fw_i_yaw + +Fixed-wing rate stabilisation I-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### fw_iterm_limit_stick_position + +Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 1 | + +--- + +### fw_iterm_throw_limit + +Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. + +| Default | Min | Max | +| --- | --- | --- | +| 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | + +--- + +### fw_level_pitch_gain + +I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 20 | + +--- + +### fw_level_pitch_trim + +Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -10 | 10 | + +--- + +### fw_loiter_direction + +Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. + +| Default | Min | Max | +| --- | --- | --- | +| RIGHT | | | + +--- + +### fw_min_throttle_down_pitch + +Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 450 | + +--- + +### fw_p_level + +Fixed-wing attitude stabilisation P-gain + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 255 | + +--- + +### fw_p_pitch + +Fixed-wing rate stabilisation P-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### fw_p_roll + +Fixed-wing rate stabilisation P-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### fw_p_yaw + +Fixed-wing rate stabilisation P-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 6 | 0 | 255 | + +--- + +### fw_reference_airspeed + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 1500 | 300 | 6000 | + +--- + +### fw_tpa_time_constant + +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 + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 5000 | + +--- + +### fw_turn_assist_pitch_gain + +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 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 2 | + +--- + +### fw_turn_assist_yaw_gain + +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 + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 2 | + +--- + +### fw_yaw_iterm_freeze_bank_angle + +Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 90 | + +--- + +### gps_auto_baud + +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 + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### gps_auto_config + +Enable automatic configuration of UBlox GPS receivers. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### gps_dyn_model + +GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. + +| Default | Min | Max | +| --- | --- | --- | +| AIR_1G | | | + +--- + +### gps_min_sats + +Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. + +| Default | Min | Max | +| --- | --- | --- | +| 6 | 5 | 10 | + +--- + +### gps_provider + +Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). + +| Default | Min | Max | +| --- | --- | --- | +| UBLOX | | | + +--- + +### gps_sbas_mode + +Which SBAS mode to be used + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### gps_ublox_use_galileo + +Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### gyro_abg_alpha + +Alpha factor for Gyro Alpha-Beta-Gamma filter + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1 | + +--- + +### gyro_abg_boost + +Boost factor for Gyro Alpha-Beta-Gamma filter + +| Default | Min | Max | +| --- | --- | --- | +| 0.35 | 0 | 2 | + +--- + +### gyro_abg_half_life + +Sample half-life for Gyro Alpha-Beta-Gamma filter + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 10 | + +--- + +### gyro_anti_aliasing_lpf_hz + +Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz + +| Default | Min | Max | +| --- | --- | --- | +| 250 | | 1000 | + +--- + +### gyro_anti_aliasing_lpf_type + +Specifies the type of the software LPF of the gyro signals. + +| Default | Min | Max | +| --- | --- | --- | +| PT1 | | | + +--- + +### gyro_dyn_lpf_curve_expo + +Expo value for the throttle-to-frequency mapping for Dynamic LPF + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 1 | 10 | + +--- + +### gyro_dyn_lpf_max_hz + +Maximum frequency of the gyro Dynamic LPF + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 40 | 1000 | + +--- + +### gyro_dyn_lpf_min_hz + +Minimum frequency of the gyro Dynamic LPF + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 40 | 400 | + +--- + +### gyro_hardware_lpf + +Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. + +| Default | Min | Max | +| --- | --- | --- | +| 256HZ | | | + +--- + +### gyro_main_lpf_hz + +Software based gyro main lowpass filter. Value is cutoff frequency (Hz) + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 500 | + +--- + +### gyro_main_lpf_type + +Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. + +| Default | Min | Max | +| --- | --- | --- | +| BIQUAD | | | + +--- + +### gyro_notch_cutoff + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 500 | + +--- + +### gyro_notch_hz + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 500 | + +--- + +### gyro_to_use + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1 | + +--- + +### gyro_use_dyn_lpf + +Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### has_flaps + +Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### heading_hold_rate_limit + +This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. + +| Default | Min | Max | +| --- | --- | --- | +| 90 | HEADING_HOLD_RATE_LIMIT_MIN | HEADING_HOLD_RATE_LIMIT_MAX | + +--- + +### hott_alarm_sound_interval + +Battery alarm delay in seconds for Hott telemetry + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 120 | + +--- + +### i2c_speed + +This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) + +| Default | Min | Max | +| --- | --- | --- | +| 400KHZ | | | + +--- + +### ibus_telemetry_type + +Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### idle_power + +Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65535 | + +--- + +### imu2_align_pitch + +Pitch alignment for Secondary IMU. 1/10 of a degree + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### imu2_align_roll + +Roll alignment for Secondary IMU. 1/10 of a degree + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### imu2_align_yaw + +Yaw alignment for Secondary IMU. 1/10 of a degree + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1800 | 3600 | + +--- + +### imu2_gain_acc_x + +Secondary IMU ACC calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_gain_acc_y + +Secondary IMU ACC calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_gain_acc_z + +Secondary IMU ACC calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_gain_mag_x + +Secondary IMU MAG calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_gain_mag_y + +Secondary IMU MAG calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_gain_mag_z + +Secondary IMU MAG calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_hardware + +Selection of a Secondary IMU hardware type. NONE disables this functionality + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### imu2_radius_acc + +Secondary IMU MAG calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_radius_mag + +Secondary IMU MAG calibration data + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### imu2_use_for_osd_ahi + +If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### imu2_use_for_osd_heading + +If set to ON, Secondary IMU data will be used for Analog OSD heading + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### imu2_use_for_stabilized + +If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### imu_acc_ignore_rate + +Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 20 | + +--- + +### imu_acc_ignore_slope + +Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 5 | + +--- + +### imu_dcm_ki + +Inertial Measurement Unit KI Gain for accelerometer measurements + +| Default | Min | Max | +| --- | --- | --- | +| 50 | | 65535 | + +--- + +### imu_dcm_ki_mag + +Inertial Measurement Unit KI Gain for compass measurements + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 65535 | + +--- + +### imu_dcm_kp + +Inertial Measurement Unit KP Gain for accelerometer measurements + +| Default | Min | Max | +| --- | --- | --- | +| 2500 | | 65535 | + +--- + +### imu_dcm_kp_mag + +Inertial Measurement Unit KP Gain for compass measurements + +| Default | Min | Max | +| --- | --- | --- | +| 10000 | | 65535 | + +--- + +### inav_allow_dead_reckoning + +Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### inav_auto_mag_decl + +Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### inav_baro_epv + +Uncertainty value for barometric sensor [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 9999 | + +--- + +### inav_gravity_cal_tolerance + +Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### inav_max_eph_epv + +Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 0 | 9999 | + +--- + +### inav_max_surface_altitude + +Max allowed altitude for surface following mode. [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 0 | 1000 | + +--- + +### inav_reset_altitude + +Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) + +| Default | Min | Max | +| --- | --- | --- | +| FIRST_ARM | | | + +--- + +### inav_reset_home + +Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM + +| Default | Min | Max | +| --- | --- | --- | +| FIRST_ARM | | | + +--- + +### inav_use_gps_no_baro + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### inav_use_gps_velned + +Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### inav_w_acc_bias + +Weight for accelerometer drift estimation + +| Default | Min | Max | +| --- | --- | --- | +| 0.01 | 0 | 1 | + +--- + +### inav_w_xy_flow_p + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1.0 | 0 | 100 | + +--- + +### inav_w_xy_flow_v + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 2.0 | 0 | 100 | + +--- + +### inav_w_xy_gps_p + +Weight of GPS coordinates in estimated UAV position and speed. + +| Default | Min | Max | +| --- | --- | --- | +| 1.0 | 0 | 10 | + +--- + +### inav_w_xy_gps_v + +Weight of GPS velocity data in estimated UAV speed + +| Default | Min | Max | +| --- | --- | --- | +| 2.0 | 0 | 10 | + +--- + +### inav_w_xy_res_v + +Decay coefficient for estimated velocity when GPS reference for position is lost + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 10 | + +--- + +### inav_w_xyz_acc_p + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1.0 | 0 | 1 | + +--- + +### inav_w_z_baro_p + +Weight of barometer measurements in estimated altitude and climb rate + +| Default | Min | Max | +| --- | --- | --- | +| 0.35 | 0 | 10 | + +--- + +### inav_w_z_gps_p + +Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes + +| Default | Min | Max | +| --- | --- | --- | +| 0.2 | 0 | 10 | + +--- + +### inav_w_z_gps_v + +Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero + +| Default | Min | Max | +| --- | --- | --- | +| 0.1 | 0 | 10 | + +--- + +### inav_w_z_res_v + +Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 10 | + +--- + +### inav_w_z_surface_p + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 3.5 | 0 | 100 | + +--- + +### inav_w_z_surface_v + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 6.1 | 0 | 100 | + +--- + +### iterm_windup + +Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 90 | + +--- + +### ledstrip_visual_beeper + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### limit_attn_filter_cutoff + +Throttle attenuation PI control output filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 1.2 | | 100 | + +--- + +### limit_burst_current + +Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 4000 | + +--- + +### limit_burst_current_falldown_time + +Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current` + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 3000 | + +--- + +### limit_burst_current_time + +Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 3000 | + +--- + +### limit_burst_power + +Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 40000 | + +--- + +### limit_burst_power_falldown_time + +Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power` + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 3000 | + +--- + +### limit_burst_power_time + +Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 3000 | + +--- + +### limit_cont_current + +Continous current limit (dA), set to 0 to disable + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 4000 | + +--- + +### limit_cont_power + +Continous power limit (dW), set to 0 to disable + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 40000 | + +--- + +### limit_pi_i + +Throttle attenuation PI control I term + +| Default | Min | Max | +| --- | --- | --- | +| 100 | | 10000 | + +--- + +### limit_pi_p + +Throttle attenuation PI control P term + +| Default | Min | Max | +| --- | --- | --- | +| 100 | | 10000 | + +--- + +### log_level + +Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. + +| Default | Min | Max | +| --- | --- | --- | +| ERROR | | | + +--- + +### log_topics + +Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4294967295 | + +--- + +### looptime + +This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | | 9000 | + +--- + +### ltm_update_rate + +Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. + +| Default | Min | Max | +| --- | --- | --- | +| NORMAL | | | + +--- + +### mag_calibration_time + +Adjust how long time the Calibration of mag will last. + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 20 | 120 | + +--- + +### mag_declination + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -18000 | 18000 | + +--- + +### mag_hardware + +Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### 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 + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1 | + +--- + +### maggain_x + +Magnetometer calibration X gain. If 1024, no calibration or calibration failed + +| Default | Min | Max | +| --- | --- | --- | +| 1024 | -32768 | 32767 | + +--- + +### maggain_y + +Magnetometer calibration Y gain. If 1024, no calibration or calibration failed + +| Default | Min | Max | +| --- | --- | --- | +| 1024 | -32768 | 32767 | + +--- + +### maggain_z + +Magnetometer calibration Z gain. If 1024, no calibration or calibration failed + +| Default | Min | Max | +| --- | --- | --- | +| 1024 | -32768 | 32767 | + +--- + +### magzero_x + +Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### magzero_y + +Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### magzero_z + +Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### manual_pitch_rate + +Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### manual_rc_expo + +Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] + +| Default | Min | Max | +| --- | --- | --- | +| 70 | 0 | 100 | + +--- + +### manual_rc_yaw_expo + +Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 100 | + +--- + +### manual_roll_rate + +Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### manual_yaw_rate + +Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 100 | + +--- + +### mavlink_ext_status_rate + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_extra1_rate + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### mavlink_extra2_rate + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_extra3_rate + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 255 | + +--- + +### mavlink_pos_rate + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### mavlink_rc_chan_rate + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### mavlink_version + +Version of MAVLink to use + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 1 | 2 | + +--- + +### max_angle_inclination_pit + +Maximum inclination in level (angle) mode (PITCH axis). 100=10° + +| Default | Min | Max | +| --- | --- | --- | +| 300 | 100 | 900 | + +--- + +### max_angle_inclination_rll + +Maximum inclination in level (angle) mode (ROLL axis). 100=10° + +| Default | Min | Max | +| --- | --- | --- | +| 300 | 100 | 900 | + +--- + +### max_check + +These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. + +| Default | Min | Max | +| --- | --- | --- | +| 1900 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### max_throttle + +This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. + +| Default | Min | Max | +| --- | --- | --- | +| 1850 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### mc_cd_lpf_hz + +Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 200 | + +--- + +### mc_cd_pitch + +Multicopter Control Derivative gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 255 | + +--- + +### mc_cd_roll + +Multicopter Control Derivative gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 255 | + +--- + +### mc_cd_yaw + +Multicopter Control Derivative gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 255 | + +--- + +### mc_d_level + +Multicopter attitude stabilisation HORIZON transition point + +| Default | Min | Max | +| --- | --- | --- | +| 75 | 0 | 255 | + +--- + +### mc_d_pitch + +Multicopter rate stabilisation D-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 23 | 0 | 255 | + +--- + +### mc_d_roll + +Multicopter rate stabilisation D-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 23 | 0 | 255 | + +--- + +### mc_d_yaw + +Multicopter rate stabilisation D-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### mc_i_level + +Multicopter attitude stabilisation low-pass filter cutoff + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 255 | + +--- + +### mc_i_pitch + +Multicopter rate stabilisation I-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 255 | + +--- + +### mc_i_roll + +Multicopter rate stabilisation I-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 255 | + +--- + +### mc_i_yaw + +Multicopter rate stabilisation I-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 45 | 0 | 255 | + +--- + +### mc_iterm_relax + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| RP | | | + +--- + +### mc_iterm_relax_cutoff + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 1 | 100 | + +--- + +### mc_p_level + +Multicopter attitude stabilisation P-gain + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 255 | + +--- + +### mc_p_pitch + +Multicopter rate stabilisation P-gain for PITCH + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 255 | + +--- + +### mc_p_roll + +Multicopter rate stabilisation P-gain for ROLL + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 255 | + +--- + +### mc_p_yaw + +Multicopter rate stabilisation P-gain for YAW + +| Default | Min | Max | +| --- | --- | --- | +| 85 | 0 | 255 | + +--- + +### min_check + +These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. + +| Default | Min | Max | +| --- | --- | --- | +| 1100 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### min_command + +This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 0 | PWM_RANGE_MAX | + +--- + +### mode_range_logic_operator + +Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. + +| Default | Min | Max | +| --- | --- | --- | +| OR | | | + +--- + +### model_preview_type + +ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. + +| Default | Min | Max | +| --- | --- | --- | +| -1 | -1 | 32767 | + +--- + +### moron_threshold + +When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. + +| Default | Min | Max | +| --- | --- | --- | +| 32 | | 128 | + +--- + +### motor_accel_time + +Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1000 | + +--- + +### motor_decel_time + +Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1000 | + +--- + +### motor_direction_inverted + +Use if you need to inverse yaw motor direction. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### motor_poles + +The number of motor poles. Required to compute motor RPM + +| Default | Min | Max | +| --- | --- | --- | +| 14 | 4 | 255 | + +--- + +### motor_pwm_protocol + +Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED + +| Default | Min | Max | +| --- | --- | --- | +| ONESHOT125 | | | + +--- + +### motor_pwm_rate + +Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. + +| Default | Min | Max | +| --- | --- | --- | +| 400 | 50 | 32000 | + +--- + +### msp_override_channels + +Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65535 | + +--- + +### name + +Craft name + +| Default | Min | Max | +| --- | --- | --- | +| _empty_ | | | + +--- + +### nav_auto_climb_rate + +Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 10 | 2000 | + +--- + +### nav_auto_speed + +Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] + +| Default | Min | Max | +| --- | --- | --- | +| 300 | 10 | 2000 | + +--- + +### nav_disarm_on_landing + +If set to ON, iNav disarms the FC after landing + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_emerg_landing_speed + +Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 100 | 2000 | + +--- + +### nav_extra_arming_safety + +If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### nav_fw_allow_manual_thr_increase + +Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_fw_bank_angle + +Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll + +| Default | Min | Max | +| --- | --- | --- | +| 35 | 5 | 80 | + +--- + +### nav_fw_climb_angle + +Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 5 | 80 | + +--- + +### nav_fw_control_smoothness + +How smoothly the autopilot controls the airplane to correct the navigation error + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 9 | + +--- + +### nav_fw_cruise_speed + +Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65535 | + +--- + +### nav_fw_cruise_thr + +Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded ) + +| Default | Min | Max | +| --- | --- | --- | +| 1400 | 1000 | 2000 | + +--- + +### nav_fw_cruise_yaw_rate + +Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 60 | + +--- + +### nav_fw_dive_angle + +Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 5 | 80 | + +--- + +### nav_fw_heading_p + +P gain of Heading Hold controller (Fixedwing) + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 255 | + +--- + +### nav_fw_land_dive_angle + +Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees + +| Default | Min | Max | +| --- | --- | --- | +| 2 | -20 | 20 | + +--- + +### nav_fw_launch_accel + +Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s + +| Default | Min | Max | +| --- | --- | --- | +| 1863 | 1000 | 20000 | + +--- + +### nav_fw_launch_climb_angle + +Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit + +| Default | Min | Max | +| --- | --- | --- | +| 18 | 5 | 45 | + +--- + +### nav_fw_launch_detect_time + +Time for which thresholds have to breached to consider launch happened [ms] + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 10 | 1000 | + +--- + +### nav_fw_launch_end_time + +Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] + +| Default | Min | Max | +| --- | --- | --- | +| 3000 | 0 | 5000 | + +--- + +### nav_fw_launch_idle_motor_delay + +Delay between raising throttle and motor starting at idle throttle (ms) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### nav_fw_launch_idle_thr + +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) + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 1000 | 2000 | + +--- + +### nav_fw_launch_max_altitude + +Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### nav_fw_launch_max_angle + +Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] + +| Default | Min | Max | +| --- | --- | --- | +| 45 | 5 | 180 | + +--- + +### nav_fw_launch_min_time + +Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 60000 | + +--- + +### nav_fw_launch_motor_delay + +Delay between detected launch and launch sequence start and throttling up (ms) + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 0 | 5000 | + +--- + +### nav_fw_launch_spinup_time + +Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 1000 | + +--- + +### nav_fw_launch_thr + +Launch throttle - throttle to be set during launch sequence (pwm units) + +| Default | Min | Max | +| --- | --- | --- | +| 1700 | 1000 | 2000 | + +--- + +### nav_fw_launch_timeout + +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) + +| Default | Min | Max | +| --- | --- | --- | +| 5000 | | 60000 | + +--- + +### nav_fw_launch_velocity + +Forward velocity threshold for swing-launch detection [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 300 | 100 | 10000 | + +--- + +### nav_fw_loiter_radius + +PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 7500 | 0 | 30000 | + +--- + +### nav_fw_max_thr + +Maximum throttle for flying wing in GPS assisted modes + +| Default | Min | Max | +| --- | --- | --- | +| 1700 | 1000 | 2000 | + +--- + +### nav_fw_min_thr + +Minimum throttle for flying wing in GPS assisted modes + +| Default | Min | Max | +| --- | --- | --- | +| 1200 | 1000 | 2000 | + +--- + +### nav_fw_pitch2thr + +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) + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 100 | + +--- + +### nav_fw_pitch2thr_smoothing + +How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. + +| Default | Min | Max | +| --- | --- | --- | +| 6 | 0 | 9 | + +--- + +### nav_fw_pitch2thr_threshold + +Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 900 | + +--- + +### nav_fw_pos_hdg_d + +D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### nav_fw_pos_hdg_i + +I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 255 | + +--- + +### nav_fw_pos_hdg_p + +P gain of heading PID controller. (Fixedwing, rovers, boats) + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 0 | 255 | + +--- + +### nav_fw_pos_hdg_pidsum_limit + +Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) + +| Default | Min | Max | +| --- | --- | --- | +| 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | + +--- + +### nav_fw_pos_xy_d + +D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero + +| Default | Min | Max | +| --- | --- | --- | +| 8 | 0 | 255 | + +--- + +### nav_fw_pos_xy_i + +I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### nav_fw_pos_xy_p + +P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH + +| Default | Min | Max | +| --- | --- | --- | +| 75 | 0 | 255 | + +--- + +### nav_fw_pos_z_d + +D gain of altitude PID controller (Fixedwing) + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### nav_fw_pos_z_i + +I gain of altitude PID controller (Fixedwing) + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 255 | + +--- + +### nav_fw_pos_z_p + +P gain of altitude PID controller (Fixedwing) + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 255 | + +--- + +### nav_fw_yaw_deadband + +Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 90 | + +--- + +### nav_land_maxalt_vspd + +Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 100 | 2000 | + +--- + +### nav_land_minalt_vspd + +Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 50 | 500 | + +--- + +### nav_land_slowdown_maxalt + +Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 2000 | 500 | 4000 | + +--- + +### nav_land_slowdown_minalt + +Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 50 | 1000 | + +--- + +### nav_manual_climb_rate + +Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 200 | 10 | 2000 | + +--- + +### nav_manual_speed + +Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 10 | 2000 | + +--- + +### nav_max_altitude + +Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 65000 | + +--- + +### nav_max_terrain_follow_alt + +Max allowed above the ground altitude for terrain following mode + +| Default | Min | Max | +| --- | --- | --- | +| 100 | | 1000 | + +--- + +### nav_mc_auto_disarm_delay + +Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) + +| Default | Min | Max | +| --- | --- | --- | +| 2000 | 100 | 10000 | + +--- + +### nav_mc_bank_angle + +Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude + +| Default | Min | Max | +| --- | --- | --- | +| 30 | 15 | 45 | + +--- + +### nav_mc_braking_bank_angle + +max angle that MR is allowed to bank in BOOST mode + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 15 | 60 | + +--- + +### nav_mc_braking_boost_disengage_speed + +BOOST will be disabled when speed goes below this value + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 1000 | + +--- + +### nav_mc_braking_boost_factor + +acceleration factor for BOOST phase + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### nav_mc_braking_boost_speed_threshold + +BOOST can be enabled when speed is above this value + +| Default | Min | Max | +| --- | --- | --- | +| 150 | 100 | 1000 | + +--- + +### nav_mc_braking_boost_timeout + +how long in ms BOOST phase can happen + +| Default | Min | Max | +| --- | --- | --- | +| 750 | 0 | 5000 | + +--- + +### nav_mc_braking_disengage_speed + +braking is disabled when speed goes below this value + +| Default | Min | Max | +| --- | --- | --- | +| 75 | 0 | 1000 | + +--- + +### nav_mc_braking_speed_threshold + +min speed in cm/s above which braking can happen + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 1000 | + +--- + +### nav_mc_braking_timeout + +timeout in ms for braking + +| Default | Min | Max | +| --- | --- | --- | +| 2000 | 100 | 5000 | + +--- + +### nav_mc_heading_p + +P gain of Heading Hold controller (Multirotor) + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 255 | + +--- + +### nav_mc_hover_thr + +Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. + +| Default | Min | Max | +| --- | --- | --- | +| 1500 | 1000 | 2000 | + +--- + +### nav_mc_pos_deceleration_time + +Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting + +| Default | Min | Max | +| --- | --- | --- | +| 120 | 0 | 255 | + +--- + +### nav_mc_pos_expo + +Expo for PosHold control + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### nav_mc_pos_xy_p + +Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity + +| Default | Min | Max | +| --- | --- | --- | +| 65 | 0 | 255 | + +--- + +### nav_mc_pos_z_p + +P gain of altitude PID controller (Multirotor) + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 255 | + +--- + +### nav_mc_vel_xy_d + +D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 255 | + +--- + +### nav_mc_vel_xy_dterm_attenuation + +Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. + +| Default | Min | Max | +| --- | --- | --- | +| 90 | 0 | 100 | + +--- + +### nav_mc_vel_xy_dterm_attenuation_end + +A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum + +| Default | Min | Max | +| --- | --- | --- | +| 60 | 0 | 100 | + +--- + +### nav_mc_vel_xy_dterm_attenuation_start + +A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 100 | + +--- + +### nav_mc_vel_xy_dterm_lpf_hz + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 100 | + +--- + +### nav_mc_vel_xy_ff + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 255 | + +--- + +### nav_mc_vel_xy_i + +I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 255 | + +--- + +### nav_mc_vel_xy_p + +P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 0 | 255 | + +--- + +### nav_mc_vel_z_d + +D gain of velocity PID controller + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + +### nav_mc_vel_z_i + +I gain of velocity PID controller + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 255 | + +--- + +### nav_mc_vel_z_p + +P gain of velocity PID controller + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 255 | + +--- + +### nav_mc_wp_slowdown + +When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### nav_min_rth_distance + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 0 | 5000 | + +--- + +### nav_overrides_motor_stop + +When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. 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 on NAV modes which take control of the throttle when 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 | Min | Max | +| --- | --- | --- | +| ALL_NAV | | | + +--- + +### nav_position_timeout + +If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 10 | + +--- + +### nav_rth_abort_threshold + +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] + +| Default | Min | Max | +| --- | --- | --- | +| 50000 | | 65000 | + +--- + +### nav_rth_allow_landing + +If set to ON drone will land as a last phase of RTH. + +| Default | Min | Max | +| --- | --- | --- | +| ALWAYS | | | + +--- + +### nav_rth_alt_control_override + +If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing) + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_rth_alt_mode + +Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details + +| Default | Min | Max | +| --- | --- | --- | +| AT_LEAST | | | + +--- + +### nav_rth_altitude + +Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | | 65000 | + +--- + +### nav_rth_climb_first + +If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### nav_rth_climb_ignore_emerg + +If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_rth_home_altitude + +Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 65000 | + +--- + +### nav_rth_tail_first + +If set to ON drone will return tail-first. Obviously meaningless for airplanes. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_use_fw_yaw_control + +Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_use_midthr_for_althold + +If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_user_control_mode + +Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. + +| Default | Min | Max | +| --- | --- | --- | +| ATTI | | | + +--- + +### nav_wp_load_on_boot + +If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_wp_radius + +Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 10 | 10000 | + +--- + +### nav_wp_safe_distance + +First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. + +| Default | Min | Max | +| --- | --- | --- | +| 10000 | | 65000 | + +--- + +### opflow_hardware + +Selection of OPFLOW hardware. + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### opflow_scale + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 10.5 | 0 | 10000 | + +--- + +### osd_ahi_bordered + +Shows a border/corners around the AHI region (pixel OSD only) + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_ahi_camera_uptilt_comp + +When set to `ON`, the AHI position is adjusted by `osd_camera_uptilt`. For example, with a cammera uptilt of 30 degrees, the AHI will appear in the middle of the OSD when the aircraft is pitched forward 30 degrees. When set to `OFF`, the AHI will appear in the center of the OSD regardless of camera angle, but can still be shifted up and down using `osd_horizon_offset` (`osd_ahi_vertical_offset` for pixel-OSD). + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_ahi_height + +AHI height in pixels (pixel OSD only) + +| Default | Min | Max | +| --- | --- | --- | +| 162 | | 255 | + +--- + +### osd_ahi_max_pitch + +Max pitch, in degrees, for OSD artificial horizon + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 10 | 90 | + +--- + +### osd_ahi_reverse_roll + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_ahi_style + +Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. + +| Default | Min | Max | +| --- | --- | --- | +| DEFAULT | | | + +--- + +### osd_ahi_vertical_offset + +AHI vertical offset from center (pixel OSD only) + +| Default | Min | Max | +| --- | --- | --- | +| -18 | -128 | 127 | + +--- + +### osd_ahi_width + +AHI width in pixels (pixel OSD only) + +| Default | Min | Max | +| --- | --- | --- | +| 132 | | 255 | + +--- + +### osd_alt_alarm + +Value above which to make the OSD relative altitude indicator blink (meters) + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 10000 | + +--- + +### osd_baro_temp_alarm_max + +Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) + +| Default | Min | Max | +| --- | --- | --- | +| 600 | -550 | 1250 | + +--- + +### osd_baro_temp_alarm_min + +Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) + +| Default | Min | Max | +| --- | --- | --- | +| -200 | -550 | 1250 | + +--- + +### osd_camera_fov_h + +Horizontal field of view for the camera in degres + +| Default | Min | Max | +| --- | --- | --- | +| 135 | 60 | 150 | + +--- + +### osd_camera_fov_v + +Vertical field of view for the camera in degres + +| Default | Min | Max | +| --- | --- | --- | +| 85 | 30 | 120 | + +--- + +### osd_camera_uptilt + +Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal. Used for correct display of HUD items and AHI (when enabled). + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -40 | 80 | + +--- + +### osd_coordinate_digits + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 9 | 8 | 11 | + +--- + +### osd_crosshairs_style + +To set the visual type for the crosshair + +| Default | Min | Max | +| --- | --- | --- | +| DEFAULT | | | + +--- + +### osd_crsf_lq_format + +To select LQ format + +| Default | Min | Max | +| --- | --- | --- | +| TYPE1 | | | + +--- + +### osd_current_alarm + +Value above which the OSD current consumption element will start blinking. Measured in full Amperes. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 255 | + +--- + +### osd_dist_alarm + +Value above which to make the OSD distance from home indicator blink (meters) + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 0 | 50000 | + +--- + +### osd_esc_temp_alarm_max + +Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) + +| Default | Min | Max | +| --- | --- | --- | +| 900 | -550 | 1500 | + +--- + +### osd_esc_temp_alarm_min + +Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) + +| Default | Min | Max | +| --- | --- | --- | +| -200 | -550 | 1500 | + +--- + +### osd_estimations_wind_compensation + +Use wind estimation for remaining flight time/distance estimation + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### osd_failsafe_switch_layout + +If enabled the OSD automatically switches to the first layout during failsafe + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_force_grid + +Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_gforce_alarm + +Value above which the OSD g force indicator will blink (g) + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 20 | + +--- + +### osd_gforce_axis_alarm_max + +Value above which the OSD axis g force indicators will blink (g) + +| Default | Min | Max | +| --- | --- | --- | +| 5 | -20 | 20 | + +--- + +### osd_gforce_axis_alarm_min + +Value under which the OSD axis g force indicators will blink (g) + +| Default | Min | Max | +| --- | --- | --- | +| -5 | -20 | 20 | + +--- + +### osd_home_position_arm_screen + +Should home position coordinates be displayed on the arming screen. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### osd_horizon_offset + +To vertically adjust the whole OSD and AHI and scrolling bars + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -2 | 2 | + +--- + +### osd_hud_homepoint + +To 3D-display the home point location in the hud + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_hud_homing + +To display little arrows around the crossair showing where the home point is in the hud + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_hud_margin_h + +Left and right margins for the hud area + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 0 | 4 | + +--- + +### osd_hud_margin_v + +Top and bottom margins for the hud area + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 1 | 3 | + +--- + +### osd_hud_radar_disp + +Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4 | + +--- + +### osd_hud_radar_nearest + +To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 4000 | + +--- + +### osd_hud_radar_range_max + +In meters, radar aircrafts further away than this will not be displayed in the hud + +| Default | Min | Max | +| --- | --- | --- | +| 4000 | 100 | 9990 | + +--- + +### osd_hud_radar_range_min + +In meters, radar aircrafts closer than this will not be displayed in the hud + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 1 | 30 | + +--- + +### osd_hud_wp_disp + +How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 3 | + +--- + +### osd_imu_temp_alarm_max + +Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) + +| Default | Min | Max | +| --- | --- | --- | +| 600 | -550 | 1250 | + +--- + +### osd_imu_temp_alarm_min + +Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) + +| Default | Min | Max | +| --- | --- | --- | +| -200 | -550 | 1250 | + +--- + +### osd_left_sidebar_scroll + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### osd_left_sidebar_scroll_step + +How many units each sidebar step represents. 0 means the default value for the scroll type. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 255 | + +--- + +### osd_link_quality_alarm + +LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% + +| Default | Min | Max | +| --- | --- | --- | +| 70 | 0 | 100 | + +--- + +### osd_main_voltage_decimals + +Number of decimals for the battery voltages displayed in the OSD [1-2]. + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 2 | + +--- + +### osd_neg_alt_alarm + +Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 10000 | + +--- + +### osd_pan_servo_index + +Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 10 | + +--- + +### osd_pan_servo_pwm2centideg + +Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -36 | 36 | + +--- + +### osd_plus_code_digits + +Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. + +| Default | Min | Max | +| --- | --- | --- | +| 11 | 10 | 13 | + +--- + +### osd_plus_code_short + +Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | | + +--- + +### osd_right_sidebar_scroll + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### osd_right_sidebar_scroll_step + +Same as left_sidebar_scroll_step, but for the right sidebar + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 255 | + +--- + +### osd_row_shiftdown + +Number of rows to shift the OSD display (increase if top rows are cut off) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 1 | + +--- + +### osd_rssi_alarm + +Value below which to make the OSD RSSI indicator blink + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 100 | + +--- + +### osd_rssi_dbm_alarm + +RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -130 | 0 | + +--- + +### osd_sidebar_height + +Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 0 | 5 | + +--- + +### osd_sidebar_horizontal_offset + +Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -128 | 127 | + +--- + +### osd_sidebar_scroll_arrows + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_snr_alarm + +Value below which Crossfire SNR Alarm pops-up. (dB) + +| Default | Min | Max | +| --- | --- | --- | +| 4 | -20 | 10 | + +--- + +### osd_speed_source + +Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR + +| Default | Min | Max | +| --- | --- | --- | +| GROUND | | | + +--- + +### osd_stats_energy_unit + +Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) + +| Default | Min | Max | +| --- | --- | --- | +| MAH | | | + +--- + +### osd_stats_min_voltage_unit + +Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. + +| Default | Min | Max | +| --- | --- | --- | +| BATTERY | | | + +--- + +### osd_telemetry + +To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### osd_temp_label_align + +Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` + +| Default | Min | Max | +| --- | --- | --- | +| LEFT | | | + +--- + +### osd_time_alarm + +Value above which to make the OSD flight time indicator blink (minutes) + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 600 | + +--- + +### osd_units + +IMPERIAL, METRIC, UK + +| Default | Min | Max | +| --- | --- | --- | +| METRIC | | | + +--- + +### osd_video_system + +Video system used. Possible values are `AUTO`, `PAL` and `NTSC` + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### pid_type + +Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### pidsum_limit + +A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help + +| Default | Min | Max | +| --- | --- | --- | +| 500 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | + +--- + +### pidsum_limit_yaw + +A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help + +| Default | Min | Max | +| --- | --- | --- | +| 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | + +--- + +### pinio_box1 + +Mode assignment for PINIO#1 + +| Default | Min | Max | +| --- | --- | --- | +| `BOX_PERMANENT_ID_NONE` | 0 | 255 | + +--- + +### pinio_box2 + +Mode assignment for PINIO#1 + +| Default | Min | Max | +| --- | --- | --- | +| `BOX_PERMANENT_ID_NONE` | 0 | 255 | + +--- + +### pinio_box3 + +Mode assignment for PINIO#1 + +| Default | Min | Max | +| --- | --- | --- | +| `BOX_PERMANENT_ID_NONE` | 0 | 255 | + +--- + +### pinio_box4 + +Mode assignment for PINIO#1 + +| Default | Min | Max | +| --- | --- | --- | +| `BOX_PERMANENT_ID_NONE` | 0 | 255 | + +--- + +### pitch_rate + +Defines rotation rate on PITCH 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. + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 4 | 180 | + +--- + +### pitot_hardware + +Selection of pitot hardware. + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### pitot_lpf_milli_hz + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 350 | 0 | 10000 | + +--- + +### pitot_scale + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1.0 | 0 | 100 | + +--- + +### platform_type + +Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented + +| Default | Min | Max | +| --- | --- | --- | +| MULTIROTOR | | | + +--- + +### pos_hold_deadband + +Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes. + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 2 | 250 | + +--- + +### prearm_timeout + +Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. + +| Default | Min | Max | +| --- | --- | --- | +| 10000 | 0 | 10000 | + +--- + +### rangefinder_hardware + +Selection of rangefinder hardware. + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + +### rangefinder_median_filter + +3-point median filtering for rangefinder readouts + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### rate_accel_limit_roll_pitch + +Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 500000 | + +--- + +### rate_accel_limit_yaw + +Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. + +| Default | Min | Max | +| --- | --- | --- | +| 10000 | | 500000 | + +--- + +### rc_expo + +Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) + +| Default | Min | Max | +| --- | --- | --- | +| 70 | 0 | 100 | + +--- + +### rc_filter_frequency + +RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### rc_yaw_expo + +Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 100 | + +--- + +### reboot_character + +Special character used to trigger reboot + +| Default | Min | Max | +| --- | --- | --- | +| 82 | 48 | 126 | + +--- + +### receiver_type + +Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | | | + +--- + +### report_cell_voltage + +S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### roll_rate + +Defines rotation rate on ROLL 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. + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 4 | 180 | + +--- + +### rpm_gyro_filter_enabled + +Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### rpm_gyro_harmonics + +Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 3 | + +--- + +### rpm_gyro_min_hz + +The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 30 | 200 | + +--- + +### rpm_gyro_q + +Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 1 | 3000 | + +--- + +### rssi_adc_channel + +ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | + +--- + +### rssi_channel + +RX channel containing the RSSI signal + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### rssi_max + +The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | + +--- + +### rssi_min + +The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). + +| Default | Min | Max | +| --- | --- | --- | +| 0 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | + +--- + +### rssi_source + +Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### rth_energy_margin + +Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 100 | + +--- + +### rx_max_usec + +Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. + +| Default | Min | Max | +| --- | --- | --- | +| 2115 | PWM_PULSE_MIN | PWM_PULSE_MAX | + +--- + +### rx_min_usec + +Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. + +| Default | Min | Max | +| --- | --- | --- | +| 885 | PWM_PULSE_MIN | PWM_PULSE_MAX | + +--- + +### rx_spi_id + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 0 | + +--- + +### rx_spi_protocol + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | | | + +--- + +### rx_spi_rf_channel_count + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 8 | + +--- + +### safehome_max_distance + +In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. + +| Default | Min | Max | +| --- | --- | --- | +| 20000 | 0 | 65000 | + +--- + +### safehome_usage_mode + +Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information. + +| Default | Min | Max | +| --- | --- | --- | +| RTH | | | + +--- + +### sbus_sync_interval + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 3000 | 500 | 10000 | + +--- + +### sdcard_detect_inverted + +This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | | | + +--- + +### serialrx_halfduplex + +Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. + +| Default | Min | Max | +| --- | --- | --- | +| AUTO | | | + +--- + +### serialrx_inverted + +Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### serialrx_provider + +When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | | | + +--- + +### servo_autotrim_rotation_limit + +Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 1 | 60 | + +--- + +### servo_center_pulse + +Servo midpoint + +| Default | Min | Max | +| --- | --- | --- | +| 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### servo_lpf_hz + +Selects the servo PWM output cutoff frequency. Value is in [Hz] + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 400 | + +--- + +### servo_protocol + +An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) + +| Default | Min | Max | +| --- | --- | --- | +| PWM | | | + +--- + +### servo_pwm_rate + +Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 50 | 498 | + +--- + +### setpoint_kalman_enabled + +Enable Kalman filter on the gyro data + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### setpoint_kalman_q + +Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 1 | 16000 | + +--- + +### sim_ground_station_number + +Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. + +| Default | Min | Max | +| --- | --- | --- | +| _empty_ | | | + +--- + +### sim_low_altitude + +Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. + +| Default | Min | Max | +| --- | --- | --- | +| -32767 | -32768 | 32767 | + +--- + +### sim_pin + +PIN code for the SIM module + +| Default | Min | Max | +| --- | --- | --- | +| 0000 | | | + +--- + +### sim_transmit_flags + +Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` + +| Default | Min | Max | +| --- | --- | --- | +| `SIM_TX_FLAG_FAILSAFE` | | 63 | + +--- + +### sim_transmit_interval + +Text message transmission interval in seconds for SIM module. Minimum value: 10 + +| Default | Min | Max | +| --- | --- | --- | +| 60 | SIM_MIN_TRANSMIT_INTERVAL | 65535 | + +--- + +### small_angle + +If the aircraft tilt angle exceed this value the copter will refuse to arm. + +| Default | Min | Max | +| --- | --- | --- | +| 25 | 0 | 180 | + +--- + +### smartport_fuel_unit + +S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] + +| Default | Min | Max | +| --- | --- | --- | +| MAH | | | + +--- + +### smartport_master_halfduplex + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### smartport_master_inverted + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### smith_predictor_delay + +Expected delay of the gyro signal. In milliseconds + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 8 | + +--- + +### smith_predictor_lpf_hz + +Cutoff frequency for the Smith Predictor Low Pass Filter + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 1 | 500 | + +--- + +### smith_predictor_strength + +The strength factor of a Smith Predictor of PID measurement. In percents + +| Default | Min | Max | +| --- | --- | --- | +| 0.5 | 0 | 1 | + +--- + +### spektrum_sat_bind + +0 = disabled. Used to bind the spektrum satellite to RX + +| Default | Min | Max | +| --- | --- | --- | +| `SPEKTRUM_SAT_BIND_DISABLED` | SPEKTRUM_SAT_BIND_DISABLED | SPEKTRUM_SAT_BIND_MAX | + +--- + +### srxl2_baud_fast + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### srxl2_unit_id + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 15 | + +--- + +### stats + +General switch of the statistics recording feature (a.k.a. odometer) + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### stats_total_dist + +Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 2147483647 | + +--- + +### stats_total_energy + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 2147483647 | + +--- + +### stats_total_time + +Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 2147483647 | + +--- + +### switch_disarm_delay + +Delay before disarming when requested by switch (ms) [0-1000] + +| Default | Min | Max | +| --- | --- | --- | +| 250 | 0 | 1000 | + +--- + +### telemetry_halfduplex + +S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### telemetry_inverted + +Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### telemetry_switch + +Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### thr_comp_weight + +Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 0 | 2 | + +--- + +### thr_expo + +Throttle exposition value + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 100 | + +--- + +### thr_mid + +Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 0 | 100 | + +--- + +### throttle_idle + +The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. + +| Default | Min | Max | +| --- | --- | --- | +| 15 | 0 | 30 | + +--- + +### throttle_scale + +Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% + +| Default | Min | Max | +| --- | --- | --- | +| 1.0 | 0 | 1 | + +--- + +### throttle_tilt_comp_str + +Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 100 | + +--- + +### tpa_breakpoint + +See tpa_rate. + +| Default | Min | Max | +| --- | --- | --- | +| 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | + +--- + +### tpa_rate + +Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 100 | + +--- + +### tri_unarmed_servo + +On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### turtle_mode_power_factor + +Turtle mode power factor + +| Default | Min | Max | +| --- | --- | --- | +| 55 | 0 | 100 | + +--- + +### tz_automatic_dst + +Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### tz_offset + +Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1440 | 1440 | + +--- + +### vbat_adc_channel + +ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | + +--- + +### vbat_cell_detect_voltage + +Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. + +| Default | Min | Max | +| --- | --- | --- | +| 425 | 100 | 500 | + +--- + +### vbat_max_cell_voltage + +Maximum voltage per cell in 0.01V units, default is 4.20V + +| Default | Min | Max | +| --- | --- | --- | +| 420 | 100 | 500 | + +--- + +### vbat_meter_type + +Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running + +| Default | Min | Max | +| --- | --- | --- | +| ADC | | | + +--- + +### vbat_min_cell_voltage + +Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) + +| Default | Min | Max | +| --- | --- | --- | +| 330 | 100 | 500 | + +--- + +### vbat_scale + +Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. + +| Default | Min | Max | +| --- | --- | --- | +| _target default_ | 0 | 65535 | + +--- + +### vbat_warning_cell_voltage + +Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) + +| Default | Min | Max | +| --- | --- | --- | +| 350 | 100 | 500 | + +--- + +### vtx_band + +Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. + +| Default | Min | Max | +| --- | --- | --- | +| 4 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | + +--- + +### vtx_channel + +Channel to use within the configured `vtx_band`. Valid values are [1, 8]. + +| Default | Min | Max | +| --- | --- | --- | +| 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | + +--- + +### vtx_halfduplex + +Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### vtx_low_power_disarm + +When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### vtx_max_power_override + +Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 10000 | + +--- + +### vtx_pit_mode_chan + +_// TODO_ + +| Default | Min | Max | +| --- | --- | --- | +| 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | + +--- + +### vtx_power + +VTX RF power level to use. The exact number of mw depends on the VTX hardware. + +| Default | Min | Max | +| --- | --- | --- | +| 1 | VTX_SETTINGS_MIN_POWER | VTX_SETTINGS_MAX_POWER | + +--- + +### vtx_smartaudio_early_akk_workaround + +Enable workaround for early AKK SAudio-enabled VTX bug. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + +### yaw_deadband + +These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 100 | + +--- + +### yaw_lpf_hz + +Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 200 | + +--- + +### yaw_rate + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 1 | 180 | + +--- -> Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file diff --git a/docs/assets/betafpvf722.png b/docs/assets/betafpvf722.png new file mode 100644 index 0000000000..626deacaa9 Binary files /dev/null and b/docs/assets/betafpvf722.png differ diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 98281caeef..4f7365952d 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions.svg b/docs/assets/images/StickPositions.svg index 49750eb870..3f32b52523 100644 --- a/docs/assets/images/StickPositions.svg +++ b/docs/assets/images/StickPositions.svg @@ -2,23 +2,23 @@ + inkscape:export-ydpi="74.996788" + xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" + xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" + xmlns="http://www.w3.org/2000/svg" + xmlns:svg="http://www.w3.org/2000/svg" + xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#" + xmlns:cc="http://creativecommons.org/ns#" + xmlns:dc="http://purl.org/dc/elements/1.1/"> + inkscape:object-nodes="true" + inkscape:pagecheckerboard="0" /> @@ -58,7 +59,7 @@ image/svg+xml - + @@ -160,7 +161,7 @@ x="533.55804" style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" xml:space="preserve">Profile 2 + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Profile 2 Calibrate Gyro + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Calibrate Gyro Calibrate Acc + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Calibrate Acc Trim Acc Left + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Left Trim Acc Right + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Right Trim Acc Backwards + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Backwards + + Load waypoint mission + Unload waypoint mission + style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.0566;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" /> + + Save Setting + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Save Setting Enter OSD Menu (CMS) + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Enter OSD Menu (CMS) Mode 2 Stick Functions + style="font-size:352.858px;line-height:1.25;text-align:center;text-anchor:middle">Mode 2 Stick Functions Battery profile 1 + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Battery profile 1 Battery profile 3 + style="font-size:286.599px;line-height:1.25;text-align:end;text-anchor:end">Battery profile 3 ` must be replaced with the name of the target that you want to b ## Windows 10 Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` . +If you are getting error "standard_init_linux.go:219: exec user process caused: no such file or directory", make sure `\cmake\docker.sh` has lf (not crlf) line endings. You'll have to manually execute the same steps that the build script does: 1. `docker build -t inav-build .` + This step is only needed the first time. -2. `docker run --rm -it -v :/src inav-build ` +2. `docker run --rm -it -u root -v :/src inav-build ` + Where `` must be replaced with the absolute path of where you cloned this repo (see above), and `` with the name of the target that you want to build. + + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above, or by removing "USER inav" line from the .\DockerFile before building image. Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details. \ No newline at end of file diff --git a/docs/development/Building in Linux.md b/docs/development/Building in Linux.md index 73287a216b..765286d012 100644 --- a/docs/development/Building in Linux.md +++ b/docs/development/Building in Linux.md @@ -65,7 +65,7 @@ The `git clone` creates an `inav` directory; we can enter this directory, config ## Build tooling -For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplies various platform and hardware dependencies required to cross compile multiple targets. `cmake` still uses GNU `make` to invoke the actual compiler. It is necessary to configure the build enviroment with `cmake` before we can build any firmware. +For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplifies various platform and hardware dependencies required to cross compile multiple targets. `cmake` still uses GNU `make` to invoke the actual compiler. It is necessary to configure the build environment with `cmake` before we can build any firmware. ## Using `cmake` @@ -96,14 +96,14 @@ Once `cmake` has generated the `build/Makefile`, this `Makfile` (with `make`) is The generated `Makefile` uses different a target selection mechanism from the older (pre 2.6) top level `Makefile`; you can generate a list of targets with `make help` (or, as the list is extremely long), pipe this into a pager, e.g. `make help | less`. -Typically, to build a single target, just pass the target name to `make`; note that unlike eariler releases, `make` without a target specified will build **all** targets. +Typically, to build a single target, just pass the target name to `make`; note that unlike earlier releases, `make` without a target specified will build **all** targets. ``` # Build the MATEKF405 firmware make MATEKF405 ``` -One can also build multiple targets from a sinlge `make` command: +One can also build multiple targets from a single `make` command: ``` make MATEKF405 MATEKF722 @@ -128,7 +128,7 @@ make clean_MATEKF405 make clean_MATEKF405 clean_MATEKF722 ``` -### `cmake` cache maintainance +### `cmake` cache maintenance `cmake` caches the build environment, so you don't need to rerun `cmake` each time you build a target. Two `make` options are provided to maintain the `cmake` cache. @@ -141,7 +141,7 @@ It is unlikely that the typical user will need to employ these options, other th In order to update your local firmware build: -* Navigate to the local iNav repository +* Navigate to the local inav repository * Use the following steps to pull the latest changes and rebuild your local version of inav firmware from the `build` directory: ``` @@ -154,3 +154,9 @@ $ make ## Advanced Usage For more advanced development information and `git` usage, please refer to the [development guide](https://github.com/iNavFlight/inav/blob/master/docs/development/Development.md). + +## Unsupported platforms + +If you're using a host platform for which Arm does not supply a cross-compiler (Arm32, IA32), and the distro either does not package a suitable compiler or it's too old, then you can usually find a suitable compiler in the [xpack devtools collection](https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack). + +You will need to configure `cmake` to use the external compiler. diff --git a/docs/development/Building in Windows 10 with Linux Subsystem.md b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md similarity index 94% rename from docs/development/Building in Windows 10 with Linux Subsystem.md rename to docs/development/Building in Windows 10 or 11 with Linux Subsystem.md index 2b8439c543..b03797d8c4 100644 --- a/docs/development/Building in Windows 10 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md @@ -20,13 +20,13 @@ Install Ubuntu: NOTE: from this point all commands are entered into the Ubunto shell command window Update the repo packages: -1. `sudo apt update` +- `sudo apt update` Install Git, Make, gcc and Ruby -1. `sudo apt-get install git` -1. `sudo apt-get install make` -1. `sudo apt-get install cmake` -1. `sudo apt-get install ruby` +- `sudo apt-get install git make cmake ruby` + +Install python and python-yaml to allow updates to settings.md +- `sudo apt-get install python3 python-yaml` ### CMAKE and Ubuntu 18_04 @@ -78,6 +78,12 @@ cd build make MATEKF722 ``` +## Updating the documents +``` +cd /mnt/c/inav +python3 src/utils/update_cli_docs.py +``` + ## Flashing: Launch windows configurator GUI and from within the firmware flasher select `Load firmware[Local]` Hex files can be found in the folder `c:\inav\build` diff --git a/docs/development/Building in Windows 10 with MSYS2.md b/docs/development/Building in Windows 10 or 11 with MSYS2.md similarity index 100% rename from docs/development/Building in Windows 10 with MSYS2.md rename to docs/development/Building in Windows 10 or 11 with MSYS2.md diff --git a/docs/development/IDE - Visual Studio Code with Windows 10.md b/docs/development/IDE - Visual Studio Code with Windows 10.md index 6095fec4e3..ee83ce74af 100644 --- a/docs/development/IDE - Visual Studio Code with Windows 10.md +++ b/docs/development/IDE - Visual Studio Code with Windows 10.md @@ -79,30 +79,6 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard // for the documentation about the tasks.json format "version": "2.0.0", "tasks": [ - { - "label": "Build Matek F722-SE", - "type": "shell", - "command": "make MATEKF722SE", - "group": "build", - "problemMatcher": [], - "options": { - "cwd": "${workspaceFolder}/build" - } - }, - { - "label": "Build Matek F722", - "type": "shell", - "command": "make MATEKF722", - "group": { - "kind": "build", - "isDefault": true - }, - "problemMatcher": [], - "options": { - "cwd": "${workspaceFolder}/build" - } - } - , { "label": "Install/Update CMAKE", "type": "shell", @@ -112,6 +88,37 @@ Edit `./.vscode/tasks.json` to enable Building with `Ctrl + Shift + B` keyboard "options": { "cwd": "${workspaceFolder}" } + }, + { + "label": "Compile autogenerated docs", + "type": "shell", + "command": "python3 src/utils/update_cli_docs.py", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + // Example of building a single target + { + "label": "Build Matek F722-WPX", + "type": "shell", + "command": "make MATEKF722WPX", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + }, + // Example of building multiple targets + { + "label": "Build Matek F405-STD & WING", + "type": "shell", + "command": "make MATEKF405 MATEKF405SE", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } } ] } diff --git a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c index 3d1d792425..d30bc514b9 100644 --- a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c +++ b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_exti.c @@ -20,11 +20,6 @@ /* Includes ------------------------------------------------------------------*/ #include "stm32h7xx_ll_exti.h" -#ifdef USE_FULL_ASSERT -#include "stm32_assert.h" -#else -#define assert_param(expr) ((void)0U) -#endif /** @addtogroup STM32H7xx_LL_Driver * @{ diff --git a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c index a1eac83a9c..ed7104bbeb 100644 --- a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c +++ b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_i2c.c @@ -21,11 +21,6 @@ /* Includes ------------------------------------------------------------------*/ #include "stm32h7xx_ll_i2c.h" #include "stm32h7xx_ll_bus.h" -#ifdef USE_FULL_ASSERT -#include "stm32_assert.h" -#else -#define assert_param(expr) ((void)0U) -#endif /** @addtogroup STM32H7xx_LL_Driver * @{ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 75e03ea448..00968141ee 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -242,7 +242,7 @@ main_sources(COMMON_SRC drivers/rangefinder/rangefinder_virtual.h drivers/rangefinder/rangefinder_us42.c drivers/rangefinder/rangefinder_us42.h - + drivers/resource.c drivers/resource.h drivers/rcc.c @@ -280,6 +280,7 @@ main_sources(COMMON_SRC fc/config.h fc/controlrate_profile.c fc/controlrate_profile.h + fc/controlrate_profile_config_struct.h fc/fc_core.c fc/fc_core.h fc/fc_init.c @@ -440,6 +441,7 @@ main_sources(COMMON_SRC sensors/acceleration.h sensors/battery.c sensors/battery.h + sensors/battery_config_structs.h sensors/boardalignment.c sensors/boardalignment.h sensors/compass.c diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index b6a561fecd..ae4eb1a3a4 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -51,5 +51,11 @@ #define EXTENDED_FASTRAM #endif +#ifdef STM32H7 +#define DMA_RAM __attribute__ ((section(".DMA_RAM"))) +#else +#define DMA_RAM +#endif + #define STATIC_FASTRAM static FASTRAM #define STATIC_FASTRAM_UNIT_TESTED STATIC_UNIT_TESTED FASTRAM diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index ff9d246ee2..6060ba40d7 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -181,6 +181,30 @@ static const CMS_Menu cmsx_menuFWSettings = { .entries = cmsx_menuFWSettingsEntries }; +static const OSD_Entry cmsx_menuMissionSettingsEntries[] = +{ + OSD_LABEL_ENTRY("-- MISSIONS --"), + + OSD_SETTING_ENTRY("MC WP SLOWDOWN", SETTING_NAV_MC_WP_SLOWDOWN), + OSD_SETTING_ENTRY("MISSION FAILSAFE", SETTING_FAILSAFE_MISSION), + OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT), + OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS), + OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE), + + OSD_BACK_AND_END_ENTRY, + }; + +static const CMS_Menu cmsx_menuMissionSettings = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUMISSIONSETTINGS", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuMissionSettingsEntries +}; + static const OSD_Entry cmsx_menuNavigationEntries[] = { OSD_LABEL_ENTRY("-- NAVIGATION --"), @@ -188,6 +212,7 @@ static const OSD_Entry cmsx_menuNavigationEntries[] = OSD_SUBMENU_ENTRY("BASIC SETTINGS", &cmsx_menuNavSettings), OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH), OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFWSettings), + OSD_SUBMENU_ENTRY("MISSIONS", &cmsx_menuMissionSettings), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c index 4c5efba684..be67a0932f 100644 --- a/src/main/common/calibration.c +++ b/src/main/common/calibration.c @@ -34,7 +34,7 @@ void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float t { // Reset parameters and state s->params.state = ZERO_CALIBRATION_IN_PROGRESS; - s->params.startTimeMs = millis(); + s->params.startTimeMs = 0; s->params.windowSizeMs = window; s->params.stdDevThreshold = threshold; s->params.allowFailure = allowFailure; @@ -60,6 +60,13 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v) return; } + // An unknown delay may have passed between `zeroCalibrationStartS` and first sample acquisition + // therefore our window measurement might be incorrect + // To account for that we reset the startTimeMs when acquiring the first sample + if (s->params.sampleCount == 0 && s->params.startTimeMs == 0) { + s->params.startTimeMs = millis(); + } + // Add value s->val.accumulatedValue += v; s->params.sampleCount++; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 0656a5dba7..afa986e8df 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -109,6 +109,72 @@ void pt1FilterReset(pt1Filter_t *filter, float input) filter->state = input; } +/* + * PT2 LowPassFilter + */ +float pt2FilterGain(float f_cut, float dT) +{ + const float order = 2.0f; + const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1); + float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut); + // float RC = 1 / (2 * 1.553773974f * M_PIf * f_cut); + // where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2 + return dT / (RC + dT); +} + +void pt2FilterInit(pt2Filter_t *filter, float k) +{ + filter->state = 0.0f; + filter->state1 = 0.0f; + filter->k = k; +} + +void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) +{ + filter->k = k; +} + +FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input) +{ + filter->state1 = filter->state1 + filter->k * (input - filter->state1); + filter->state = filter->state + filter->k * (filter->state1 - filter->state); + return filter->state; +} + +/* + * PT3 LowPassFilter + */ +float pt3FilterGain(float f_cut, float dT) +{ + const float order = 3.0f; + const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1); + float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut); + // float RC = 1 / (2 * 1.961459177f * M_PIf * f_cut); + // where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3 + return dT / (RC + dT); +} + +void pt3FilterInit(pt3Filter_t *filter, float k) +{ + filter->state = 0.0f; + filter->state1 = 0.0f; + filter->state2 = 0.0f; + filter->k = k; +} + +void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) +{ + filter->k = k; +} + +FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input) +{ + filter->state1 = filter->state1 + filter->k * (input - filter->state1); + filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2); + filter->state = filter->state + filter->k * (filter->state2 - filter->state); + return filter->state; +} + // rate_limit = maximum rate of change of the output value in units per second void rateLimitFilterInit(rateLimitFilter_t *filter) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index a6406ee91c..7e6a579efe 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -27,6 +27,17 @@ typedef struct pt1Filter_s { float dT; float alpha; } pt1Filter_t; +typedef struct pt2Filter_s { + float state; + float state1; + float k; +} pt2Filter_t; +typedef struct pt3Filter_s { + float state; + float state1; + float state2; + float k; +} pt3Filter_t; /* this holds the data required to update samples thru a filter */ typedef struct biquadFilter_s { @@ -87,6 +98,22 @@ float pt1FilterApply3(pt1Filter_t *filter, float input, float dT); float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); void pt1FilterReset(pt1Filter_t *filter, float input); +/* + * PT2 LowPassFilter + */ +float pt2FilterGain(float f_cut, float dT); +void pt2FilterInit(pt2Filter_t *filter, float k); +void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k); +float pt2FilterApply(pt2Filter_t *filter, float input); + +/* + * PT3 LowPassFilter + */ +float pt3FilterGain(float f_cut, float dT); +void pt3FilterInit(pt3Filter_t *filter, float k); +void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k); +float pt3FilterApply(pt3Filter_t *filter, float input); + void rateLimitFilterInit(rateLimitFilter_t *filter); float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 26b3fe21ba..a99c3f346b 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -25,6 +25,10 @@ #include "quaternion.h" #include "platform.h" +#ifdef USE_ARM_MATH +#include "arm_math.h" +#endif + FILE_COMPILE_FOR_SPEED // http://lolengine.net/blog/2011/12/21/better-function-approximations @@ -95,7 +99,7 @@ float atan2_approx(float y, float x) float acos_approx(float x) { float xa = fabsf(x); - float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa)))); + float result = fast_fsqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa)))); if (x < 0.0f) return M_PIf - result; else @@ -200,7 +204,7 @@ float devVariance(stdev_t *dev) float devStandardDeviation(stdev_t *dev) { - return sqrtf(devVariance(dev)); + return fast_fsqrtf(devVariance(dev)); } float degreesToRadians(int16_t degrees) @@ -508,7 +512,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu sensorCalibration_SolveLGS(state->XtX, beta, state->XtY); for (int i = 0; i < 3; i++) { - result[i] = sqrtf(beta[i]); + result[i] = fast_fsqrtf(beta[i]); } return sensorCalibrationValidateResult(result); @@ -518,3 +522,13 @@ float bellCurve(const float x, const float curveWidth) { return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth))); } + +float fast_fsqrtf(const double value) { +#ifdef USE_ARM_MATH + float squirt; + arm_sqrt_f32(value, &squirt); + return squirt; +#else + return sqrtf(value); +#endif +} \ No newline at end of file diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 6f2d9b1245..d84fc25751 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -176,3 +176,4 @@ float acos_approx(float x); void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); float bellCurve(const float x, const float curveWidth); +float fast_fsqrtf(const double value); diff --git a/src/main/common/quaternion.h b/src/main/common/quaternion.h index beaf566c55..6fbbfc7527 100644 --- a/src/main/common/quaternion.h +++ b/src/main/common/quaternion.h @@ -142,7 +142,7 @@ static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, cons static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q) { - float mod = sqrtf(quaternionNormSqared(q)); + float mod = fast_fsqrtf(quaternionNormSqared(q)); if (mod < 1e-6f) { // Length is too small - re-initialize to zero rotation result->q0 = 1; diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 25d02ea3cb..449a0973b3 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -67,7 +67,7 @@ static inline float vectorNormSquared(const fpVector3_t * v) static inline fpVector3_t * vectorNormalize(fpVector3_t * result, const fpVector3_t * v) { - float length = sqrtf(vectorNormSquared(v)); + float length = fast_fsqrtf(vectorNormSquared(v)); if (length != 0) { result->x = v->x / length; result->y = v->y / length; diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 290a9ef583..3a1783a04d 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -31,7 +31,7 @@ #endif #if defined(STM32H7) -#define ADC_VALUES_ALIGNMENT(def) __attribute__ ((section(".DMA_RAM"))) def __attribute__ ((aligned (32))) +#define ADC_VALUES_ALIGNMENT(def) DMA_RAM def __attribute__ ((aligned (32))) #else #define ADC_VALUES_ALIGNMENT(def) def #endif diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index d39f74f7bf..915e295480 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -87,7 +87,7 @@ typedef struct i2cBusState_s { I2CDevice device; bool initialized; i2cState_t state; - uint32_t timeout; + timeUs_t timeout; /* Active transfer */ bool allowRawAccess; diff --git a/src/main/drivers/bus_i2c_stm32f40x.c b/src/main/drivers/bus_i2c_stm32f40x.c index 0f4d8b45d8..ae96d65f15 100644 --- a/src/main/drivers/bus_i2c_stm32f40x.c +++ b/src/main/drivers/bus_i2c_stm32f40x.c @@ -121,7 +121,7 @@ typedef struct i2cBusState_s { I2CDevice device; bool initialized; i2cState_t state; - uint32_t timeout; + timeUs_t timeout; /* Active transfer */ bool allowRawAccess; diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 62c6db4313..5fa0872d30 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -34,7 +34,7 @@ #define SYM_AH_KM 0x08 // 008 Ah/km #define SYM_AH_MI 0x09 // 009 Ah/mi #define SYM_MAH_MI_0 0x0A // 010 mAh/mi left -#define SYM_MAH_MI_1 0x0B // 010 mAh/mi left +#define SYM_MAH_MI_1 0x0B // 011 mAh/mi right #define SYM_LQ 0x0C // 012 LQ #define SYM_TEMP_F 0x0D // 013 °F @@ -109,7 +109,7 @@ #define SYM_AH_CH_CENTER 0x7E // 126 Crossair center -// 0x7F // 127 - +#define SYM_GLIDESLOPE 0x7F // 127 Glideslope #define SYM_AH_H_START 0x80 // 128 to 136 Horizontal AHI diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 48f7a10d5c..20429e07cb 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -91,7 +91,7 @@ typedef struct { bool requestTelemetry; } pwmOutputMotor_t; -static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; +static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; static pwmOutputMotor_t motors[MAX_MOTORS]; static motorPwmProtocolTypes_e initMotorProtocol; @@ -105,11 +105,9 @@ static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; #endif -#ifdef BEEPER_PWM static pwmOutputPort_t beeperPwmPort; static pwmOutputPort_t *beeperPwm; static uint16_t beeperFrequency = 0; -#endif static uint8_t allocatedOutputPortCount = 0; @@ -634,7 +632,6 @@ void pwmWriteServo(uint8_t index, uint16_t value) } } -#ifdef BEEPER_PWM void pwmWriteBeeper(bool onoffBeep) { if (beeperPwm == NULL) @@ -666,4 +663,3 @@ void beeperPwmInit(ioTag_t tag, uint16_t frequency) pwmOutConfigTimer(beeperPwm, tch, PWM_TIMER_HZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2); } } -#endif diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 4dc37cbd16..e7ba58ac35 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -23,11 +23,10 @@ #include "drivers/time.h" #include "drivers/io.h" -#ifdef BEEPER_PWM #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" -#endif +#include "fc/config.h" #include "sound_beeper.h" @@ -44,13 +43,18 @@ void systemBeep(bool onoff) { #if !defined(BEEPER) UNUSED(onoff); -#elif defined(BEEPER_PWM) - pwmWriteBeeper(onoff); - beeperState = onoff; #else - IOWrite(beeperIO, beeperInverted ? onoff : !onoff); - beeperState = onoff; + + if (beeperConfig()->pwmMode) { + pwmWriteBeeper(onoff); + beeperState = onoff; + } else { + IOWrite(beeperIO, beeperInverted ? onoff : !onoff); + beeperState = onoff; + } + #endif + } void systemBeepToggle(void) @@ -70,11 +74,11 @@ void beeperInit(const beeperDevConfig_t *config) if (beeperIO) { IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT, 0); -#if defined(BEEPER_PWM) - beeperPwmInit(config->ioTag, BEEPER_PWM_FREQUENCY); -#else - IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); -#endif + if (beeperConfig()->pwmMode) { + beeperPwmInit(config->ioTag, BEEPER_PWM_FREQUENCY); + } else { + IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP); + } } systemBeep(false); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e74e1b9813..ef745dc858 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -119,6 +119,9 @@ uint8_t cliMode = 0; #include "sensors/opflow.h" #include "sensors/sensors.h" #include "sensors/temperature.h" +#ifdef USE_ESC_SENSOR +#include "sensors/esc_sensor.h" +#endif #include "telemetry/frsky_d.h" #include "telemetry/telemetry.h" @@ -1387,7 +1390,7 @@ static void cliWaypoints(char *cmdline) } else if (sl_strcasecmp(cmdline, "reset") == 0) { resetWaypointList(); } else if (sl_strcasecmp(cmdline, "load") == 0) { - loadNonVolatileWaypointList(); + loadNonVolatileWaypointList(true); } else if (sl_strcasecmp(cmdline, "save") == 0) { posControl.waypointListValid = false; for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { @@ -1460,7 +1463,7 @@ static void cliWaypoints(char *cmdline) if (!(validArgumentCount == 6 || validArgumentCount == 8)) { cliShowParseError(); - } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) { + } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST || flag == NAV_WP_FLAG_HOME)) { cliShowParseError(); } else { posControl.waypointList[i].action = action; @@ -2930,28 +2933,28 @@ static void printImu2Status(void) cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc); cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag); cliPrintLine("Calibration gains:"); - + cliPrintLinef( - "Gyro: %d %d %d", - secondaryImuConfig()->calibrationOffsetGyro[X], - secondaryImuConfig()->calibrationOffsetGyro[Y], + "Gyro: %d %d %d", + secondaryImuConfig()->calibrationOffsetGyro[X], + secondaryImuConfig()->calibrationOffsetGyro[Y], secondaryImuConfig()->calibrationOffsetGyro[Z] ); cliPrintLinef( - "Acc: %d %d %d", - secondaryImuConfig()->calibrationOffsetAcc[X], - secondaryImuConfig()->calibrationOffsetAcc[Y], + "Acc: %d %d %d", + secondaryImuConfig()->calibrationOffsetAcc[X], + secondaryImuConfig()->calibrationOffsetAcc[Y], secondaryImuConfig()->calibrationOffsetAcc[Z] ); cliPrintLinef( - "Mag: %d %d %d", - secondaryImuConfig()->calibrationOffsetMag[X], - secondaryImuConfig()->calibrationOffsetMag[Y], + "Mag: %d %d %d", + secondaryImuConfig()->calibrationOffsetMag[X], + secondaryImuConfig()->calibrationOffsetMag[Y], secondaryImuConfig()->calibrationOffsetMag[Z] ); cliPrintLinef( - "Radius: %d %d", - secondaryImuConfig()->calibrationRadiusAcc, + "Radius: %d %d", + secondaryImuConfig()->calibrationRadiusAcc, secondaryImuConfig()->calibrationRadiusMag ); } @@ -3193,6 +3196,18 @@ static void cliStatus(char *cmdline) hardwareSensorStatusNames[getHwGPSStatus()] ); +#ifdef USE_ESC_SENSOR + uint8_t motorCount = getMotorCount(); + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + cliPrintLinef("ESC Temperature(s): Motor Count = %d", motorCount); + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry + cliPrintf("ESC %d: %d\260C, ", i, escState->temperature); + } + cliPrintLinefeed(); + } +#endif + #ifdef USE_SDCARD cliSdInfo(NULL); #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8937097a7b..1fbafc3165 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -123,13 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = SETTING_NAME_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 2); PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, .beeper_off_flags = 0, .preferred_beeper_off_flags = 0, .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT, .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT, + .pwmMode = SETTING_BEEPER_PWM_MODE_DEFAULT, ); PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 4db38b6d6a..90d7b0d298 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -86,6 +86,7 @@ typedef struct beeperConfig_s { uint32_t preferred_beeper_off_flags; bool dshot_beeper_enabled; uint8_t dshot_beeper_tone; + bool pwmMode; } beeperConfig_t; PG_DECLARE(beeperConfig_t, beeperConfig); diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 4dc84dafe3..424e80f1ad 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -21,35 +21,10 @@ #include "config/parameter_group.h" -#define MAX_CONTROL_RATE_PROFILE_COUNT 3 +#include "fc/controlrate_profile_config_struct.h" +#include "fc/settings.h" -typedef struct controlRateConfig_s { - - struct { - uint8_t rcMid8; - uint8_t rcExpo8; - uint8_t dynPID; - uint16_t pa_breakpoint; // Breakpoint where TPA is activated - uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter - } throttle; - - struct { - uint8_t rcExpo8; - uint8_t rcYawExpo8; - uint8_t rates[3]; - } stabilized; - - struct { - uint8_t rcExpo8; - uint8_t rcYawExpo8; - uint8_t rates[3]; - } manual; - - struct { - uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis) - } misc; - -} controlRateConfig_t; +#define MAX_CONTROL_RATE_PROFILE_COUNT SETTING_CONSTANT_MAX_CONTROL_RATE_PROFILE_COUNT PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h new file mode 100644 index 0000000000..30e50d2a2b --- /dev/null +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -0,0 +1,52 @@ +/* + * This file is part of iNav + * + * iNav free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * iNav distributed in the hope that it + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +typedef struct controlRateConfig_s { + + struct { + uint8_t rcMid8; + uint8_t rcExpo8; + uint8_t dynPID; + uint16_t pa_breakpoint; // Breakpoint where TPA is activated + uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter + } throttle; + + struct { + uint8_t rcExpo8; + uint8_t rcYawExpo8; + uint8_t rates[3]; + } stabilized; + + struct { + uint8_t rcExpo8; + uint8_t rcYawExpo8; + uint8_t rates[3]; + } manual; + + struct { + uint8_t fpvCamAngleDegrees; // Camera angle to treat as "forward" base axis in ACRO (Roll and Yaw sticks will command rotation considering this axis) + } misc; + +} controlRateConfig_t; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 2fa1ea07f4..a21137ed1d 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -223,7 +223,7 @@ static void updateArmingStatus(void) /* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */ if (isNavLaunchEnabled()) { - if (areSticksDeflectedMoreThanPosHoldDeadband()) { + if (isRollPitchStickDeflected()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED); } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b80c33cfe0..10501ddb3f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -774,7 +774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->mincommand); - sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); + sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); #ifdef USE_GPS sbufWriteU8(dst, gpsConfig()->provider); // gps_type @@ -815,7 +815,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, motorConfig()->maxthrottle); sbufWriteU16(dst, motorConfig()->mincommand); - sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); + sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); #ifdef USE_GPS sbufWriteU8(dst, gpsConfig()->provider); // gps_type @@ -1041,7 +1041,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_FAILSAFE_CONFIG: sbufWriteU8(dst, failsafeConfig()->failsafe_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay); - sbufWriteU16(dst, failsafeConfig()->failsafe_throttle); + sbufWriteU16(dst, currentBatteryProfile->failsafe_throttle); sbufWriteU8(dst, 0); // was failsafe_kill_switch sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay); sbufWriteU8(dst, failsafeConfig()->failsafe_procedure); @@ -1322,7 +1322,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold); - sbufWriteU16(dst, navConfig()->mc.hover_throttle); + sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); break; case MSP_RTH_AND_LAND_CONFIG: @@ -1342,13 +1342,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_FW_CONFIG: - sbufWriteU16(dst, navConfig()->fw.cruise_throttle); - sbufWriteU16(dst, navConfig()->fw.min_throttle); - sbufWriteU16(dst, navConfig()->fw.max_throttle); + sbufWriteU16(dst, currentBatteryProfile->nav.fw.cruise_throttle); + sbufWriteU16(dst, currentBatteryProfile->nav.fw.min_throttle); + sbufWriteU16(dst, currentBatteryProfile->nav.fw.max_throttle); sbufWriteU8(dst, navConfig()->fw.max_bank_angle); sbufWriteU8(dst, navConfig()->fw.max_climb_angle); sbufWriteU8(dst, navConfig()->fw.max_dive_angle); - sbufWriteU8(dst, navConfig()->fw.pitch_to_throttle); + sbufWriteU8(dst, currentBatteryProfile->nav.fw.pitch_to_throttle); sbufWriteU16(dst, navConfig()->fw.loiter_radius); break; #endif @@ -1664,7 +1664,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) { uint8_t tmp_u8; uint16_t tmp_u16; - batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile; const unsigned int dataSize = sbufBytesRemaining(src); @@ -1867,7 +1866,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); - failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); + currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); #ifdef USE_GPS gpsConfigMutable()->provider = sbufReadU8(src); // gps_type @@ -1915,7 +1914,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX); - failsafeConfigMutable()->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); + currentBatteryProfileMutable->failsafe_throttle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); #ifdef USE_GPS gpsConfigMutable()->provider = sbufReadU8(src); // gps_type @@ -2293,7 +2292,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src); navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src); - navConfigMutable()->mc.hover_throttle = sbufReadU16(src); + currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src); } else return MSP_RESULT_ERROR; break; @@ -2319,13 +2318,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_FW_CONFIG: if (dataSize >= 12) { - navConfigMutable()->fw.cruise_throttle = sbufReadU16(src); - navConfigMutable()->fw.min_throttle = sbufReadU16(src); - navConfigMutable()->fw.max_throttle = sbufReadU16(src); + currentBatteryProfileMutable->nav.fw.cruise_throttle = sbufReadU16(src); + currentBatteryProfileMutable->nav.fw.min_throttle = sbufReadU16(src); + currentBatteryProfileMutable->nav.fw.max_throttle = sbufReadU16(src); navConfigMutable()->fw.max_bank_angle = sbufReadU8(src); navConfigMutable()->fw.max_climb_angle = sbufReadU8(src); navConfigMutable()->fw.max_dive_angle = sbufReadU8(src); - navConfigMutable()->fw.pitch_to_throttle = sbufReadU8(src); + currentBatteryProfileMutable->nav.fw.pitch_to_throttle = sbufReadU8(src); navConfigMutable()->fw.loiter_radius = sbufReadU16(src); } else return MSP_RESULT_ERROR; @@ -2695,7 +2694,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize >= 20) { failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src); - failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src); + currentBatteryProfileMutable->failsafe_throttle = sbufReadU16(src); sbufReadU8(src); // was failsafe_kill_switch failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src); failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src); @@ -2847,7 +2846,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE case MSP_WP_MISSION_LOAD: sbufReadU8Safe(NULL, src); // Mission ID (reserved) - if ((dataSize != 1) || (!loadNonVolatileWaypointList())) + if ((dataSize != 1) || (!loadNonVolatileWaypointList(false))) return MSP_RESULT_ERROR; break; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 5ec8491e52..9b0a90e54b 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -326,7 +326,7 @@ void initActiveBoxIds(void) #endif } -#define IS_ENABLED(mask) (mask == 0 ? 0 : 1) +#define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1) #define CHECK_ACTIVE_BOX(condition, index) do { if (IS_ENABLED(condition)) { activeBoxes[index] = 1; } } while(0) void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index c5c67436a1..610e62f16e 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -49,6 +49,7 @@ #include "io/beeper.h" #include "io/vtx.h" +#include "sensors/battery.h" #include "sensors/boardalignment.h" #include "rx/rx.h" @@ -500,10 +501,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: - applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX); + applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, ¤tBatteryProfileMutable->nav.fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX); break; case ADJUSTMENT_NAV_FW_PITCH2THR: - applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX); + applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, ¤tBatteryProfileMutable->nav.fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX); break; case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: updateBoardAlignment(delta, 0); @@ -578,7 +579,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t navigationUsePIDs(); break; case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: - applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); + applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, ¤tBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); break; #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) case ADJUSTMENT_VTX_POWER_LEVEL: @@ -722,3 +723,14 @@ bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction) { } return false; } + +uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions) { + uint8_t adjustmentCount = 0; + for (uint8_t i = 0; i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; i++) { + if (adjustmentStates[i].config) { + adjustmentCount++; + adjustmentFunctions[i] = adjustmentStates[i].config->adjustmentFunction; + } + } + return adjustmentCount; +} diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 8736978825..93dd3d7e61 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -144,3 +144,4 @@ void updateAdjustmentStates(bool canUseRxData); struct controlRateConfig_s; void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData); bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction); +uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 563d181fb6..b5d4648838 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -71,12 +71,13 @@ stickPositions_e rcStickPositions; FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 3); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband = SETTING_DEADBAND_DEFAULT, .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT, .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT, + .control_deadband = SETTING_CONTROL_DEADBAND_DEFAULT, .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT, .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT, .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT, @@ -97,9 +98,14 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -bool areSticksDeflectedMoreThanPosHoldDeadband(void) +bool areSticksDeflected(void) { - return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband); + return (ABS(rcCommand[ROLL]) > rcControlsConfig()->control_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->control_deadband) || (ABS(rcCommand[YAW]) > rcControlsConfig()->control_deadband); +} + +bool isRollPitchStickDeflected(void) +{ + return (ABS(rcCommand[ROLL]) > rcControlsConfig()->control_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->control_deadband); } throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type) @@ -254,9 +260,14 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // Load waypoint list if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) { - const bool success = loadNonVolatileWaypointList(); + const bool success = loadNonVolatileWaypointList(false); beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); } + + if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_HI) { + resetWaypointList(); + beeper(BEEPER_ACTION_FAIL); // The above cannot fail, but traditionally, we play FAIL for not-loading + } #endif // Multiple configuration profiles diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index f5258a9c9e..dd617f29e7 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -84,7 +84,8 @@ extern int16_t rcCommand[4]; typedef struct rcControlsConfig_s { uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. - uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode) + uint8_t pos_hold_deadband; // Deadband for position hold + uint8_t control_deadband; // General deadband to check if sticks are deflected, us PWM. uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered @@ -106,7 +107,8 @@ stickPositions_e getRcStickPositions(void); bool checkStickPosition(stickPositions_e stickPos); bool areSticksInApModePosition(uint16_t ap_mode); -bool areSticksDeflectedMoreThanPosHoldDeadband(void); +bool areSticksDeflected(void); +bool isRollPitchStickDeflected(void); throttleStatus_e calculateThrottleStatus(throttleStatusType_e type); rollPitchStatus_e calculateRollPitchCenterStatus(void); void processRcStickPositions(throttleStatus_e throttleStatus); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index e73f7d65c1..b14289ba27 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -67,9 +67,9 @@ typedef struct { } __attribute__((packed)) setting_t; -static inline setting_type_e SETTING_TYPE(const setting_t *s) { return s->type & SETTING_TYPE_MASK; } -static inline setting_section_e SETTING_SECTION(const setting_t *s) { return s->type & SETTING_SECTION_MASK; } -static inline setting_mode_e SETTING_MODE(const setting_t *s) { return s->type & SETTING_MODE_MASK; } +static inline setting_type_e SETTING_TYPE(const setting_t *s) { return (setting_type_e)(s->type & SETTING_TYPE_MASK); } +static inline setting_section_e SETTING_SECTION(const setting_t *s) { return (setting_section_e)(s->type & SETTING_SECTION_MASK); } +static inline setting_mode_e SETTING_MODE(const setting_t *s) { return (setting_mode_e)(s->type & SETTING_MODE_MASK); } void settingGetName(const setting_t *val, char *buf); bool settingNameContains(const setting_t *val, char *buf, const char *cmdline); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9345c16a05..dc1fe0088b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -64,7 +64,7 @@ tables: - name: nav_rth_alt_mode values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"] - name: osd_unit - values: ["IMPERIAL", "METRIC", "UK"] + values: ["IMPERIAL", "METRIC", "METRIC_MPH", "UK"] enum: osd_unit_e - name: osd_stats_energy_unit values: ["MAH", "WH"] @@ -179,17 +179,25 @@ tables: - name: nav_rth_climb_first enum: navRTHClimbFirst_e values: ["OFF", "ON", "ON_FW_SPIRAL"] + - name: djiRssiSource + values: ["RSSI", "CRSF_LQ"] + enum: djiRssiSource_e + constants: RPYL_PID_MIN: 0 - RPYL_PID_MAX: 200 + RPYL_PID_MAX: 255 MANUAL_RATE_MIN: 0 MANUAL_RATE_MAX: 100 - ROLL_PITCH_RATE_MIN: 6 + ROLL_PITCH_RATE_MIN: 4 ROLL_PITCH_RATE_MAX: 180 + MAX_CONTROL_RATE_PROFILE_COUNT: 3 + MAX_BATTERY_PROFILE_COUNT: 3 + + groups: - name: PG_GYRO_CONFIG type: gyroConfig_t @@ -214,7 +222,7 @@ groups: description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz" default_value: 250 field: gyro_anti_aliasing_lpf_hz - max: 255 + max: 1000 - name: gyro_anti_aliasing_lpf_type description: "Specifies the type of the software LPF of the gyro signals." default_value: "PT1" @@ -320,6 +328,19 @@ groups: condition: USE_ALPHA_BETA_GAMMA_FILTER min: 0 max: 10 + - name: setpoint_kalman_enabled + description: "Enable Kalman filter on the gyro data" + default_value: OFF + condition: USE_GYRO_KALMAN + field: kalmanEnabled + type: bool + - name: setpoint_kalman_q + description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds" + default_value: 100 + field: kalman_q + condition: USE_GYRO_KALMAN + min: 1 + max: 16000 - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t @@ -857,31 +878,12 @@ groups: default_value: "ONESHOT125" field: motorPwmProtocol table: motor_pwm_protocol - - name: throttle_scale - description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%" - default_value: 1.0 - field: throttleScale - min: 0 - max: 1 - - name: throttle_idle - description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle." - default_value: 15 - field: throttleIdle - min: 0 - max: 30 - name: motor_poles field: motorPoleCount description: "The number of motor poles. Required to compute motor RPM" min: 4 max: 255 default_value: 14 - - name: turtle_mode_power_factor - field: turtleModePowerFactor - default_value: 55 - description: "Turtle mode power factor" - condition: "USE_DSHOT" - min: 0 - max: 100 - name: PG_FAILSAFE_CONFIG type: failsafeConfig_t @@ -902,11 +904,6 @@ groups: default_value: 200 min: 0 max: 200 - - name: failsafe_throttle - description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)." - default_value: 1000 - min: PWM_RANGE_MIN - max: PWM_RANGE_MAX - name: failsafe_throttle_low_delay description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout" default_value: 0 @@ -999,7 +996,7 @@ groups: - name: PG_BATTERY_METERS_CONFIG type: batteryMetersConfig_t - headers: ["sensors/battery.h"] + headers: ["sensors/battery_config_structs.h"] members: - name: vbat_meter_type description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running" @@ -1013,8 +1010,8 @@ groups: default_value: :target field: voltage.scale condition: USE_ADC - min: VBAT_SCALE_MIN - max: VBAT_SCALE_MAX + min: 0 + max: 65535 - name: current_meter_scale description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt." default_value: :target @@ -1065,7 +1062,7 @@ groups: - name: PG_BATTERY_PROFILES type: batteryProfile_t - headers: ["sensors/battery.h"] + headers: ["sensors/battery_config_structs.h"] value_type: BATTERY_CONFIG_VALUE members: - name: bat_cells @@ -1127,6 +1124,133 @@ groups: field: capacity.unit table: bat_capacity_unit type: uint8_t + - name: controlrate_profile + description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile" + default_value: 0 + field: controlRateProfile + min: 0 + max: MAX_CONTROL_RATE_PROFILE_COUNT + + - name: throttle_scale + description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%" + default_value: 1.0 + field: motor.throttleScale + min: 0 + max: 1 + - name: throttle_idle + description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle." + default_value: 15 + field: motor.throttleIdle + min: 0 + max: 30 + - name: turtle_mode_power_factor + field: motor.turtleModePowerFactor + default_value: 55 + description: "Turtle mode power factor" + condition: USE_DSHOT + min: 0 + max: 100 + - name: failsafe_throttle + description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)." + default_value: 1000 + min: PWM_RANGE_MIN + max: PWM_RANGE_MAX + - name: fw_min_throttle_down_pitch + description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)" + default_value: 0 + field: fwMinThrottleDownPitchAngle + min: 0 + max: 450 + - name: nav_mc_hover_thr + description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." + default_value: 1500 + field: nav.mc.hover_throttle + min: 1000 + max: 2000 + - name: nav_fw_cruise_thr + description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )" + default_value: 1400 + field: nav.fw.cruise_throttle + min: 1000 + max: 2000 + - name: nav_fw_min_thr + description: "Minimum throttle for flying wing in GPS assisted modes" + default_value: 1200 + field: nav.fw.min_throttle + min: 1000 + max: 2000 + - name: nav_fw_max_thr + description: "Maximum throttle for flying wing in GPS assisted modes" + default_value: 1700 + field: nav.fw.max_throttle + min: 1000 + max: 2000 + - name: nav_fw_pitch2thr + description: "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)" + default_value: 10 + field: nav.fw.pitch_to_throttle + min: 0 + max: 100 + - name: nav_fw_launch_thr + description: "Launch throttle - throttle to be set during launch sequence (pwm units)" + default_value: 1700 + field: nav.fw.launch_throttle + min: 1000 + max: 2000 + - name: nav_fw_launch_idle_thr + description: "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)" + default_value: 1000 + field: nav.fw.launch_idle_throttle + min: 1000 + max: 2000 + - name: limit_cont_current + description: "Continous current limit (dA), set to 0 to disable" + condition: USE_POWER_LIMITS + default_value: 0 + field: powerLimits.continuousCurrent + max: 4000 + - name: limit_burst_current + description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable" + condition: USE_POWER_LIMITS + default_value: 0 + field: powerLimits.burstCurrent + max: 4000 + - name: limit_burst_current_time + description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced" + condition: USE_POWER_LIMITS + default_value: 0 + field: powerLimits.burstCurrentTime + max: 3000 + - name: limit_burst_current_falldown_time + description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`" + condition: USE_POWER_LIMITS + default_value: 0 + field: powerLimits.burstCurrentFalldownTime + max: 3000 + - name: limit_cont_power + description: "Continous power limit (dW), set to 0 to disable" + condition: USE_POWER_LIMITS && USE_ADC + default_value: 0 + field: powerLimits.continuousPower + max: 40000 + - name: limit_burst_power + description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable" + condition: USE_POWER_LIMITS && USE_ADC + default_value: 0 + field: powerLimits.burstPower + max: 40000 + - name: limit_burst_power_time + description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced" + condition: USE_POWER_LIMITS && USE_ADC + default_value: 0 + field: powerLimits.burstPowerTime + max: 3000 + - name: limit_burst_power_falldown_time + description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`" + condition: USE_POWER_LIMITS && USE_ADC + default_value: 0 + field: powerLimits.burstPowerFalldownTime + max: 3000 - name: PG_MIXER_CONFIG type: mixerConfig_t @@ -1153,12 +1277,6 @@ groups: field: appliedMixerPreset min: -1 max: INT16_MAX - - name: fw_min_throttle_down_pitch - description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)" - default_value: 0 - field: fwMinThrottleDownPitchAngle - min: 0 - max: 450 - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t @@ -1226,7 +1344,7 @@ groups: - name: PG_CONTROL_RATE_PROFILES type: controlRateConfig_t - headers: ["fc/controlrate_profile.h"] + headers: ["fc/controlrate_profile_config_struct.h"] value_type: CONTROL_RATE_VALUE members: - name: thr_mid @@ -1289,7 +1407,7 @@ groups: description: "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." default_value: 20 field: stabilized.rates[FD_YAW] - min: 2 + min: 1 max: 180 - name: manual_rc_expo description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" @@ -1505,7 +1623,12 @@ groups: min: 0 max: 100 - name: pos_hold_deadband - description: "Stick deadband in [r/c points], applied after r/c deadband and expo" + description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes." + default_value: 10 + min: 2 + max: 250 + - name: control_deadband + description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered." default_value: 10 min: 2 max: 250 @@ -2058,33 +2181,6 @@ groups: field: controlDerivativeLpfHz min: 0 max: 200 - - name: setpoint_kalman_enabled - description: "Enable Kalman filter on the PID controller setpoint" - default_value: OFF - condition: USE_GYRO_KALMAN - field: kalmanEnabled - type: bool - - name: setpoint_kalman_q - description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds" - default_value: 100 - field: kalman_q - condition: USE_GYRO_KALMAN - min: 1 - max: 16000 - - name: setpoint_kalman_w - description: "Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response" - default_value: 4 - field: kalman_w - condition: USE_GYRO_KALMAN - min: 1 - max: 40 - - name: setpoint_kalman_sharpness - description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets" - default_value: 100 - field: kalman_sharpness - condition: USE_GYRO_KALMAN - min: 1 - max: 16000 - name: fw_level_pitch_trim description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" default_value: 0 @@ -2118,19 +2214,13 @@ groups: field: fixedWingLevelTrimGain min: 0 max: 20 - - name: fw_level_pitch_deadband - description: "Deadband for automatic leveling when AUTOLEVEL mode is used." - default_value: 5 - field: fixedWingLevelTrimDeadband - min: 0 - max: 20 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t condition: USE_AUTOTUNE_FIXED_WING members: - name: fw_autotune_min_stick - description: "Minimum stick input [%] to consider overshoot/undershoot detection" + description: "Minimum stick input [%], after applying deadband and expo, to start recording the plane's response to stick input." default_value: 50 field: fw_min_stick min: 0 @@ -2483,12 +2573,6 @@ groups: field: mc.max_bank_angle min: 15 max: 45 - - name: nav_mc_hover_thr - description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." - default_value: 1500 - field: mc.hover_throttle - min: 1000 - max: 2000 - name: nav_mc_auto_disarm_delay description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms)" default_value: 2000 @@ -2570,24 +2654,6 @@ groups: default_value: ON field: mc.slowDownForTurning type: bool - - name: nav_fw_cruise_thr - description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )" - default_value: 1400 - field: fw.cruise_throttle - min: 1000 - max: 2000 - - name: nav_fw_min_thr - description: "Minimum throttle for flying wing in GPS assisted modes" - default_value: 1200 - field: fw.min_throttle - min: 1000 - max: 2000 - - name: nav_fw_max_thr - description: "Maximum throttle for flying wing in GPS assisted modes" - default_value: 1700 - field: fw.max_throttle - min: 1000 - max: 2000 - name: nav_fw_auto_disarm_delay description: "Delay before plane disarms when `nav_disarm_on_landing` is set (ms)" default_value: 2000 @@ -2612,12 +2678,6 @@ groups: field: fw.max_dive_angle min: 5 max: 80 - - name: nav_fw_pitch2thr - description: "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)" - default_value: 10 - field: fw.pitch_to_throttle - min: 0 - max: 100 - name: nav_fw_pitch2thr_smoothing description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh." default_value: 6 @@ -2681,18 +2741,12 @@ groups: field: fw.launch_time_thresh min: 10 max: 1000 - - name: nav_fw_launch_thr - description: "Launch throttle - throttle to be set during launch sequence (pwm units)" - default_value: 1700 - field: fw.launch_throttle - min: 1000 - max: 2000 - - name: nav_fw_launch_idle_thr - description: "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)" - default_value: 1000 - 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: 60000 - name: nav_fw_launch_motor_delay description: "Delay between detected launch and launch sequence start and throttling up (ms)" default_value: 500 @@ -3132,6 +3186,13 @@ groups: field: link_quality_alarm min: 0 max: 100 + - name: osd_rssi_dbm_alarm + condition: USE_SERIALRX_CRSF + description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm" + default_value: 0 + field: rssi_dbm_alarm + min: -130 + max: 0 - name: osd_temp_label_align description: "Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT`" default_value: "LEFT" @@ -3170,11 +3231,16 @@ groups: min: -2 max: 2 - name: osd_camera_uptilt - description: "Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal" + description: "Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal. Used for correct display of HUD items and AHI (when enabled)." default_value: 0 field: camera_uptilt min: -40 max: 80 + - name: osd_ahi_camera_uptilt_comp + description: "When set to `ON`, the AHI position is adjusted by `osd_camera_uptilt`. For example, with a cammera uptilt of 30 degrees, the AHI will appear in the middle of the OSD when the aircraft is pitched forward 30 degrees. When set to `OFF`, the AHI will appear in the center of the OSD regardless of camera angle, but can still be shifted up and down using `osd_horizon_offset` (`osd_ahi_vertical_offset` for pixel-OSD)." + default_value: OFF + field: ahi_camera_uptilt_comp + type: bool - name: osd_camera_fov_h description: "Horizontal field of view for the camera in degres" default_value: 135 @@ -3623,7 +3689,7 @@ groups: max: UINT8_MAX default_value: 1 - 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" + description: "Re-purpose the craft name field for messages." default_value: ON field: use_name_for_messages type: bool @@ -3633,6 +3699,30 @@ groups: field: esc_temperature_source table: djiOsdTempSource type: uint8_t + - name: dji_message_speed_source + description: "Sets the speed type displayed by the DJI OSD in craft name: GROUND, 3D, AIR" + default_value: "3D" + field: messageSpeedSource + table: osdSpeedSource + type: uint8_t + - name: dji_rssi_source + description: "Source of the DJI RSSI field: RSSI, CRSF_LQ" + default_value: "RSSI" + field: rssi_source + table: djiRssiSource + type: uint8_t + - name: dji_use_adjustments + description: "Show inflight adjustments in craft name field" + default_value: OFF + type: bool + field: useAdjustments + - name: dji_cn_alternating_duration + description: "Alternating duration of craft name elements, in tenths of a second" + default_value: 30 + min: 1 + max: 150 + field: craftNameAlternatingDuration + type: uint8_t - name: PG_BEEPER_CONFIG type: beeperConfig_t @@ -3650,56 +3740,17 @@ groups: default_value: 1 field: dshot_beeper_tone type: uint8_t + - name: beeper_pwm_mode + field: pwmMode + type: bool + default_value: OFF + description: "Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if the external beeper sound is weak. Do not switch from OFF to ON without checking if the board supports PWM beeper mode" - name: PG_POWER_LIMITS_CONFIG type: powerLimitsConfig_t headers: ["flight/power_limits.h"] condition: USE_POWER_LIMITS members: - - name: limit_cont_current - description: "Continous current limit (dA), set to 0 to disable" - default_value: 0 - field: continuousCurrent - max: 4000 - - name: limit_burst_current - description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable" - default_value: 0 - field: burstCurrent - max: 4000 - - name: limit_burst_current_time - description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced" - default_value: 0 - field: burstCurrentTime - max: 3000 - - name: limit_burst_current_falldown_time - description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`" - default_value: 0 - field: burstCurrentFalldownTime - max: 3000 - - name: limit_cont_power - condition: USE_ADC - description: "Continous power limit (dW), set to 0 to disable" - default_value: 0 - field: continuousPower - max: 40000 - - name: limit_burst_power - condition: USE_ADC - description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable" - default_value: 0 - field: burstPower - max: 40000 - - name: limit_burst_power_time - condition: USE_ADC - description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced" - default_value: 0 - field: burstPowerTime - max: 3000 - - name: limit_burst_power_falldown_time - condition: USE_ADC - description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`" - default_value: 0 - field: burstPowerFalldownTime - max: 3000 - name: limit_pi_p description: "Throttle attenuation PI control P term" default_value: 100 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 90173dc473..fa1d31c34f 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -51,6 +51,7 @@ #include "rx/rx.h" +#include "sensors/battery.h" #include "sensors/sensors.h" /* @@ -66,13 +67,12 @@ static failsafeState_t failsafeState; -PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2); PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay) .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec - .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off. .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left @@ -218,7 +218,7 @@ bool failsafeRequiresMotorStop(void) { return failsafeState.active && failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && - failsafeConfig()->failsafe_throttle < getThrottleIdleValue(); + currentBatteryProfile->failsafe_throttle < getThrottleIdleValue(); } void failsafeStartMonitoring(void) @@ -264,13 +264,13 @@ void failsafeApplyControlInput(void) autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); autoRcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); - autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; + autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; } else { for (int i = 0; i < 3; i++) { autoRcCommand[i] = 0; } - autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; + autoRcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; } // Apply channel values diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index cafc01b007..e2ab280ac1 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -30,7 +30,6 @@ typedef struct failsafeConfig_s { - uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND) uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3401e3b00c..2b2f348638 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -241,7 +241,7 @@ static float imuGetPGainScaleFactor(void) static void imuResetOrientationQuaternion(const fpVector3_t * accBF) { - const float accNorm = sqrtf(vectorNormSquared(accBF)); + const float accNorm = fast_fsqrtf(vectorNormSquared(accBF)); orientation.q0 = accBF->z + accNorm; orientation.q1 = accBF->y; @@ -436,12 +436,12 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero. // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision - // then we can safely use the "low angle" approximated version without loss of accuracy. - if (thetaMagnitudeSq < sqrtf(24.0f * 1e-6f)) { + if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) { quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f); deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f; } else { - const float thetaMagnitude = sqrtf(thetaMagnitudeSq); + const float thetaMagnitude = fast_fsqrtf(thetaMagnitudeSq); quaternionScale(&deltaQ, &deltaQ, sin_approx(thetaMagnitude) / thetaMagnitude); deltaQ.q0 = cos_approx(thetaMagnitude); } @@ -483,7 +483,7 @@ static float imuCalculateAccelerometerWeight(const float dT) accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis]; } - const float accWeight_Nearness = bellCurve(sqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS); + const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS); // Experiment: if rotation rate on a FIXED_WING_LEGACY is higher than a threshold - centrifugal force messes up too much and we // should not use measured accel for AHRS comp @@ -504,7 +504,7 @@ static float imuCalculateAccelerometerWeight(const float dT) float accWeight_RateIgnore = 1.0f; if (ARMING_FLAG(ARMED) && STATE(FIXED_WING_LEGACY) && imuConfig()->acc_ignore_rate) { - const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); + const float rotRateMagnitude = fast_fsqrtf(sq(imuMeasuredRotationBF.y) + sq(imuMeasuredRotationBF.z)); const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, IMU_CENTRIFUGAL_LPF, dT); if (imuConfig()->acc_ignore_slope) { diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c index 99b0a00b2b..7ac431cb26 100755 --- a/src/main/flight/kalman.c +++ b/src/main/flight/kalman.c @@ -30,45 +30,35 @@ FILE_COMPILE_FOR_SPEED kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT]; -static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q, uint16_t w, uint16_t sharpness) +static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q) { memset(filter, 0, sizeof(kalman_t)); filter->q = q * 0.03f; //add multiplier to make tuning easier filter->r = 88.0f; //seeding R at 88.0f filter->p = 30.0f; //seeding P at 30.0f filter->e = 1.0f; - filter->s = sharpness / 10.0f; - filter->w = w * 8; + filter->w = MAX_KALMAN_WINDOW_SIZE; filter->inverseN = 1.0f / (float)(filter->w); } -void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness) +void gyroKalmanInitialize(uint16_t q) { - gyroKalmanInitAxis(&kalmanFilterStateRate[X], q, w, sharpness); - gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q, w, sharpness); - gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q, w, sharpness); + gyroKalmanInitAxis(&kalmanFilterStateRate[X], q); + gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q); + gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q); } -float kalman_process(kalman_t *kalmanState, float input, float target) +float kalman_process(kalman_t *kalmanState, float input) { - float targetAbs = fabsf(target); //project the state ahead using acceleration kalmanState->x += (kalmanState->x - kalmanState->lastX); - //figure out how much to boost or reduce our error in the estimate based on setpoint target. - //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint. //update last state kalmanState->lastX = kalmanState->x; if (kalmanState->lastX != 0.0f) { - // calculate the error and add multiply sharpness boost - float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s; - - // give a boost to the setpoint, used to caluclate the kalman q, based on the error and setpoint/gyrodata - errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f); - - kalmanState->e = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX))); + kalmanState->e = fabsf(1.0f - (kalmanState->setpoint / kalmanState->lastX)); } //prediction update @@ -92,7 +82,7 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate) kalmanState->varianceWindow[kalmanState->windex] = varianceElement; kalmanState->windex++; - if (kalmanState->windex >= kalmanState->w) { + if (kalmanState->windex > kalmanState->w) { kalmanState->windex = 0; } @@ -108,13 +98,17 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate) kalmanState->r = squirt * VARIANCE_SCALE; } -float gyroKalmanUpdate(uint8_t axis, float input, float setpoint) +float NOINLINE gyroKalmanUpdate(uint8_t axis, float input) { updateAxisVariance(&kalmanFilterStateRate[axis], input); DEBUG_SET(DEBUG_KALMAN_GAIN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain - return kalman_process(&kalmanFilterStateRate[axis], input, setpoint); + return kalman_process(&kalmanFilterStateRate[axis], input); +} + +void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint) { + kalmanFilterStateRate[axis].setpoint = setpoint; } #endif \ No newline at end of file diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h index 17b714b8ff..620473be5e 100755 --- a/src/main/flight/kalman.h +++ b/src/main/flight/kalman.h @@ -23,7 +23,7 @@ #include "sensors/gyro.h" #include "common/filter.h" -#define MAX_KALMAN_WINDOW_SIZE 512 +#define MAX_KALMAN_WINDOW_SIZE 64 #define VARIANCE_SCALE 0.67f @@ -36,12 +36,13 @@ typedef struct kalman float x; //state float lastX; //previous state float e; - float s; + float setpoint; + float axisVar; uint16_t windex; - float axisWindow[MAX_KALMAN_WINDOW_SIZE]; - float varianceWindow[MAX_KALMAN_WINDOW_SIZE]; + float axisWindow[MAX_KALMAN_WINDOW_SIZE + 1]; + float varianceWindow[MAX_KALMAN_WINDOW_SIZE + 1]; float axisSumMean; float axisMean; float axisSumVar; @@ -49,5 +50,6 @@ typedef struct kalman uint16_t w; } kalman_t; -void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness); -float gyroKalmanUpdate(uint8_t axis, float input, float setpoint); +void gyroKalmanInitialize(uint16_t q); +float gyroKalmanUpdate(uint8_t axis, float input); +void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint); \ No newline at end of file diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2ee79621f0..b92c999e3b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -83,14 +83,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 4); PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, .platformType = SETTING_PLATFORM_TYPE_DEFAULT, .hasFlaps = SETTING_HAS_FLAPS_DEFAULT, .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only - .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT ); #ifdef BRUSHED_MOTORS @@ -103,7 +102,7 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, #define DEFAULT_MAX_THROTTLE 1850 -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 8); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, @@ -112,12 +111,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT, .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT, - .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT, - .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles -#ifdef USE_DSHOT - .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT, -#endif ); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); @@ -130,7 +124,7 @@ static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn; int getThrottleIdleValue(void) { if (!throttleIdleValue) { - throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle); + throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle); } return throttleIdleValue; @@ -330,7 +324,7 @@ static uint16_t handleOutputScaling( static void applyTurtleModeToMotors(void) { if (ARMING_FLAG(ARMED)) { - const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f; + const float flipPowerFactor = ((float)currentBatteryProfile->motor.turtleModePowerFactor)/100.0f; const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f); const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f); const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f); @@ -347,8 +341,8 @@ static void applyTurtleModeToMotors(void) { float signRoll = rcCommand[ROLL] < 0 ? 1 : -1; float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1)); - float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); - float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); + float stickDeflectionLength = fast_fsqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs)); + float stickDeflectionExpoLength = fast_fsqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo)); if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) { // If yaw is the dominant, disable pitch and roll @@ -362,8 +356,8 @@ static void applyTurtleModeToMotors(void) { } const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / - (sqrtf(2.0f) * stickDeflectionLength) : 0; - const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f) + (fast_fsqrtf(2.0f) * stickDeflectionLength) : 0; + const float cosThreshold = fast_fsqrtf(3.0f) / 2.0f; // cos(PI/6.0f) if (cosPhi < cosThreshold) { // Enforce either roll or pitch exclusively, if not on diagonal @@ -602,9 +596,9 @@ void FAST_CODE mixTable(const float dT) // Throttle scaling to limit max throttle when battery is full #ifdef USE_PROGRAMMING_FRAMEWORK - mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin; + mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin; #else - mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin; + mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin; #endif // Throttle compensation based on battery voltage if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 195df58c30..e40842cb81 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -62,7 +62,6 @@ typedef struct mixerConfig_s { uint8_t platformType; bool hasFlaps; int16_t appliedMixerPreset; - uint16_t fwMinThrottleDownPitchAngle; } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig); @@ -84,10 +83,7 @@ typedef struct motorConfig_s { uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] uint16_t digitalIdleOffsetValue; - float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent - float throttleScale; // Scaling factor for throttle. uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry - uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6f6222d901..ac4b36cf45 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,6 +56,7 @@ FILE_COMPILE_FOR_SPEED #include "rx/rx.h" +#include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/gyro.h" #include "sensors/acceleration.h" @@ -109,7 +110,7 @@ typedef struct { bool itermLimitActive; bool itermFreezeActive; - biquadFilter_t rateTargetFilter; + pt2Filter_t rateTargetFilter; smithPredictor_t smithPredictor; } pidState_t; @@ -169,7 +170,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 4); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -299,16 +300,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT, .controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT, -#ifdef USE_GYRO_KALMAN - .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, - .kalman_w = SETTING_SETPOINT_KALMAN_W_DEFAULT, - .kalman_sharpness = SETTING_SETPOINT_KALMAN_SHARPNESS_DEFAULT, - .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT, -#endif - .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, - .fixedWingLevelTrimDeadband = SETTING_FW_LEVEL_PITCH_DEADBAND_DEFAULT, #ifdef USE_SMITH_PREDICTOR .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, @@ -363,30 +356,30 @@ bool pidInitFilters(void) if (pidProfile()->controlDerivativeLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&pidState[axis].rateTargetFilter, pidProfile()->controlDerivativeLpfHz, getLooptime()); + pt2FilterInit(&pidState[axis].rateTargetFilter, pt2FilterGain(pidProfile()->controlDerivativeLpfHz, refreshRate * 1e-6f)); } } #ifdef USE_SMITH_PREDICTOR smithPredictorInit( - &pidState[FD_ROLL].smithPredictor, - pidProfile()->smithPredictorDelay, - pidProfile()->smithPredictorStrength, - pidProfile()->smithPredictorFilterHz, + &pidState[FD_ROLL].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, getLooptime() ); smithPredictorInit( - &pidState[FD_PITCH].smithPredictor, - pidProfile()->smithPredictorDelay, - pidProfile()->smithPredictorStrength, - pidProfile()->smithPredictorFilterHz, + &pidState[FD_PITCH].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, getLooptime() ); smithPredictorInit( - &pidState[FD_YAW].smithPredictor, - pidProfile()->smithPredictorDelay, - pidProfile()->smithPredictorStrength, - pidProfile()->smithPredictorFilterHz, + &pidState[FD_YAW].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, getLooptime() ); #endif @@ -413,7 +406,7 @@ void pidResetErrorAccumulators(void) } } -void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) +void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) { pidState[axis].errorGyroIf -= delta; pidState[axis].errorGyroIfLimit -= delta; @@ -421,10 +414,10 @@ void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) float getTotalRateTarget(void) { - return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget)); + return fast_fsqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget)); } -float getAxisIterm(uint8_t axis) +float getAxisIterm(uint8_t axis) { return pidState[axis].errorGyroIf; } @@ -529,7 +522,7 @@ void updatePIDCoefficients() #ifdef USE_ANTIGRAVITY if (usedPidControllerType == PID_TYPE_PID) { antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]); - iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain); + iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain); } #endif @@ -607,7 +600,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) { - angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); + angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, currentBatteryProfile->fwMinThrottleDownPitchAngle); } //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming @@ -616,7 +609,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10); DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z)); - /* + /* * fixedWingLevelTrim has opposite sign to rcCommand. * Positive rcCommand means nose should point downwards * Negative rcCommand mean nose should point upwards @@ -625,24 +618,10 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h * Positive fixedWingLevelTrim means nose should point upwards * Negative fixedWingLevelTrim means nose should point downwards */ - angleTarget -= fixedWingLevelTrim; + angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10); } - //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming - if (axis == FD_PITCH && STATE(AIRPLANE)) { - /* - * fixedWingLevelTrim has opposite sign to rcCommand. - * Positive rcCommand means nose should point downwards - * Negative rcCommand mean nose should point upwards - * This is counter intuitive and a natural way suggests that + should mean UP - * This is why fixedWingLevelTrim has opposite sign to rcCommand - * Positive fixedWingLevelTrim means nose should point upwards - * Negative fixedWingLevelTrim means nose should point downwards - */ - angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); - } - #ifdef USE_SECONDARY_IMU float actual; if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) { @@ -717,23 +696,23 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) { #ifdef USE_D_BOOST static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { - + float dBoost = 1.0f; - + if (dBoostFactor > 1) { const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); - + const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); dBoost = constrainf(dBoost, 1.0f, dBoostFactor); - } + } return dBoost; } -#else +#else static float applyDBoost(pidState_t *pidState, float dT) { UNUSED(pidState); UNUSED(dT); @@ -762,7 +741,7 @@ static float dTermProcess(pidState_t *pidState, float dT) { static void applyItermLimiting(pidState_t *pidState) { if (pidState->itermLimitActive) { pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); - } else + } else { pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); } @@ -795,7 +774,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); #ifdef USE_AUTOTUNE_FIXED_WING if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { @@ -821,7 +800,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); return itermErrorRate * itermRelaxFactor; } @@ -837,18 +816,12 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid const float newDTerm = dTermProcess(pidState, dT); const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; - const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); + const float rateTargetDeltaFiltered = pt2FilterApply(&pidState->rateTargetFilter, rateTargetDelta); /* * Compute Control Derivative - * CD is enabled only when ANGLE and HORIZON modes are not enabled! */ - float newCDTerm; - if (levelingEnabled) { - newCDTerm = 0.0F; - } else { - newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT); - } + const float newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT); DEBUG_SET(DEBUG_CD, axis, newCDTerm); // TODO: Get feedback from mixer on available correction range for each axis @@ -1066,12 +1039,12 @@ void checkItermLimitingActive(pidState_t *pidState) bool shouldActivate; if (usedPidControllerType == PID_TYPE_PIFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); - } else + } else { shouldActivate = mixerIsOutputSaturated(); } - pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; + pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; } void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) @@ -1089,7 +1062,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis { pidState->itermFreezeActive = false; } - + } void FAST_CODE pidController(float dT) @@ -1117,7 +1090,7 @@ void FAST_CODE pidController(float dT) } else { #ifdef USE_PROGRAMMING_FRAMEWORK rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]); -#else +#else rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]); #endif } @@ -1126,9 +1099,7 @@ void FAST_CODE pidController(float dT) pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); #ifdef USE_GYRO_KALMAN - if (pidProfile()->kalmanEnabled) { - pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); - } + gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget); #endif #ifdef USE_SMITH_PREDICTOR @@ -1168,7 +1139,7 @@ void FAST_CODE pidController(float dT) for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits pidApplySetpointRateLimiting(&pidState[axis], axis, dT); - + // Step 4: Run gyro-driven control checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); @@ -1180,7 +1151,7 @@ void FAST_CODE pidController(float dT) pidType_e pidIndexGetType(pidIndex_e pidIndex) { if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) { - return usedPidControllerType; + return usedPidControllerType; } if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) { @@ -1216,7 +1187,7 @@ void pidInit(void) antigravityGain = pidProfile()->antigravityGain; antigravityAccelerator = pidProfile()->antigravityAccelerator; #endif - + for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { if (axis == FD_YAW) { pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw; @@ -1233,7 +1204,7 @@ void pidInit(void) if (pidProfile()->pidControllerType == PID_TYPE_AUTO) { if ( - mixerConfig()->platformType == PLATFORM_AIRPLANE || + mixerConfig()->platformType == PLATFORM_AIRPLANE || mixerConfig()->platformType == PLATFORM_BOAT || mixerConfig()->platformType == PLATFORM_ROVER ) { @@ -1272,11 +1243,6 @@ void pidInit(void) } pidResetTPAFilter(); -#ifdef USE_GYRO_KALMAN - if (pidProfile()->kalmanEnabled) { - gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness); - } -#endif fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim; @@ -1291,10 +1257,10 @@ void pidInit(void) } -const pidBank_t * pidBank(void) { - return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; +const pidBank_t * pidBank(void) { + return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } -pidBank_t * pidBankMutable(void) { +pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } @@ -1313,25 +1279,24 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) */ if (ARMING_FLAG(ARMED) && !previousArmingState) { navPidReset(&fixedWingLevelTrimController); - } + } /* * On disarm update the default value */ if (!ARMING_FLAG(ARMED) && previousArmingState) { pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE); - } + } /* * Prepare flags for the PID controller */ pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR; - //Iterm should freeze when pitch stick is deflected + //Iterm should freeze when sticks are deflected if ( !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || - rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) || - rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) || + areSticksDeflected() || (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) || navigationIsControllingAltitude() ) { @@ -1355,3 +1320,8 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) previousArmingState = !!ARMING_FLAG(ARMED); } + +float getFixedWingLevelTrim(void) +{ + return STATE(AIRPLANE) ? fixedWingLevelTrim : 0; +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 0d4719acb8..7e1ab17a76 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -156,16 +156,8 @@ typedef struct pidProfile_s { uint16_t navFwPosHdgPidsumLimit; uint8_t controlDerivativeLpfHz; -#ifdef USE_GYRO_KALMAN - uint16_t kalman_q; - uint16_t kalman_w; - uint16_t kalman_sharpness; - uint8_t kalmanEnabled; -#endif - float fixedWingLevelTrim; float fixedWingLevelTrimGain; - float fixedWingLevelTrimDeadband; #ifdef USE_SMITH_PREDICTOR float smithPredictorStrength; float smithPredictorDelay; @@ -233,3 +225,4 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa pidType_e pidIndexGetType(pidIndex_e pidIndex); void updateFixedWingLevelTrim(timeUs_t currentTimeUs); +float getFixedWingLevelTrim(void); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 9679e736e2..151d23be05 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -47,14 +47,14 @@ #include "flight/pid.h" #define AUTOTUNE_FIXED_WING_MIN_FF 10 -#define AUTOTUNE_FIXED_WING_MAX_FF 200 +#define AUTOTUNE_FIXED_WING_MAX_FF 255 #define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40 #define AUTOTUNE_FIXED_WING_MIN_YAW_RATE 10 #define AUTOTUNE_FIXED_WING_MAX_RATE 720 #define AUTOTUNE_FIXED_WING_CONVERGENCE_RATE 10 #define AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL 20 // ms -#define AUTOTUNE_FIXED_WING_SAMPLES 1000 // Use averagea over the last 20 seconds -#define AUTOTUNE_FIXED_WING_MIN_SAMPLES 250 // Start updating tune after 5 seconds +#define AUTOTUNE_FIXED_WING_SAMPLES 1000 // Use average over the last 20 seconds of hard maneuvers +#define AUTOTUNE_FIXED_WING_MIN_SAMPLES 250 // Start updating tune after 5 seconds of hard maneuvers PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2); @@ -248,13 +248,14 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa if (gainsUpdated) { // Set P-gain to 10% of FF gain (quite agressive - FIXME) tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f); + tuneCurrent[axis].gainP = constrainf(tuneCurrent[axis].gainP, 1.0f, 20.0f); // Set D-gain relative to P-gain (0 for now) tuneCurrent[axis].gainD = tuneCurrent[axis].gainP * (pidAutotuneConfig()->fw_p_to_d_gain / 100.0f); // Set integrator gain to reach the same response as FF gain in 0.667 second tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER; - tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f); + tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 30.0f); autotuneUpdateGains(tuneCurrent); switch (axis) { diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c index 1ce770aca4..549ead0019 100644 --- a/src/main/flight/power_limits.c +++ b/src/main/flight/power_limits.c @@ -45,22 +45,12 @@ FILE_COMPILE_FOR_SPEED #define LIMITING_THR_FILTER_TCONST 50 -PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 1); PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, - .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA - .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA - .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS - .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS -#ifdef USE_ADC - .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW - .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW - .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS - .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS -#endif .piP = SETTING_LIMIT_PI_P_DEFAULT, .piI = SETTING_LIMIT_PI_I_DEFAULT, - .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz + .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz ); static float burstCurrentReserve; // cA.µs @@ -84,29 +74,29 @@ static bool wasLimitingPower = false; #endif void powerLimiterInit(void) { - if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) { - powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent; + if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) { + currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent; } - activeCurrentLimit = powerLimitsConfig()->burstCurrent; + activeCurrentLimit = currentBatteryProfile->powerLimits.burstCurrent; - uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; - burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6; - burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6; + uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent; + burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6; + burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6; pt1FilterInit(¤tThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); pt1FilterInitRC(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); #ifdef USE_ADC - if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) { - powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower; + if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) { + currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower; } - activePowerLimit = powerLimitsConfig()->burstPower; + activePowerLimit = currentBatteryProfile->powerLimits.burstPower; - uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; - burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6; - burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6; + uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower; + burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6; + burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6; pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); @@ -118,7 +108,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui float spentReserveChunk = continuousDiff * timeDelta; *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax); - if (powerLimitsConfig()->burstCurrentFalldownTime) { + if (currentBatteryProfile->powerLimits.burstCurrentFalldownTime) { return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10; } @@ -127,7 +117,7 @@ static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, ui void currentLimiterUpdate(timeDelta_t timeDelta) { activeCurrentLimit = calculateActiveLimit(getAmperage(), - powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent, + currentBatteryProfile->powerLimits.continuousCurrent, currentBatteryProfile->powerLimits.burstCurrent, &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax, timeDelta); } @@ -135,7 +125,7 @@ void currentLimiterUpdate(timeDelta_t timeDelta) { #ifdef USE_ADC void powerLimiterUpdate(timeDelta_t timeDelta) { activePowerLimit = calculateActiveLimit(getPower(), - powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower, + currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower, &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax, timeDelta); } @@ -192,7 +182,6 @@ void powerLimiterApply(int16_t *throttleCommand) { currentThrottleCommand = currentThrAttned; } else { wasLimitingCurrent = false; - currentThrAttnIntegrator = 0; pt1FilterReset(¤tThrAttnFilter, 0); currentThrottleCommand = *throttleCommand; @@ -222,7 +211,6 @@ void powerLimiterApply(int16_t *throttleCommand) { powerThrottleCommand = powerThrAttned; } else { wasLimitingPower = false; - powerThrAttnIntegrator = 0; pt1FilterReset(&powerThrAttnFilter, 0); powerThrottleCommand = *throttleCommand; @@ -256,18 +244,18 @@ bool powerLimiterIsLimitingPower(void) { // returns seconds float powerLimiterGetRemainingBurstTime(void) { - uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; + uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent; float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7; #ifdef USE_ADC - uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; + uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower; float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7; - if (!powerLimitsConfig()->continuousCurrent) { + if (!currentBatteryProfile->powerLimits.continuousCurrent) { return remainingPowerBurstTime; } - if (!powerLimitsConfig()->continuousPower) { + if (!currentBatteryProfile->powerLimits.continuousPower) { return remainingCurrentBurstTime; } diff --git a/src/main/flight/power_limits.h b/src/main/flight/power_limits.h index b82b938c58..2f4385252c 100644 --- a/src/main/flight/power_limits.h +++ b/src/main/flight/power_limits.h @@ -25,23 +25,13 @@ #include +#include "platform.h" + #include "config/parameter_group.h" #if defined(USE_POWER_LIMITS) typedef struct { - uint16_t continuousCurrent; // dA - uint16_t burstCurrent; // dA - uint16_t burstCurrentTime; // ds - uint16_t burstCurrentFalldownTime; // ds - -#ifdef USE_ADC - uint16_t continuousPower; // dW - uint16_t burstPower; // dW - uint16_t burstPowerTime; // ds - uint16_t burstPowerFalldownTime; // ds -#endif - uint16_t piP; uint16_t piI; diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 7bae4fa731..747f326753 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -77,9 +77,9 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) { // pitch in degrees // output in Watt static float estimatePitchPower(float pitch) { - int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle; - altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); - const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue()); + int16_t altitudeChangeThrottle = (int16_t)pitch * currentBatteryProfile->nav.fw.pitch_to_throttle; + altitudeChangeThrottle = constrain(altitudeChangeThrottle, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle); + const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (currentBatteryProfile->nav.fw.cruise_throttle - getThrottleIdleValue()); return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; } diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index c3a242497c..6fb74e319e 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -490,6 +490,7 @@ void processServoAutotrimMode(void) #define SERVO_AUTOTRIM_CENTER_MIN 1300 #define SERVO_AUTOTRIM_CENTER_MAX 1700 #define SERVO_AUTOTRIM_UPDATE_SIZE 5 +#define SERVO_AUTOTRIM_ATIITUDE_LIMIT 50 // 5 degrees void processContinuousServoAutotrim(const float dT) { @@ -497,21 +498,22 @@ void processContinuousServoAutotrim(const float dT) static servoAutotrimState_e trimState = AUTOTRIM_IDLE; static uint32_t servoMiddleUpdateCount; - const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); - const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); - const float targetRateMagnitude = getTotalRateTarget(); - const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT); if (ARMING_FLAG(ARMED)) { trimState = AUTOTRIM_COLLECTING; if ((millis() - lastUpdateTimeMs) > 500) { const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit; - const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f; + const bool sticksAreCentered = !areSticksDeflected(); + const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT + && ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT; if ( planeIsFlyingStraight && noRotationCommanded && planeIsFlyingLevel && + sticksAreCentered && !FLIGHT_MODE(MANUAL_MODE) && isGPSHeadingValid() // TODO: proper flying detection ) { diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 0de1f18ad4..2561ff6b21 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -72,7 +72,7 @@ float getEstimatedHorizontalWindSpeed(uint16_t *angle) } *angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle); } - return sqrtf(sq(xWindSpeed) + sq(yWindSpeed)); + return fast_fsqrtf(sq(xWindSpeed) + sq(yWindSpeed)); } void updateWindEstimator(timeUs_t currentTimeUs) @@ -133,7 +133,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z]; // estimate airspeed it using equation 6 - float V = (sqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / sqrtf(diffLengthSq); + float V = (fast_fsqrtf(sq(groundVelocityDiff[0]) + sq(groundVelocityDiff[1]) + sq(groundVelocityDiff[2]))) / fast_fsqrtf(diffLengthSq); fuselageDirectionSum[X] = fuselageDirection[X] + lastFuselageDirection[X]; fuselageDirectionSum[Y] = fuselageDirection[Y] + lastFuselageDirection[Y]; @@ -155,8 +155,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11 wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12 - float prevWindLength = sqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z])); - float windLength = sqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z])); + float prevWindLength = fast_fsqrtf(sq(estimatedWind[X]) + sq(estimatedWind[Y]) + sq(estimatedWind[Z])); + float windLength = fast_fsqrtf(sq(wind[X]) + sq(wind[Y]) + sq(wind[Z])); if (windLength < prevWindLength + 2000) { // TODO: Better filtering diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index f19797afc4..57cba56a61 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -132,6 +132,10 @@ static const uint8_t beep_launchModeBeep[] = { static const uint8_t beep_launchModeLowThrottleBeep[] = { 5, 5, 5, 5, 3, 100, BEEPER_COMMAND_STOP }; +// 4 short beeps and a pause. Warning motor about to start at idle throttle +static const uint8_t beep_launchModeIdleStartBeep[] = { + 5, 5, 5, 5, 5, 5, 5, 80, BEEPER_COMMAND_STOP +}; // short beeps static const uint8_t beep_hardwareFailure[] = { 10, 10, BEEPER_COMMAND_STOP @@ -153,8 +157,8 @@ static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 2]; #define BEEPER_CONFIRMATION_BEEP_GAP_DURATION 20 -// Beeper off = 0 Beeper on = 1 -static uint8_t beeperIsOn = 0; +// Beeper off = false Beeper on = true +static bool beeperIsOn = false; // Place in current sequence static uint16_t beeperPos = 0; @@ -196,11 +200,12 @@ typedef struct beeperTableEntry_s { { BEEPER_ENTRY(BEEPER_USB, 18, NULL, "ON_USB") }, { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_ENABLED, 19, beep_launchModeBeep, "LAUNCH_MODE") }, { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_LOW_THROTTLE, 20, beep_launchModeLowThrottleBeep, "LAUNCH_MODE_LOW_THROTTLE") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 21, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, - { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 22, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, + { BEEPER_ENTRY(BEEPER_LAUNCH_MODE_IDLE_START, 21, beep_launchModeIdleStartBeep, "LAUNCH_MODE_IDLE_START") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 22, beep_camOpenBeep, "CAM_CONNECTION_OPEN") }, + { BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 23, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") }, - { BEEPER_ENTRY(BEEPER_ALL, 23, NULL, "ALL") }, - { BEEPER_ENTRY(BEEPER_PREFERENCE, 24, NULL, "PREFERED") }, + { BEEPER_ENTRY(BEEPER_ALL, 24, NULL, "ALL") }, + { BEEPER_ENTRY(BEEPER_PREFERENCE, 25, NULL, "PREFERED") }, }; static const beeperTableEntry_t *currentBeeperEntry = NULL; @@ -254,7 +259,7 @@ void beeperSilence(void) warningLedRefresh(); - beeperIsOn = 0; + beeperIsOn = false; beeperNextToggleTime = 0; beeperPos = 0; @@ -347,7 +352,7 @@ void beeperUpdate(timeUs_t currentTimeUs) } #endif - beeperIsOn = 1; + beeperIsOn = true; if (currentBeeperEntry->sequence[beeperPos] != 0) { if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) BEEP_ON; @@ -363,7 +368,7 @@ void beeperUpdate(timeUs_t currentTimeUs) } } } else { - beeperIsOn = 0; + beeperIsOn = false; if (currentBeeperEntry->sequence[beeperPos] != 0) { BEEP_OFF; warningLedDisable(); diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 38f38b236e..e87abdadd5 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -44,6 +44,7 @@ typedef enum { BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_LAUNCH_MODE_ENABLED, // Fixed-wing launch mode enabled BEEPER_LAUNCH_MODE_LOW_THROTTLE, // Fixed-wing launch mode enabled, but throttle is low + BEEPER_LAUNCH_MODE_IDLE_START, // Fixed-wing launch mode enabled, motor about to start at idle after set delay BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index 35e7d12a1f..af90f711ad 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -89,7 +89,7 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr) gpsSol.velNED[X] = pkt->nedVelNorth; gpsSol.velNED[Y] = pkt->nedVelEast; gpsSol.velNED[Z] = pkt->nedVelDown; - gpsSol.groundSpeed = sqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast)); + gpsSol.groundSpeed = fast_fsqrtf(sq((float)pkt->nedVelNorth) + sq((float)pkt->nedVelEast)); gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); diff --git a/src/main/io/gps_naza.c b/src/main/io/gps_naza.c index 9413ed2588..91e3d09fcd 100644 --- a/src/main/io/gps_naza.c +++ b/src/main/io/gps_naza.c @@ -199,7 +199,7 @@ static bool NAZA_parse_gps(void) gpsSol.eph = gpsConstrainEPE(h_acc / 10); // hAcc in cm gpsSol.epv = gpsConstrainEPE(v_acc / 10); // vAcc in cm gpsSol.numSat = _buffernaza.nav.satellites; - gpsSol.groundSpeed = sqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s + gpsSol.groundSpeed = fast_fsqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s // calculate gps heading from VELNE gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f)); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fda29b0234..6445034d75 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -194,7 +194,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 4); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); static int digitCount(int32_t value) @@ -317,6 +317,8 @@ static void osdLeftAlignString(char *buff) static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) { switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, 3)) { buff[3] = SYM_DIST_MI; @@ -325,7 +327,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) } buff[4] = '\0'; break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, 3)) { @@ -344,32 +346,34 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) */ static void osdFormatDistanceStr(char *buff, int32_t dist) { - int32_t centifeet; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + int32_t centifeet; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); + } else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); + } + break; } - break; - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (abs(dist) < METERS_PER_KILOMETER * 100) { - // Show meters when dist < 1km - tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); - } else { - // Show kilometers when dist >= 1km - tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), - (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); - } - break; - } } /** @@ -381,6 +385,8 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: return (vel * 224) / 10000; // Convert to mph case OSD_UNIT_METRIC: @@ -399,6 +405,8 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); break; @@ -422,6 +430,8 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: centivalue = (ws * 224) / 100; suffix = SYM_MPH; @@ -449,29 +459,38 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ void osdFormatAltitudeSymbol(char *buff, int32_t alt) { + int digits; + if (alt < 0) { + digits = 4; + } else { + digits = 3; + buff[0] = ' '; + } switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) { + if (osdFormatCentiNumber(buff + 4 - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { // Scaled to kft - buff[3] = SYM_ALT_KFT; + buff[4] = SYM_ALT_KFT; } else { // Formatted in feet - buff[3] = SYM_ALT_FT; + buff[4] = SYM_ALT_FT; } - buff[4] = '\0'; + buff[5] = '\0'; break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: // alt is alredy in cm - if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) { + if (osdFormatCentiNumber(buff + 4 - digits, alt, 1000, 0, 2, digits)) { // Scaled to km - buff[3] = SYM_ALT_KM; + buff[4] = SYM_ALT_KM; } else { // Formatted in m - buff[3] = SYM_ALT_M; + buff[4] = SYM_ALT_M; } - buff[4] = '\0'; + buff[5] = '\0'; break; } } @@ -484,11 +503,13 @@ static void osdFormatAltitudeStr(char *buff, int32_t alt) { int32_t value; switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: value = CENTIMETERS_TO_FEET(alt); tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: value = CENTIMETERS_TO_METERS(alt); @@ -654,6 +675,9 @@ static void osdFormatCraftName(char *buff) static const char * osdArmingDisabledReasonMessage(void) { + const char *message = NULL; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + switch (isArmingDisabledReason()) { case ARMING_DISABLED_FAILSAFE_SYSTEM: // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c @@ -678,6 +702,7 @@ static const char * osdArmingDisabledReasonMessage(void) #if defined(USE_NAV) // Check the exact reason switch (navigationIsBlockingArming(NULL)) { + char buf[6]; case NAV_ARMING_BLOCKER_NONE: break; case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: @@ -685,7 +710,9 @@ static const char * osdArmingDisabledReasonMessage(void) case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: - return OSD_MESSAGE_STR(OSD_MSG_1ST_WP_TOO_FAR); + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); + tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); + return message = messageBuf; case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); } @@ -1083,12 +1110,14 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: initialScale = 16; // 16m ~= 0.01miles break; - case OSD_UNIT_UK: - FALLTHROUGH; default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: initialScale = 10; // 10m as initial scale break; @@ -1385,6 +1414,25 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); } +int8_t getGeoWaypointNumber(int8_t waypointIndex) +{ + static int8_t lastWaypointIndex = 1; + static int8_t geoWaypointIndex; + + if (waypointIndex != lastWaypointIndex) { + lastWaypointIndex = geoWaypointIndex = waypointIndex; + for (uint8_t i = 0; i <= waypointIndex; i++) { + if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || + posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || + posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) { + geoWaypointIndex -= 1; + } + } + } + + return geoWaypointIndex + 1; +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -1497,6 +1545,27 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_GLIDESLOPE: + { + float horizontalSpeed = gpsSol.groundSpeed; + float sinkRate = -getEstimatedActualVelocity(Z); + static pt1Filter_t gsFilterState; + const timeMs_t currentTimeMs = millis(); + static timeMs_t gsUpdatedTimeMs; + float glideSlope = horizontalSpeed / sinkRate; + glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); + gsUpdatedTimeMs = currentTimeMs; + + buff[0] = SYM_GLIDESLOPE; + if (glideSlope > 0.0f && glideSlope < 100.0f) { + osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = '\0'; + break; + } + case OSD_GPS_LAT: osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat); break; @@ -1716,10 +1785,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: { - static timeUs_t updatedTimestamp = 0; /*static int32_t updatedTimeSeconds = 0;*/ - timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; +#if defined(USE_ADC) && defined(USE_GPS) + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); @@ -1728,10 +1798,13 @@ static bool osdDrawSingleElement(uint8_t item) #endif updatedTimestamp = currentTimeUs; } +#endif if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { buff[0] = SYM_FLY_M; strcpy(buff + 1, "--:--"); +#if defined(USE_ADC) && defined(USE_GPS) updatedTimestamp = 0; +#endif } else if (timeSeconds == -2) { // Wind is too strong to come back with cruise throttle buff[0] = SYM_FLY_M; @@ -1748,9 +1821,10 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_REMAINING_DISTANCE_BEFORE_RTH:; + static int32_t distanceMeters = -1; +#if defined(USE_ADC) && defined(USE_GPS) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); - static int32_t distanceMeters = -1; if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); @@ -1759,6 +1833,7 @@ static bool osdDrawSingleElement(uint8_t item) #endif updatedTimestamp = currentTimeUs; } +#endif buff[0] = SYM_TRIP_DIST; if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { buff[4] = SYM_DIST_M; @@ -1860,6 +1935,8 @@ static bool osdDrawSingleElement(uint8_t item) } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; } @@ -1895,7 +1972,7 @@ static bool osdDrawSingleElement(uint8_t item) snrUpdated = millis(); const char* showsnr = "-20"; - const char* hidesnr = " "; + const char* hidesnr = " "; if (snrFiltered > osdConfig()->snr_alarm) { if (cmsInMenu) { buff[0] = SYM_SNR; @@ -1986,8 +2063,9 @@ static bool osdDrawSingleElement(uint8_t item) fpVector3_t poi; geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); - while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more - osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i); + j = getGeoWaypointNumber(j); + while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more (48 = ascii 0) + osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 48 + j, i); } } } @@ -2030,7 +2108,8 @@ static bool osdDrawSingleElement(uint8_t item) rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); #endif - pitchAngle -= DEGREES_TO_RADIANS(osdConfig()->camera_uptilt); + pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0; + pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim()); if (osdConfig()->ahi_reverse_roll) { rollAngle = -rollAngle; } @@ -2069,6 +2148,8 @@ static bool osdDrawSingleElement(uint8_t item) sym = SYM_FTS; break; default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: // Already in cm/s sym = SYM_MS; @@ -2195,15 +2276,15 @@ static bool osdDrawSingleElement(uint8_t item) return true; case OSD_NAV_FW_CRUISE_THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); return true; case OSD_NAV_FW_PITCH2THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); return true; case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)currentBatteryProfile->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); return true; case OSD_FW_ALT_PID_OUTPUTS: @@ -2371,6 +2452,8 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = '\0'; } break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, 3); if (!moreThanAh) { @@ -2416,6 +2499,8 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); buff[3] = SYM_WH_MI; break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); buff[3] = SYM_WH_KM; @@ -2599,6 +2684,8 @@ static bool osdDrawSingleElement(uint8_t item) int maxDecimals; switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() scaleUnitDivisor = 0; @@ -2606,9 +2693,9 @@ static bool osdDrawSingleElement(uint8_t item) symScaled = SYM_MI; maxDecimals = 2; break; - case OSD_UNIT_UK: - FALLTHROUGH; default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m @@ -2731,7 +2818,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: - if (powerLimitsConfig()->continuousCurrent) { + if (currentBatteryProfile->powerLimits.continuousCurrent) { osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); buff[3] = SYM_AMP; buff[4] = '\0'; @@ -2745,7 +2832,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_ADC case OSD_PLIMIT_ACTIVE_POWER_LIMIT: { - if (powerLimitsConfig()->continuousPower) { + if (currentBatteryProfile->powerLimits.continuousPower) { bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; @@ -2883,6 +2970,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, + .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, @@ -2896,6 +2984,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, + .ahi_camera_uptilt_comp = SETTING_OSD_AHI_CAMERA_UPTILT_COMP_DEFAULT, .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, @@ -2958,6 +3047,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); + osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2); osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2); @@ -2983,17 +3073,17 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) // OSD_VARIO_NUM at the right of OSD_VARIO osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7); osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11); - osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6); + osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6); osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); #ifdef USE_SERIALRX_CRSF - osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(24, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(24, 11); - osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(25, 9); - osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(25, 10); + osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); + osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); + osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); #endif osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); @@ -3168,12 +3258,20 @@ static void osdCompleteAsyncInitialization(void) #define STATS_VALUE_X_POS 24 if (statsConfig()->stats_enabled) { displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:"); - if (osdConfig()->units == OSD_UNIT_IMPERIAL) { - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); - string_buffer[5] = SYM_MI; - } else { - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); - string_buffer[5] = SYM_KM; + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + string_buffer[5] = SYM_MI; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + string_buffer[5] = SYM_KM; + break; } string_buffer[6] = '\0'; displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer); @@ -3193,12 +3291,20 @@ static void osdCompleteAsyncInitialization(void) displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:"); if (statsConfig()->stats_total_dist) { uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km - if (osdConfig()->units == OSD_UNIT_IMPERIAL) { - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_MI; - } else { - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_KM; + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_MI; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_KM; + break; } } else { string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; @@ -3424,6 +3530,8 @@ static void osdShowStatsPage2(void) } } break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_METRIC: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3); @@ -3546,7 +3654,7 @@ static void osdFilterData(timeUs_t currentTimeUs) { static timeUs_t lastRefresh = 0; float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); - GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; + GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; if (lastRefresh) { @@ -3830,7 +3938,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // Countdown display for remaining Waypoints char buf[6]; osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); - tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf); + tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // WP hold time countdown in seconds @@ -3866,11 +3974,14 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); + if (FLIGHT_MODE(MANUAL_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); + } } if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8f4e4d4bf9..8863d9a732 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -53,7 +53,6 @@ #define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" #define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" #define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" -#define OSD_MSG_1ST_WP_TOO_FAR "FIRST WAYPOINT IS TOO FAR" #define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" #define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" #define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" @@ -98,6 +97,7 @@ #define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" #define OSD_MSG_AUTOTRIM "(AUTOTRIM)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" +#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" #define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" @@ -231,13 +231,15 @@ typedef enum { OSD_PLIMIT_REMAINING_BURST_TIME, OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, OSD_PLIMIT_ACTIVE_POWER_LIMIT, + OSD_GLIDESLOPE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC, - OSD_UNIT_UK, // Show speed in mp/h, other values in metric + OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph + OSD_UNIT_UK, // Show everything in imperial, temperature in C OSD_UNIT_MAX = OSD_UNIT_UK, } osd_unit_e; @@ -312,6 +314,7 @@ typedef struct osdConfig_s { #ifdef USE_SERIALRX_CRSF int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; + int16_t rssi_dbm_alarm; // in dBm #endif #ifdef USE_BARO int16_t baro_temp_alarm_min; @@ -331,6 +334,7 @@ typedef struct osdConfig_s { uint8_t crosshairs_style; // from osd_crosshairs_style_e int8_t horizon_offset; int8_t camera_uptilt; + bool ahi_camera_uptilt_comp; uint8_t camera_fov_h; uint8_t camera_fov_v; uint8_t hud_margin_h; diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 9d7a965bde..d22c1813ac 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -502,9 +502,11 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) break; case OSD_SIDEBAR_SCROLL_ALTITUDE: switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: return CENTIMETERS_TO_CENTIFEET(osdGetAltitude()); - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: return osdGetAltitude(); @@ -517,6 +519,8 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: // cms/s to (mi/h) * 100 return speed * 224 / 100; @@ -530,9 +534,11 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: #if defined(USE_GPS) switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: return CENTIMETERS_TO_CENTIFEET(GPS_distanceToHome * 100); - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: return GPS_distanceToHome * 100; @@ -575,13 +581,15 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os break; case OSD_SIDEBAR_SCROLL_ALTITUDE: switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: unit->symbol = SYM_ALT_FT; unit->divisor = FEET_PER_KILOFEET; unit->divided_symbol = SYM_ALT_KFT; *countsPerStep = 50; break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: unit->symbol = SYM_ALT_M; @@ -595,6 +603,8 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: unit->symbol = SYM_MPH; unit->divisor = 0; @@ -611,13 +621,15 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os break; case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: unit->symbol = SYM_FT; unit->divisor = FEET_PER_MILE; unit->divided_symbol = SYM_MI; *countsPerStep = 300; break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: unit->symbol = SYM_M; diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index adad66c6e2..3fa1d19d51 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -195,6 +195,6 @@ int16_t osdGet3DSpeed(void) { int16_t vert_speed = getEstimatedActualVelocity(Z); int16_t hor_speed = gpsSol.groundSpeed; - return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); + return (int16_t)fast_fsqrtf(sq(hor_speed) + sq(vert_speed)); } #endif diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 7c5d1886b0..d748bb2513 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -49,6 +49,7 @@ #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/rc_adjustments.h" #include "flight/imu.h" #include "flight/pid.h" @@ -70,6 +71,7 @@ #include "sensors/esc_sensor.h" #include "sensors/temperature.h" #include "sensors/pitotmeter.h" +#include "sensors/boardalignment.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -125,8 +127,25 @@ PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, .use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT, .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT, .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT, + .messageSpeedSource = SETTING_DJI_MESSAGE_SPEED_SOURCE_DEFAULT, + .rssi_source = SETTING_DJI_RSSI_SOURCE_DEFAULT, + .useAdjustments = SETTING_DJI_USE_ADJUSTMENTS_DEFAULT, + .craftNameAlternatingDuration = SETTING_DJI_CN_ALTERNATING_DURATION_DEFAULT ); +#define RSSI_BOUNDARY(PERCENT) (RSSI_MAX_VALUE / 100 * PERCENT) + +typedef enum { + DJI_OSD_CN_MESSAGES, + DJI_OSD_CN_THROTTLE, + DJI_OSD_CN_THROTTLE_AUTO_THR, + DJI_OSD_CN_AIR_SPEED, + DJI_OSD_CN_EFFICIENCY, + DJI_OSD_CN_DISTANCE, + DJI_OSD_CN_ADJUSTEMNTS, + DJI_OSD_CN_MAX_ELEMENTS +} DjiCraftNameElements_t; + // External dependency on looptime extern timeDelta_t cycleTime; @@ -413,7 +432,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height } -static const char * osdArmingDisabledReasonMessage(void) +static char * osdArmingDisabledReasonMessage(void) { switch (isArmingDisabledReason()) { case ARMING_DISABLED_FAILSAFE_SYSTEM: @@ -524,7 +543,7 @@ static const char * osdArmingDisabledReasonMessage(void) return NULL; } -static const char * osdFailsafePhaseMessage(void) +static char * osdFailsafePhaseMessage(void) { // See failsafe.h for each phase explanation switch (failsafePhase()) { @@ -565,7 +584,7 @@ static const char * osdFailsafePhaseMessage(void) return NULL; } -static const char * osdFailsafeInfoMessage(void) +static char * osdFailsafeInfoMessage(void) { if (failsafeIsReceivingRxData()) { // User must move sticks to exit FS mode @@ -575,7 +594,7 @@ static const char * osdFailsafeInfoMessage(void) return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG); } -static const char * navigationStateMessage(void) +static char * navigationStateMessage(void) { switch (NAV_Status.state) { case MW_NAV_STATE_NONE: @@ -635,10 +654,10 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; - + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: return (vel * 224) / 10000; // Convert to mph - case OSD_UNIT_METRIC: return (vel * 36) / 1000; // Convert to kmh } @@ -651,16 +670,37 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) * Converts velocity into a string based on the current unit system. * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) */ -void osdDJIFormatVelocityStr(char* buff, int32_t vel ) +void osdDJIFormatVelocityStr(char* buff) { + char sourceBuf[4]; + int vel = 0; + switch (djiOsdConfig()->messageSpeedSource) { + case OSD_SPEED_SOURCE_GROUND: + strcpy(sourceBuf, "GRD"); + vel = gpsSol.groundSpeed; + break; + case OSD_SPEED_SOURCE_3D: + strcpy(sourceBuf, "3D"); + vel = osdGet3DSpeed(); + break; + case OSD_SPEED_SOURCE_AIR: + strcpy(sourceBuf, "AIR"); +#ifdef USE_PITOT + vel = pitot.airSpeed; +#endif + break; + } + switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: - tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "MPH"); + tfp_sprintf(buff, "%s %3d MPH", sourceBuf, (int)osdConvertVelocityToUnit(vel)); break; case OSD_UNIT_METRIC: - tfp_sprintf(buff, "%3d%s", (int)osdConvertVelocityToUnit(vel), "KMH"); + tfp_sprintf(buff, "%s %3d KPH", sourceBuf, (int)osdConvertVelocityToUnit(vel)); break; } } @@ -683,6 +723,8 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) int32_t centifeet; switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: centifeet = CENTIMETERS_TO_CENTIFEET(dist); if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { @@ -695,7 +737,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi"); } break; - case OSD_UNIT_UK: + case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: if (abs(dist) < METERS_PER_KILOMETER * 100) { @@ -728,175 +770,362 @@ static void osdDJIEfficiencyMahPerKM(char *buff) 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; - } - else { + } else { value = eFilterState.state; } } if (value > 0 && value <= 999) { tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM"); - } - else { + } else { tfp_sprintf(buff, "%s", "---mAhKM"); } } -static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) +static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction) { - // :W T S E D - // | | | | Distance Trip - // | | | Efficiency mA/KM - // | | S 3dSpeed - // | Throttle - // Warnings - const char *message = " "; - const char *enabledElements = name + 1; - char djibuf[24]; - - // clear name from chars : and leading W - if (enabledElements[0] == 'W') { - enabledElements += 1; - } - - int elemLen = strlen(enabledElements); - - if (elemLen > 0) { - switch (enabledElements[OSD_ALTERNATING_CHOICES(3000, elemLen)]){ - case 'T': - osdDJIFormatThrottlePosition(djibuf,true); - break; - case 'S': - osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed()); - break; - case 'E': - osdDJIEfficiencyMahPerKM(djibuf); - break; - case 'D': - osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance()); - break; - case 'W': - tfp_sprintf(djibuf, "%s", "MAKE_W_FIRST"); - break; - default: - tfp_sprintf(djibuf, "%s", "UNKOWN_ELEM"); - break; - } - - if (djibuf[0] != '\0') { - message = djibuf; - } - } - - if (name[1] == 'W') { - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - if (ARMING_FLAG(ARMED)) { - // Aircraft is armed. We might have up to 5 - // messages to show. - const char *messages[5]; - unsigned messageCount = 0; - - if (FLIGHT_MODE(FAILSAFE_MODE)) { - // In FS mode while being armed too - const char *failsafePhaseMessage = osdFailsafePhaseMessage(); - const char *failsafeInfoMessage = osdFailsafeInfoMessage(); - const char *navStateFSMessage = navigationStateMessage(); - - if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; - } - - if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; - } - - if (navStateFSMessage) { - messages[messageCount++] = navStateFSMessage; - } - - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; - if (message == failsafeInfoMessage) { - // failsafeInfoMessage is not useful for recovering - // a lost model, but might help avoiding a crash. - // Blink to grab user attention. - //doesnt work TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - // We're shoing either failsafePhaseMessage or - // navStateFSMessage. Don't BLINK here since - // having this text available might be crucial - // during a lost aircraft recovery and blinking - // will cause it to be missing from some frames. - } - } - else { - if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; - } - } - else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = "AUTOLAUNCH"; - } - else { - if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { - // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO - // when it doesn't require ANGLE mode (required only in FW - // right now). If if requires ANGLE, its display is handled - // by OSD_FLYMODE. - messages[messageCount++] = "(ALT HOLD)"; - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { - messages[messageCount++] = "(AUTOTRIM)"; - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - messages[messageCount++] = "(AUTOTUNE)"; - } - if (FLIGHT_MODE(HEADFREE_MODE)) { - messages[messageCount++] = "(HEADFREE)"; - } - } - // Pick one of the available messages. Each message lasts - // a second. - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; - } - } - } - else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { - unsigned invalidIndex; - // Check if we're unable to arm for some reason - if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - const setting_t *setting = settingGet(invalidIndex); - settingGetName(setting, messageBuf); - for (int ii = 0; messageBuf[ii]; ii++) { - messageBuf[ii] = sl_toupper(messageBuf[ii]); - } - message = messageBuf; - } - else { - message = "ERR SETTING"; - // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } - } - else { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - message = "CANT ARM"; - // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } else { - // Show the reason for not arming - message = osdArmingDisabledReasonMessage(); - } - } - } - } - - if (message[0] != '\0') { - sbufWriteData(dst, message, strlen(message)); + switch (adjustmentFunction) { + case ADJUSTMENT_RC_EXPO: + tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8); + break; + case ADJUSTMENT_RC_YAW_EXPO: + tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8); + break; + case ADJUSTMENT_MANUAL_RC_EXPO: + tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8); + break; + case ADJUSTMENT_MANUAL_RC_YAW_EXPO: + tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8); + break; + case ADJUSTMENT_THROTTLE_EXPO: + tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8); + break; + case ADJUSTMENT_PITCH_ROLL_RATE: + tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]); + break; + case ADJUSTMENT_PITCH_RATE: + tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + break; + case ADJUSTMENT_ROLL_RATE: + tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + break; + case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE: + tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]); + break; + case ADJUSTMENT_MANUAL_PITCH_RATE: + tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]); + break; + case ADJUSTMENT_MANUAL_ROLL_RATE: + tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]); + break; + case ADJUSTMENT_YAW_RATE: + tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]); + break; + case ADJUSTMENT_MANUAL_YAW_RATE: + tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]); + break; + case ADJUSTMENT_PITCH_ROLL_P: + tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P); + break; + case ADJUSTMENT_PITCH_P: + tfp_sprintf(buff, "PP %3d", pidBankMutable()->pid[PID_PITCH].P); + break; + case ADJUSTMENT_ROLL_P: + tfp_sprintf(buff, "RP %3d", pidBankMutable()->pid[PID_ROLL].P); + break; + case ADJUSTMENT_PITCH_ROLL_I: + tfp_sprintf(buff, "PRI %3d %3d", pidBankMutable()->pid[PID_PITCH].I, pidBankMutable()->pid[PID_ROLL].I); + break; + case ADJUSTMENT_PITCH_I: + tfp_sprintf(buff, "PI %3d", pidBankMutable()->pid[PID_PITCH].I); + break; + case ADJUSTMENT_ROLL_I: + tfp_sprintf(buff, "RI %3d", pidBankMutable()->pid[PID_ROLL].I); + break; + case ADJUSTMENT_PITCH_ROLL_D: + tfp_sprintf(buff, "PRD %3d %3d", pidBankMutable()->pid[PID_PITCH].D, pidBankMutable()->pid[PID_ROLL].D); + break; + case ADJUSTMENT_PITCH_ROLL_FF: + tfp_sprintf(buff, "PRFF %3d %3d", pidBankMutable()->pid[PID_PITCH].FF, pidBankMutable()->pid[PID_ROLL].FF); + break; + case ADJUSTMENT_PITCH_D: + tfp_sprintf(buff, "PD %3d", pidBankMutable()->pid[PID_PITCH].D); + break; + case ADJUSTMENT_PITCH_FF: + tfp_sprintf(buff, "PFF %3d", pidBankMutable()->pid[PID_PITCH].FF); + break; + case ADJUSTMENT_ROLL_D: + tfp_sprintf(buff, "RD %3d", pidBankMutable()->pid[PID_ROLL].D); + break; + case ADJUSTMENT_ROLL_FF: + tfp_sprintf(buff, "RFF %3d", pidBankMutable()->pid[PID_ROLL].FF); + break; + case ADJUSTMENT_YAW_P: + tfp_sprintf(buff, "YP %3d", pidBankMutable()->pid[PID_YAW].P); + break; + case ADJUSTMENT_YAW_I: + tfp_sprintf(buff, "YI %3d", pidBankMutable()->pid[PID_YAW].I); + break; + case ADJUSTMENT_YAW_D: + tfp_sprintf(buff, "YD %3d", pidBankMutable()->pid[PID_YAW].D); + break; + case ADJUSTMENT_YAW_FF: + tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF); + break; + case ADJUSTMENT_NAV_FW_CRUISE_THR: + tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle); + break; + case ADJUSTMENT_NAV_FW_PITCH2THR: + tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle); + break; + case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: + tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees); + break; + case ADJUSTMENT_PITCH_BOARD_ALIGNMENT: + tfp_sprintf(buff, "PBA %3d", boardAlignment()->pitchDeciDegrees); + break; + case ADJUSTMENT_LEVEL_P: + tfp_sprintf(buff, "LP %3d", pidBankMutable()->pid[PID_LEVEL].P); + break; + case ADJUSTMENT_LEVEL_I: + tfp_sprintf(buff, "LI %3d", pidBankMutable()->pid[PID_LEVEL].I); + break; + case ADJUSTMENT_LEVEL_D: + tfp_sprintf(buff, "LD %3d", pidBankMutable()->pid[PID_LEVEL].D); + break; + case ADJUSTMENT_POS_XY_P: + tfp_sprintf(buff, "PXYP %3d", pidBankMutable()->pid[PID_POS_XY].P); + break; + case ADJUSTMENT_POS_XY_I: + tfp_sprintf(buff, "PXYI %3d", pidBankMutable()->pid[PID_POS_XY].I); + break; + case ADJUSTMENT_POS_XY_D: + tfp_sprintf(buff, "PXYD %3d", pidBankMutable()->pid[PID_POS_XY].D); + break; + case ADJUSTMENT_POS_Z_P: + tfp_sprintf(buff, "PZP %3d", pidBankMutable()->pid[PID_POS_Z].P); + break; + case ADJUSTMENT_POS_Z_I: + tfp_sprintf(buff, "PZI %3d", pidBankMutable()->pid[PID_POS_Z].I); + break; + case ADJUSTMENT_POS_Z_D: + tfp_sprintf(buff, "PZD %3d", pidBankMutable()->pid[PID_POS_Z].D); + break; + case ADJUSTMENT_HEADING_P: + tfp_sprintf(buff, "HP %3d", pidBankMutable()->pid[PID_HEADING].P); + break; + case ADJUSTMENT_VEL_XY_P: + tfp_sprintf(buff, "VXYP %3d", pidBankMutable()->pid[PID_VEL_XY].P); + break; + case ADJUSTMENT_VEL_XY_I: + tfp_sprintf(buff, "VXYI %3d", pidBankMutable()->pid[PID_VEL_XY].I); + break; + case ADJUSTMENT_VEL_XY_D: + tfp_sprintf(buff, "VXYD %3d", pidBankMutable()->pid[PID_VEL_XY].D); + break; + case ADJUSTMENT_VEL_Z_P: + tfp_sprintf(buff, "VZP %3d", pidBankMutable()->pid[PID_VEL_Z].P); + break; + case ADJUSTMENT_VEL_Z_I: + tfp_sprintf(buff, "VZI %3d", pidBankMutable()->pid[PID_VEL_Z].I); + break; + case ADJUSTMENT_VEL_Z_D: + tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D); + break; + case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: + tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle); + break; + case ADJUSTMENT_TPA: + tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); + break; + case ADJUSTMENT_TPA_BREAKPOINT: + tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint); + break; + case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: + tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness); + break; + default: + tfp_sprintf(buff, "UNSUPPORTED"); + break; } } + +static bool osdDJIFormatAdjustments(char *buff) +{ + uint8_t adjustmentFunctions[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; + uint8_t adjustmentCount = getActiveAdjustmentFunctions(adjustmentFunctions); + + if (adjustmentCount > 0 && buff != NULL) { + osdDJIAdjustmentMessage(buff, adjustmentFunctions[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, adjustmentCount)]); + } + + return adjustmentCount > 0; +} + + +static bool djiFormatMessages(char *buff) +{ + bool haveMessage = false; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + if (ARMING_FLAG(ARMED)) { + // Aircraft is armed. We might have up to 6 + // messages to show. + char *messages[6]; + unsigned messageCount = 0; + + if (FLIGHT_MODE(FAILSAFE_MODE)) { + // In FS mode while being armed too + char *failsafePhaseMessage = osdFailsafePhaseMessage(); + char *failsafeInfoMessage = osdFailsafeInfoMessage(); + char *navStateFSMessage = navigationStateMessage(); + + if (failsafePhaseMessage) { + messages[messageCount++] = failsafePhaseMessage; + } + + if (failsafeInfoMessage) { + messages[messageCount++] = failsafeInfoMessage; + } + + if (navStateFSMessage) { + messages[messageCount++] = navStateFSMessage; + } + } else { +#ifdef USE_SERIALRX_CRSF + if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) { + messages[messageCount++] = "CRSF LOW RF"; + } +#endif + if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } + } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + messages[messageCount++] = "AUTOLAUNCH"; + } else { + if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { + // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO + // when it doesn't require ANGLE mode (required only in FW + // right now). If if requires ANGLE, its display is handled + // by OSD_FLYMODE. + messages[messageCount++] = "(ALT HOLD)"; + } + + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { + messages[messageCount++] = "(AUTOTRIM)"; + } + + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { + messages[messageCount++] = "(AUTOTUNE)"; + } + + if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) { + messages[messageCount++] = "(AUTOLEVEL)"; + } + + if (FLIGHT_MODE(HEADFREE_MODE)) { + messages[messageCount++] = "(HEADFREE)"; + } + + if (FLIGHT_MODE(MANUAL_MODE)) { + messages[messageCount++] = "(MANUAL)"; + } + } + } + // Pick one of the available messages. Each message lasts + // a second. + if (messageCount > 0) { + strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);; + haveMessage = true; + } + } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { + unsigned invalidIndex; + // Check if we're unable to arm for some reason + if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { + if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) { + const setting_t *setting = settingGet(invalidIndex); + settingGetName(setting, messageBuf); + for (int ii = 0; messageBuf[ii]; ii++) { + messageBuf[ii] = sl_toupper(messageBuf[ii]); + } + strcpy(buff, messageBuf); + } else { + strcpy(buff, "ERR SETTING"); + // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } + } else { + if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) { + strcpy(buff, "CANT ARM"); + // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } else { + // Show the reason for not arming + strcpy(buff, osdArmingDisabledReasonMessage()); + } + } + haveMessage = true; + } + return haveMessage; +} + +static void djiSerializeCraftNameOverride(sbuf_t *dst) +{ + char djibuf[DJI_CRAFT_NAME_LENGTH] = "\0"; + uint16_t *osdLayoutConfig = (uint16_t*)(osdLayoutsConfig()->item_pos[0]); + + if (!(OSD_VISIBLE(osdLayoutConfig[OSD_MESSAGES]) && djiFormatMessages(djibuf)) + && !(djiOsdConfig()->useAdjustments && osdDJIFormatAdjustments(djibuf))) { + + DjiCraftNameElements_t activeElements[DJI_OSD_CN_MAX_ELEMENTS]; + uint8_t activeElementsCount = 0; + + if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS])) { + activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE; + } + + if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS_AUTO_THR])) { + activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR; + } + + if (OSD_VISIBLE(osdLayoutConfig[OSD_3D_SPEED])) { + activeElements[activeElementsCount++] = DJI_OSD_CN_AIR_SPEED; + } + + if (OSD_VISIBLE(osdLayoutConfig[OSD_EFFICIENCY_MAH_PER_KM])) { + activeElements[activeElementsCount++] = DJI_OSD_CN_EFFICIENCY; + } + + if (OSD_VISIBLE(osdLayoutConfig[OSD_TRIP_DIST])) { + activeElements[activeElementsCount++] = DJI_OSD_CN_DISTANCE; + } + + switch (activeElements[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, activeElementsCount)]) + { + case DJI_OSD_CN_THROTTLE: + osdDJIFormatThrottlePosition(djibuf, false); + break; + case DJI_OSD_CN_THROTTLE_AUTO_THR: + osdDJIFormatThrottlePosition(djibuf, true); + break; + case DJI_OSD_CN_AIR_SPEED: + osdDJIFormatVelocityStr(djibuf); + break; + case DJI_OSD_CN_EFFICIENCY: + osdDJIEfficiencyMahPerKM(djibuf); + break; + case DJI_OSD_CN_DISTANCE: + osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance()); + break; + default: + break; + } + } + + if (djibuf[0] != '\0') { + sbufWriteData(dst, djibuf, strlen(djibuf)); + } +} + #endif @@ -933,26 +1162,17 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms case DJI_MSP_NAME: { - const char * name = systemConfig()->name; - #if defined(USE_OSD) if (djiOsdConfig()->use_name_for_messages) { - if (name[0] == ':') { - // If craft name starts with a semicolon - use it as a template for what we want to show - djiSerializeCraftNameOverride(dst, name); - } - else { - // Otherwise fall back to just warnings - djiSerializeCraftNameOverride(dst, ":W"); - } - } - else + djiSerializeCraftNameOverride(dst); + } else { #endif - { - int len = strlen(name); - sbufWriteData(dst, name, MAX(len, 12)); - break; + sbufWriteData(dst, systemConfig()->name, MAX((int)strlen(systemConfig()->name), OSD_MESSAGE_LENGTH)); +#if defined(USE_OSD) } +#endif + + break; } break; @@ -1029,7 +1249,22 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms case DJI_MSP_ANALOG: sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery - sbufWriteU16(dst, getRSSI()); +#ifdef USE_SERIALRX_CRSF + // Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz + if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ) { + uint16_t scaledLq = 0; + if (rxLinkStatistics.rfMode >= 2) { + scaledLq = RSSI_MAX_VALUE; + } else { + scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98)); + } + sbufWriteU16(dst, scaledLq); + } else { +#endif + sbufWriteU16(dst, getRSSI()); +#ifdef USE_SERIALRX_CRSF + } +#endif sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A sbufWriteU16(dst, getBatteryVoltage()); break; diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h index d105b275b6..4b3e0a1479 100644 --- a/src/main/io/osd_dji_hd.h +++ b/src/main/io/osd_dji_hd.h @@ -62,12 +62,21 @@ #define DJI_MSP_SET_PID 202 #define DJI_MSP_SET_RC_TUNING 204 +#define DJI_CRAFT_NAME_LENGTH 24 +#define DJI_ALTERNATING_DURATION_LONG (djiOsdConfig()->craftNameAlternatingDuration * 100) +#define DJI_ALTERNATING_DURATION_SHORT 1000 + enum djiOsdTempSource_e { DJI_OSD_TEMP_ESC = 0, DJI_OSD_TEMP_CORE = 1, DJI_OSD_TEMP_BARO = 2 }; +enum djiRssiSource_e { + DJI_RSSI = 0, + DJI_CRSF_LQ = 1 +}; + enum djiOsdProtoWorkarounds_e { DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0, }; @@ -76,6 +85,10 @@ typedef struct djiOsdConfig_s { uint8_t use_name_for_messages; uint8_t esc_temperature_source; uint8_t proto_workarounds; + uint8_t messageSpeedSource; + uint8_t rssi_source; + uint8_t useAdjustments; + uint8_t craftNameAlternatingDuration; } djiOsdConfig_t; PG_DECLARE(djiOsdConfig_t, djiOsdConfig); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 79aaa905dc..3eeed22c56 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -95,7 +95,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -141,7 +141,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // MC-specific .mc = { .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees - .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT, .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed #ifdef USE_MR_BRAKING_MODE @@ -165,12 +164,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees - .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT, .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT, - .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT, - .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT, - .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle) .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT, .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT, .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m @@ -182,9 +177,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G) .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms - .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 @@ -1041,7 +1035,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime; if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate)); - float centidegsPerIteration = rateTarget * timeDifference / 1000.0f; + float centidegsPerIteration = rateTarget * timeDifference * 0.001f; posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw)); posControl.cruise.lastYawAdjustmentTime = currentTimeMs; @@ -1730,7 +1724,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle. if (feature(FEATURE_FW_LAUNCH)) { throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); - if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) { + if ((throttleStatus == THROTTLE_LOW) && (isRollPitchStickDeflected())) { abortFixedWingLaunch(); return NAV_FSM_EVENT_SWITCH_TO_IDLE; } @@ -1936,7 +1930,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali posControl.actualState.agl.vel.x = newVelX; posControl.actualState.agl.vel.y = newVelY; - posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY)); + posControl.actualState.velXY = fast_fsqrtf(sq(newVelX) + sq(newVelY)); // CASE 1: POS & VEL valid if (estPosValid && estVelValid) { @@ -2070,7 +2064,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void) *-----------------------------------------------------------*/ static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY) { - return sqrtf(sq(deltaX) + sq(deltaY)); + return fast_fsqrtf(sq(deltaX) + sq(deltaY)); } static int32_t calculateBearingFromDelta(float deltaX, float deltaY) @@ -2147,8 +2141,8 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp static void updateHomePositionCompatibility(void) { geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); - GPS_distanceToHome = posControl.homeDistance / 100; - GPS_directionToHome = posControl.homeDirection / 100; + GPS_distanceToHome = posControl.homeDistance * 0.01f; + GPS_directionToHome = posControl.homeDirection * 0.01f; } // Backdoor for RTH estimator @@ -2863,13 +2857,23 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) { if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) { // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission) + static int8_t nonGeoWaypointCount = 0; + if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) { posControl.waypointList[wpNumber - 1] = *wpData; - if(wpData->action == NAV_WP_ACTION_JUMP) { - posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #) + if(wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD || wpData->action == NAV_WP_ACTION_JUMP) { + nonGeoWaypointCount += 1; + if(wpData->action == NAV_WP_ACTION_JUMP) { + posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #) + } } + posControl.waypointCount = wpNumber; posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST); + posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount; + if (posControl.waypointListValid) { + nonGeoWaypointCount = 0; + } } } } @@ -2881,6 +2885,7 @@ void resetWaypointList(void) if (!ARMING_FLAG(ARMED)) { posControl.waypointCount = 0; posControl.waypointListValid = false; + posControl.geoWaypointCount = 0; } } @@ -2895,13 +2900,13 @@ int getWaypointCount(void) } #ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE -bool loadNonVolatileWaypointList(void) +bool loadNonVolatileWaypointList(bool clearIfLoaded) { if (ARMING_FLAG(ARMED)) return false; - // if waypoints are already loaded, just unload them. - if (posControl.waypointCount > 0) { + // if forced and waypoints are already loaded, just unload them. + if (clearIfLoaded && posControl.waypointCount > 0) { resetWaypointList(); return false; } @@ -2952,8 +2957,15 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint { gpsLocation_t wpLLH; - wpLLH.lat = waypoint->lat; - wpLLH.lon = waypoint->lon; + /* Default to home position if lat & lon = 0 or HOME flag set + * Applicable to WAYPOINT, HOLD_TIME & LANDING WP types */ + if ((waypoint->lat == 0 && waypoint->lon == 0) || waypoint->flag == NAV_WP_FLAG_HOME) { + wpLLH.lat = GPS_home.lat; + wpLLH.lon = GPS_home.lon; + } else { + wpLLH.lat = waypoint->lat; + wpLLH.lon = waypoint->lon; + } wpLLH.alt = waypoint->alt; geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv); @@ -3358,6 +3370,13 @@ bool navigationTerrainFollowingEnabled(void) return posControl.flags.isTerrainFollowEnabled; } +uint32_t distanceToFirstWP(void) +{ + fpVector3_t startingWaypointPos; + mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); + return calculateDistanceToDestination(&startingWaypointPos); +} + navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); @@ -3374,7 +3393,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) // Apply extra arming safety only if pilot has any of GPS modes configured if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) { if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS && - (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || rxGetChannelValue(YAW) > 1750)) { + (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || checkStickPosition(YAW_HI))) { if (usedBypass) { *usedBypass = true; } @@ -3390,12 +3409,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) // Don't allow arming if first waypoint is farther than configured safe distance if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) { - fpVector3_t startingWaypointPos; - mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); - - const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; - - if (navWpMissionStartTooFar) { + if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) { return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR; } } @@ -3617,7 +3631,7 @@ void navigationInit(void) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) if (navConfig()->general.waypoint_load_on_boot) - loadNonVolatileWaypointList(); + loadNonVolatileWaypointList(true); #endif } @@ -3761,7 +3775,7 @@ static void GPS_distance_cm_bearing(int32_t currentLat1, int32_t currentLon1, in const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees const float dLon = (float)(destinationLon2 - currentLon1) * GPS_scaleLonDown; - *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; + *dist = fast_fsqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 8ef7af6ccb..9ee4d92df8 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -225,7 +225,6 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // multicopter max banking angle (deg) - uint16_t hover_throttle; // multicopter hover throttle uint16_t auto_disarm_delay; // multicopter safety delay for landing detector #ifdef USE_MR_BRAKING_MODE @@ -248,12 +247,8 @@ typedef struct navConfig_s { uint8_t max_bank_angle; // Fixed wing max banking angle (deg) uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) - uint16_t cruise_throttle; // Cruise throttle uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation - uint16_t min_throttle; // Minimum allowed throttle in auto mode - uint16_t max_throttle; // Maximum allowed throttle in auto mode - uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing @@ -261,9 +256,8 @@ typedef struct navConfig_s { uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s) uint16_t launch_time_thresh; // Time threshold for launch detection (ms) - 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 @@ -306,6 +300,7 @@ typedef enum { } navWaypointHeadings_e; typedef enum { + NAV_WP_FLAG_HOME = 0x48, NAV_WP_FLAG_LAST = 0xA5 } navWaypointFlags_e; @@ -467,7 +462,7 @@ bool isWaypointListValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); void resetWaypointList(void); -bool loadNonVolatileWaypointList(void); +bool loadNonVolatileWaypointList(bool); bool saveNonVolatileWaypointList(void); float getFinalRTHAltitude(void); @@ -514,6 +509,7 @@ geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e da /* Distance/bearing calculation */ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); +uint32_t distanceToFirstWP(void); /* Failsafe-forced RTH mode */ void activateForcedRTH(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 18fd865198..59936c5d5f 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -53,6 +53,8 @@ #include "rx/rx.h" +#include "sensors/battery.h" + // Base frequencies for smoothing pitch and roll #define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f #define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f @@ -134,10 +136,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // velocity error. We use PID controller on altitude error and calculate desired pitch angle // Update energies - const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS; + const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS; const float demSKE = 0.0f; - const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS; + const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS; const float estSKE = 0.0f; // speedWeight controls balance between potential and kinetic energy used for pitch controller @@ -236,13 +238,28 @@ void resetFixedWingPositionController(void) static int8_t loiterDirection(void) { int8_t dir = 1; //NAV_LOITER_RIGHT - if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1; + + if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) { + dir = -1; + } + if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { - if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise - if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c + + if (rcCommand[YAW] < -250) { + loiterDirYaw = 1; //RIGHT //yaw is contrariwise + } + + if (rcCommand[YAW] > 250) { + loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c + } + dir = loiterDirYaw; } - if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1; + + if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { + dir *= -1; + } + return dir; } @@ -251,7 +268,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); + float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY)); // Limit minimum forward velocity to 1 m/s float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); @@ -274,7 +291,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // We have temporary loiter target. Recalculate distance and position error posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y; - distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); + distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY)); } // Calculate virtual waypoint @@ -318,7 +335,7 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr -limit, limit, yawPidFlags - ) / 100.0f; + ) * 0.01f; DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional); DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral); @@ -485,18 +502,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { // Unfiltered throttle correction outside of pitch deadband - return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; + return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle; } else { // Filtered throttle correction inside of pitch deadband - return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle; + return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle; } } void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) { - int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle; - int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle; + int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle; + int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle; if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) { // ROLL >0 right, <0 left @@ -531,15 +548,16 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); } - uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); + uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle); // Manual throttle increase if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { - if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95) - correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle); - else + if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ + correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); + } else { correctedThrottleValue = motorConfig()->maxthrottle; - isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle); + } + isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle); } else { isAutoThrottleManuallyIncreased = false; } @@ -645,7 +663,7 @@ void applyFixedWingEmergencyLandingController(void) rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); - rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; + rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; } /*----------------------------------------------------------- @@ -682,24 +700,28 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, // Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control resetFixedWingAltitudeController(); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); - } else + } else { applyFixedWingAltitudeAndThrottleController(currentTimeUs); + } } - if (navStateFlags & NAV_CTL_POS) + if (navStateFlags & NAV_CTL_POS) { applyFixedWingPositionController(currentTimeUs); + } } else { posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[ROLL] = 0; } - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) { rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); + } //if (navStateFlags & NAV_CTL_YAW) - if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) + if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs); + } } } diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 086d9a378d..f4c8fc29d4 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -50,10 +50,13 @@ #include "io/gps.h" +#include "sensors/battery.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_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 +64,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 +82,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 +95,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); @@ -111,6 +117,7 @@ typedef struct fixedWingLaunchData_s { } fixedWingLaunchData_t; static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch; +static bool idleMotorAboutToStart; static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = { @@ -125,19 +132,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] = { @@ -211,13 +227,13 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi static bool isThrottleIdleEnabled(void) { - return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); + return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue(); } static void applyThrottleIdleLogic(bool forceMixerIdle) { if (isThrottleIdleEnabled() && !forceMixerIdle) { - rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; + rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle; } else { ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped @@ -237,7 +253,7 @@ static inline bool isLaunchMaxAltitudeReached(void) static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) { - return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband(); + return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && isRollPitchStickDeflected(); } static void resetPidsIfNeeded(void) { @@ -287,6 +303,24 @@ 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) { + idleMotorAboutToStart = false; + return FW_LAUNCH_EVENT_SUCCESS; + } + // 5 second warning motor about to start at idle, changes Beeper sound + idleMotorAboutToStart = navConfig()->fw.launch_idle_motor_timer - currentStateElapsedMs(currentTimeUs) < 5000; + + return FW_LAUNCH_EVENT_NONE; +} + static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) { if (isThrottleLow()) { @@ -299,7 +333,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t return FW_LAUNCH_EVENT_SUCCESS; } else { - rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle); } @@ -365,14 +399,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time; - const uint16_t launchThrottle = navConfig()->fw.launch_throttle; + const uint16_t launchThrottle = currentBatteryProfile->nav.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); + const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); } @@ -381,7 +415,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { - rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; + rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_throttle; if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state @@ -403,7 +437,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; - if (areSticksDeflectedMoreThanPosHoldDeadband()) { + if (isRollPitchStickDeflected()) { return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state } if (elapsedTimeMs > endTimeMs) { @@ -411,7 +445,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr } 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]); + rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, currentBatteryProfile->nav.fw.launch_throttle, rcCommand[THROTTLE]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]); } @@ -441,7 +475,11 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); } else { - beeper(BEEPER_LAUNCH_MODE_ENABLED); + if (idleMotorAboutToStart) { + beeper(BEEPER_LAUNCH_MODE_IDLE_START); + } else { + beeper(BEEPER_LAUNCH_MODE_ENABLED); + } } } @@ -476,6 +514,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; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 182ad27aa8..9c2faa926d 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -52,6 +52,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "sensors/battery.h" + /*----------------------------------------------------------- * Altitude controller for multicopter aircraft *-----------------------------------------------------------*/ @@ -105,8 +107,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle; - const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle; + const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; + const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); @@ -240,7 +242,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); // Save processed throttle for future use rcCommandAdjustedThrottle = rcCommand[THROTTLE]; @@ -438,7 +440,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed) float newVelY = posErrorY * posControl.pids.pos[Y].param.kP; // Scale velocity to respect max_speed - float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY)); + float newVelTotal = fast_fsqrtf(sq(newVelX) + sq(newVelY)); /* * We override computed speed with max speed in following cases: @@ -498,7 +500,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float setpointX = posControl.desiredState.vel.x; const float setpointY = posControl.desiredState.vel.y; - const float setpointXY = sqrtf(powf(setpointX, 2)+powf(setpointY, 2)); + const float setpointXY = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2)); // Calculate velocity error const float velErrorX = setpointX - measurementX; @@ -506,7 +508,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Calculate XY-acceleration limit according to velocity error limit float accelLimitX, accelLimitY; - const float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY)); + const float velErrorMagnitude = fast_fsqrtf(sq(velErrorX) + sq(velErrorY)); if (velErrorMagnitude > 0.1f) { accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX); accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY); @@ -777,7 +779,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[THROTTLE] = getThrottleIdleValue(); } else { - rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; + rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; } return; @@ -804,7 +806,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 700909aaa4..236420c89b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -164,7 +164,7 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs) predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT; /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */ - gpsDistance = sqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y)); + gpsDistance = fast_fsqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y)); if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) { isGlitching = false; } @@ -640,7 +640,7 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x; const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y; - const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual)); + const float gpsPosResidualMag = fast_fsqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual)); //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f); const float gpsWeightScaler = 1.0f; diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index 60ecaad865..d3fac7c5de 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -109,7 +109,7 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt; ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt; - ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, fast_fsqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p); } DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X])); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h old mode 100755 new mode 100644 index d53a618de1..638d591ac4 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -359,8 +359,9 @@ typedef struct { navWaypoint_t waypointList[NAV_MAX_WAYPOINTS]; bool waypointListValid; int8_t waypointCount; + int8_t geoWaypointCount; // total geospatial WPs in mission - navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation + navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation int8_t activeWaypointIndex; float wpInitialAltitude; // Altitude at start of WP float wpInitialDistance; // Distance when starting flight to WP diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c index 40c075107b..62cce388c8 100644 --- a/src/main/navigation/navigation_rover_boat.c +++ b/src/main/navigation/navigation_rover_boat.c @@ -29,13 +29,19 @@ FILE_COMPILE_FOR_SIZE #ifdef USE_NAV #include "build/debug.h" + #include "common/utils.h" + #include "fc/rc_controls.h" #include "fc/config.h" + #include "flight/mixer.h" + #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "sensors/battery.h" + static bool isYawAdjustmentValid = false; static int32_t navHeadingError; @@ -125,7 +131,7 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat rcCommand[YAW] = posControl.rcAdjustment[YAW]; } - rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle); + rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle); } } } @@ -136,7 +142,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, rcCommand[ROLL] = 0; rcCommand[PITCH] = 0; rcCommand[YAW] = 0; - rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; + rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; } else if (navStateFlags & NAV_CTL_POS) { applyRoverBoatPositionController(currentTimeUs); applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 01e2dada95..5fa6f92733 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -514,7 +514,7 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE: //in m - return constrain(sqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX); + return constrain(fast_fsqrtf(sq(GPS_distanceToHome) + sq(getEstimatedActualPosition(Z)/100)), 0, INT16_MAX); break; case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ: diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index a54e16d76d..951b0eba10 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -19,6 +19,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" +#define MAVLINK_COMM_NUM_BUFFERS 1 #include "common/mavlink.h" #pragma GCC diagnostic pop diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index b03276aacb..75b532e515 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -299,7 +299,9 @@ void FAST_CODE NOINLINE scheduler(void) #if defined(SCHEDULER_DEBUG) DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler #endif - } else { + } + + if (!selectedTask || forcedRealTimeTask) { // Execute system real-time callbacks and account for them to SYSTEM account const timeUs_t currentTimeBeforeTaskCall = micros(); taskRunRealtimeCallbacks(currentTimeBeforeTaskCall); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index aeb6af2965..1f5d1de04d 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -616,20 +616,20 @@ void updateAccExtremes(void) if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis]; } - float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2])); + float gforce = fast_fsqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2])); if (gforce > acc.maxG) acc.maxG = gforce; } void accGetVibrationLevels(fpVector3_t *accVibeLevels) { - accVibeLevels->x = sqrtf(acc.accVibeSq[X]); - accVibeLevels->y = sqrtf(acc.accVibeSq[Y]); - accVibeLevels->z = sqrtf(acc.accVibeSq[Z]); + accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]); + accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]); + accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]); } float accGetVibrationLevel(void) { - return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); + return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); } uint32_t accGetClipCount(void) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index feaf9c46ba..8fdea16684 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -36,6 +36,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/fc_core.h" #include "fc/runtime_config.h" #include "fc/stats.h" @@ -93,7 +94,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 1); void pgResetFn_batteryProfiles(batteryProfile_t *instance) { @@ -115,7 +116,54 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT, .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT, .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, + }, + + .controlRateProfile = 0, + + .motor = { + .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT, + .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT, +#ifdef USE_DSHOT + .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT, +#endif + }, + + .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off. + + .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT, + + .nav = { + + .mc = { + .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT, + }, + + .fw = { + .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT, + .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT, + .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT, + .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle) + .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT, + .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP + } + + }, + +#if defined(USE_POWER_LIMITS) + .powerLimits = { + .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA + .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA + .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS + .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS +#ifdef USE_ADC + .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW + .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW + .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS + .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS +#endif // USE_ADC } +#endif // USE_POWER_LIMITS + ); } } @@ -197,6 +245,9 @@ void setBatteryProfile(uint8_t profileIndex) profileIndex = 0; } currentBatteryProfile = batteryProfiles(profileIndex); + if ((currentBatteryProfile->controlRateProfile > 0) && (currentBatteryProfile->controlRateProfile < MAX_CONTROL_RATE_PROFILE_COUNT)) { + setConfigProfile(currentBatteryProfile->controlRateProfile - 1); + } } void activateBatteryProfile(void) diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 6c4cd1bede..549515f13b 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -18,13 +18,16 @@ #pragma once #include "config/parameter_group.h" + #include "drivers/time.h" +#include "fc/settings.h" + +#include "sensors/battery_config_structs.h" + #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 1100 #endif -#define VBAT_SCALE_MIN 0 -#define VBAT_SCALE_MAX 65535 #ifndef CURRENT_METER_SCALE #define CURRENT_METER_SCALE 400 // for Allegro ACS758LCB-100U (40mV/A) @@ -35,90 +38,21 @@ #endif #ifndef MAX_BATTERY_PROFILE_COUNT -#define MAX_BATTERY_PROFILE_COUNT 3 +#define MAX_BATTERY_PROFILE_COUNT SETTING_CONSTANT_MAX_BATTERY_PROFILE_COUNT #endif -typedef enum { - CURRENT_SENSOR_NONE = 0, - CURRENT_SENSOR_ADC, - CURRENT_SENSOR_VIRTUAL, - CURRENT_SENSOR_ESC, - CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL -} currentSensor_e; - -typedef enum { - VOLTAGE_SENSOR_NONE = 0, - VOLTAGE_SENSOR_ADC, - VOLTAGE_SENSOR_ESC -} voltageSensor_e; - -typedef enum { - BAT_CAPACITY_UNIT_MAH, - BAT_CAPACITY_UNIT_MWH, -} batCapacityUnit_e; - typedef struct { uint8_t profile_index; uint16_t max_voltage; } profile_comp_t; -typedef enum { - BAT_VOLTAGE_RAW, - BAT_VOLTAGE_SAG_COMP -} batVoltageSource_e; - -typedef struct batteryMetersConfig_s { - -#ifdef USE_ADC - struct { - uint16_t scale; - voltageSensor_e type; - } voltage; -#endif - - struct { - int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A - int16_t offset; // offset of the current sensor in millivolt steps - currentSensor_e type; // type of current meter used, either ADC or virtual - } current; - - batVoltageSource_e voltageSource; - - uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW) - uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW) - uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH - - float throttle_compensation_weight; - -} batteryMetersConfig_t; - -typedef struct batteryProfile_s { - -#ifdef USE_ADC - uint8_t cells; - - struct { - uint16_t cellDetect; // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V) - uint16_t cellMax; // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V) - uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V) - uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V) - } voltage; -#endif - - struct { - uint32_t value; // mAh or mWh (see capacity.unit) - uint32_t warning; // mAh or mWh (see capacity.unit) - uint32_t critical; // mAh or mWh (see capacity.unit) - batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical - } capacity; - -} batteryProfile_t; - PG_DECLARE(batteryMetersConfig_t, batteryMetersConfig); PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles); extern const batteryProfile_t *currentBatteryProfile; +#define currentBatteryProfileMutable ((batteryProfile_t*)currentBatteryProfile) + typedef enum { BATTERY_OK = 0, BATTERY_WARNING, diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h new file mode 100644 index 0000000000..6443b886a4 --- /dev/null +++ b/src/main/sensors/battery_config_structs.h @@ -0,0 +1,144 @@ +/* + * This file is part of iNav + * + * iNav free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * iNav distributed in the hope that it + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include "platform.h" + +typedef enum { + CURRENT_SENSOR_NONE = 0, + CURRENT_SENSOR_ADC, + CURRENT_SENSOR_VIRTUAL, + CURRENT_SENSOR_ESC, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL +} currentSensor_e; + +typedef enum { + VOLTAGE_SENSOR_NONE = 0, + VOLTAGE_SENSOR_ADC, + VOLTAGE_SENSOR_ESC +} voltageSensor_e; + +typedef enum { + BAT_CAPACITY_UNIT_MAH, + BAT_CAPACITY_UNIT_MWH, +} batCapacityUnit_e; + +typedef enum { + BAT_VOLTAGE_RAW, + BAT_VOLTAGE_SAG_COMP +} batVoltageSource_e; + +typedef struct batteryMetersConfig_s { + +#ifdef USE_ADC + struct { + uint16_t scale; + voltageSensor_e type; + } voltage; +#endif + + struct { + int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + int16_t offset; // offset of the current sensor in millivolt steps + currentSensor_e type; // type of current meter used, either ADC or virtual + } current; + + batVoltageSource_e voltageSource; + + uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW) + uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW) + uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH + + float throttle_compensation_weight; + +} batteryMetersConfig_t; + +typedef struct batteryProfile_s { + +#ifdef USE_ADC + uint8_t cells; + + struct { + uint16_t cellDetect; // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V) + uint16_t cellMax; // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V) + uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V) + uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V) + } voltage; +#endif + + struct { + uint32_t value; // mAh or mWh (see capacity.unit) + uint32_t warning; // mAh or mWh (see capacity.unit) + uint32_t critical; // mAh or mWh (see capacity.unit) + batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical + } capacity; + + uint8_t controlRateProfile; + + struct { + float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent + float throttleScale; // Scaling factor for throttle. +#ifdef USE_DSHOT + uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash +#endif + } motor; + + uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. + + uint16_t fwMinThrottleDownPitchAngle; + + struct { + + struct { + uint16_t hover_throttle; // multicopter hover throttle + } mc; + + struct { + uint16_t cruise_throttle; // Cruise throttle + uint16_t min_throttle; // Minimum allowed throttle in auto mode + uint16_t max_throttle; // Maximum allowed throttle in auto mode + uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) + uint16_t launch_idle_throttle; // Throttle to keep at launch idle + uint16_t launch_throttle; // Launch throttle + } fw; + + } nav; + +#if defined(USE_POWER_LIMITS) + struct { + uint16_t continuousCurrent; // dA + uint16_t burstCurrent; // dA + uint16_t burstCurrentTime; // ds + uint16_t burstCurrentFalldownTime; // ds + +#ifdef USE_ADC + uint16_t continuousPower; // dW + uint16_t burstPower; // dW + uint16_t burstPowerTime; // ds + uint16_t burstPowerFalldownTime; // ds +#endif // USE_ADC + } powerLimits; +#endif // USE_POWER_LIMITS + +} batteryProfile_t; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 825d6828bf..c978c48829 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -75,6 +75,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/gyroanalyse.h" #include "flight/rpm_filter.h" #include "flight/dynamic_gyro_notch.h" +#include "flight/kalman.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -111,7 +112,7 @@ STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT]; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 15); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -142,6 +143,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT, .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT, #endif +#ifdef USE_GYRO_KALMAN + .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, + .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT, +#endif ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -318,6 +323,11 @@ static void gyroInitFilters(void) } } #endif +#ifdef USE_GYRO_KALMAN + if (gyroConfig()->kalmanEnabled) { + gyroKalmanInitialize(gyroConfig()->kalman_q); + } +#endif } bool gyroInit(void) @@ -501,6 +511,12 @@ void FAST_CODE NOINLINE gyroFilter() } #endif +#ifdef USE_GYRO_KALMAN + if (gyroConfig()->kalmanEnabled) { + gyroADCf = gyroKalmanUpdate(axis, gyroADCf); + } +#endif + gyro.gyroADCf[axis] = gyroADCf; } @@ -517,6 +533,7 @@ void FAST_CODE NOINLINE gyroFilter() } } #endif + } void FAST_CODE NOINLINE gyroUpdate() diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index ebf38f2e96..6646007194 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -63,7 +63,7 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. - uint8_t gyro_anti_aliasing_lpf_hz; + uint16_t gyro_anti_aliasing_lpf_hz; uint8_t gyro_anti_aliasing_lpf_type; #ifdef USE_DUAL_GYRO uint8_t gyro_to_use; @@ -87,6 +87,10 @@ typedef struct gyroConfig_s { float alphaBetaGammaBoost; float alphaBetaGammaHalfLife; #endif +#ifdef USE_GYRO_KALMAN + uint16_t kalman_q; + uint8_t kalmanEnabled; +#endif } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index 9c1b6f265e..aaecd8bc53 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -237,8 +237,8 @@ void opflowUpdate(timeUs_t currentTimeUs) else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) { // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime; - opflowCalibrationBodyAcc += sqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y])); - opflowCalibrationFlowAcc += sqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt; + opflowCalibrationBodyAcc += fast_fsqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y])); + opflowCalibrationFlowAcc += fast_fsqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt; } } diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index a330be79bb..332f98e02e 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -223,7 +223,7 @@ STATIC_PROTOTHREAD(pitotThread) // // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale - pitot.airSpeed = pitotmeterConfig()->pitot_scale * sqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100; + pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / AIR_DENSITY_SEA_LEVEL_15C) * 100; } else { performPitotCalibrationCycle(); pitot.airSpeed = 0; diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index d0e13b8a12..28ee33cf22 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -146,7 +146,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h index 4a554cdd56..460d2ae820 100644 --- a/src/main/target/AIRBOTF7/target.h +++ b/src/main/target/AIRBOTF7/target.h @@ -59,7 +59,6 @@ #endif // *************** OSD ***************************** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PC15 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 9f48f87c37..8e727f1171 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -138,7 +138,6 @@ #define I2C1_SCL PB6 #define I2C1_SDA PB7 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index fbd9683c92..0d9b879e04 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -125,7 +125,6 @@ #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 0bb51355ec..2ba9486297 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -111,7 +111,6 @@ #define USE_FLASH_M25P16 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h index d9018a5cd9..ea2699dc2f 100644 --- a/src/main/target/ASGARD32F4/target.h +++ b/src/main/target/ASGARD32F4/target.h @@ -117,7 +117,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h index 97e2ad210e..dc82fb74be 100644 --- a/src/main/target/ASGARD32F7/target.h +++ b/src/main/target/ASGARD32F7/target.h @@ -119,7 +119,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 9d86514f9d..9c78cf7738 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -57,7 +57,6 @@ #define TEMPERATURE_I2C_BUS BUS_I2C1 #define BNO055_I2C_BUS BUS_I2C1 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 1c4cc3fe68..71e469db2c 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -79,7 +79,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA1 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 872327b41a..4116a52001 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -45,7 +45,6 @@ #define BMP280_SPI_BUS BUS_SPI2 #define BMP280_CS_PIN PB3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/BETAFPVF722/CMakeLists.txt b/src/main/target/BETAFPVF722/CMakeLists.txt new file mode 100644 index 0000000000..bad557907d --- /dev/null +++ b/src/main/target/BETAFPVF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(BETAFPVF722) diff --git a/src/main/target/BETAFPVF722/target.c b/src/main/target/BETAFPVF722/target.c new file mode 100755 index 0000000000..2d02a8a71e --- /dev/null +++ b/src/main/target/BETAFPVF722/target.c @@ -0,0 +1,44 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR, 0, 0), // S4 D(2, 7, 7) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S5 DMA1_ST2 + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S6 DMA2_ST6 + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), //camera + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFPVF722/target.h b/src/main/target/BETAFPVF722/target.h new file mode 100755 index 0000000000..4342800797 --- /dev/null +++ b/src/main/target/BETAFPVF722/target.h @@ -0,0 +1,166 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BHF7" + +#define USBD_PRODUCT_STRING "BETAFPVF722" + +#define LED0 PC15 + +#define BEEPER PC14 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2_SHARES_UART3 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +// *************** OSD************************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + + +// *************** FLASH ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 6 +#define TARGET_MOTOR_COUNT 4 + +// ESC-related features +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIALSHOT diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index 56c3918aff..45778103a9 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -64,7 +64,6 @@ #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index c2787f71ef..a36a59a63a 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -66,7 +66,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h index ca29604c6c..2ff18eeccb 100644 --- a/src/main/target/DALRCF722DUAL/target.h +++ b/src/main/target/DALRCF722DUAL/target.h @@ -103,7 +103,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index e7090f09b7..d1e6e71818 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -89,7 +89,7 @@ void targetConfiguration(void) failsafeConfigMutable()->failsafe_delay = 5; failsafeConfigMutable()->failsafe_recovery_delay = 5; failsafeConfigMutable()->failsafe_off_delay = 200; - failsafeConfigMutable()->failsafe_throttle = 1200; + currentBatteryProfile->failsafe_throttle = 1200; failsafeConfigMutable()->failsafe_procedure = FAILSAFE_PROCEDURE_RTH; boardAlignmentMutable()->rollDeciDegrees = 0; @@ -120,7 +120,7 @@ void targetConfiguration(void) navConfigMutable()->general.rth_altitude = 1000; navConfigMutable()->mc.max_bank_angle = 30; - navConfigMutable()->mc.hover_throttle = 1500; + currentBatteryProfile->nav.mc.hover_throttle = 1500; navConfigMutable()->mc.auto_disarm_delay = 2000; /* @@ -192,4 +192,6 @@ void targetConfiguration(void) ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = 0; ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = 10; ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = 1600; + + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h index cdb8ba4636..5d02d9c744 100755 --- a/src/main/target/FALCORE/target.h +++ b/src/main/target/FALCORE/target.h @@ -25,7 +25,6 @@ #define USE_HARDWARE_PREBOOT_SETUP // FALCORE board requires some hardware to be set up before booting and detecting sensors #define BEEPER PB4 -#define BEEPER_PWM #define BEEPER_PWM_FREQUENCY 2700 #define MPU6500_CS_PIN PC0 diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index a916b186d7..a415ae541a 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -48,7 +48,6 @@ #define BMP280_CS_PIN PC5 #define BMP280_SPI_BUS BUS_SPI3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_CS_PIN PA4 #define MAX7456_SPI_BUS BUS_SPI1 diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 669f4bd440..00c154dd94 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -58,7 +58,6 @@ /*---------------------------------*/ /*-------------OSD-----------------*/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB3 diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 59337ad53b..244ddec53b 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -73,7 +73,6 @@ #if defined(FF_PIKOF4OSD) /*-------------OSD-----------------*/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA4 diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index a0ea4428da..51ea748abc 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -175,7 +175,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 45b8b5b865..331de0d918 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -78,7 +78,6 @@ #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 34d110d7a5..f949cbf4f6 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -88,7 +88,6 @@ #define USE_MAG_LIS3MDL // *************** SPI OSD ***************************** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h index ddf5690b2d..c129a87c0b 100644 --- a/src/main/target/FLYWOOF745/target.h +++ b/src/main/target/FLYWOOF745/target.h @@ -92,22 +92,21 @@ #define USE_SPI_DEVICE_2 //OSD #define USE_SPI_DEVICE_4 //ICM20689 -#define SPI1_NSS_PIN PA4// -#define SPI1_SCK_PIN PA5// -#define SPI1_MISO_PIN PA6// -#define SPI1_MOSI_PIN PA7// +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 -#define SPI2_NSS_PIN PB12// -#define SPI2_SCK_PIN PB13/// -#define SPI2_MISO_PIN PB14// -#define SPI2_MOSI_PIN PB15// +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 -#define SPI4_NSS_PIN PE4// -#define SPI4_SCK_PIN PE2// -#define SPI4_MISO_PIN PE5// -#define SPI4_MOSI_PIN PE6// +#define SPI4_NSS_PIN PE4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 @@ -128,13 +127,28 @@ #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 #define I2C2_SDA PB11 + +//External I2C bus is different than internal one +#define MAG_I2C_BUS BUS_I2C2 +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C2 +#define DEFAULT_I2C_BUS BUS_I2C2 + +#else + +//External I2C bus is the same as interal one +#define MAG_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define DEFAULT_I2C_BUS BUS_I2C1 + #endif + #define USE_BARO #define USE_BARO_BMP280 #define BARO_I2C_BUS BUS_I2C1 #define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_MAG3110 @@ -142,14 +156,10 @@ #define USE_MAG_IST8308 #define USE_MAG_LIS3MDL -#define TEMPERATURE_I2C_BUS BUS_I2C1 - -#define RANGEFINDER_I2C_BUS BUS_I2C1 - #define USE_ADC -#define ADC_CHANNEL_1_PIN PC2// -#define ADC_CHANNEL_2_PIN PC3// -#define ADC_CHANNEL_3_PIN PC5// +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC3 +#define ADC_CHANNEL_3_PIN PC5 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 diff --git a/src/main/target/FLYWOOF7DUAL/target.h b/src/main/target/FLYWOOF7DUAL/target.h index 10b8d285bb..8cae941538 100644 --- a/src/main/target/FLYWOOF7DUAL/target.h +++ b/src/main/target/FLYWOOF7DUAL/target.h @@ -73,7 +73,6 @@ #define M25P16_SPI_BUS BUS_SPI3 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_CS_PIN PB12 #define MAX7456_SPI_BUS BUS_SPI2 diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h index 2b8d4e2ac0..2b9f5ae7a0 100644 --- a/src/main/target/FOXEERF405/target.h +++ b/src/main/target/FOXEERF405/target.h @@ -72,7 +72,6 @@ #define M25P16_SPI_BUS BUS_SPI2 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -147,4 +146,5 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h index b339cef7a9..f946150985 100644 --- a/src/main/target/FOXEERF722DUAL/target.h +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -81,7 +81,6 @@ #define M25P16_SPI_BUS BUS_SPI2 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PC3 @@ -158,4 +157,5 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index b046c399cb..304fa22214 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -56,7 +56,6 @@ #define I2C1_SDA PB7 #define USE_SPI -#define USE_OSD // include the max7456 driver #define USE_MAX7456 diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index 4d950a18a7..9b6cee6b39 100755 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -37,7 +37,6 @@ #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/FRSKYPILOT/config.c b/src/main/target/FRSKYPILOT/config.c index 05ec1b5a85..60f29d3884 100644 --- a/src/main/target/FRSKYPILOT/config.c +++ b/src/main/target/FRSKYPILOT/config.c @@ -22,6 +22,7 @@ #include "io/serial.h" #include "rx/rx.h" +#include "fc/config.h" void targetConfiguration(void) { @@ -29,4 +30,6 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_FRSKY_OSD; rxConfigMutable()->serialrx_inverted = 1; + + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h index d4da84dde0..539b465461 100644 --- a/src/main/target/FRSKYPILOT/target.h +++ b/src/main/target/FRSKYPILOT/target.h @@ -29,7 +29,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define BEEPER_PWM #define BEEPER_PWM_FREQUENCY 4000 #define USE_SPI @@ -152,7 +151,6 @@ #define UART5_AF 1 // OSD -#define USE_OSD #define USE_UART6 #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 diff --git a/src/main/target/FRSKY_ROVERF7/target.h b/src/main/target/FRSKY_ROVERF7/target.h index 6347cd9bd9..f729d2bd60 100755 --- a/src/main/target/FRSKY_ROVERF7/target.h +++ b/src/main/target/FRSKY_ROVERF7/target.h @@ -87,7 +87,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PD2 diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index d5ccf15a95..5b7b75dd5b 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -67,7 +67,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h index 440a657c1b..a2453b1e61 100644 --- a/src/main/target/HGLRCF722/target.h +++ b/src/main/target/HGLRCF722/target.h @@ -92,7 +92,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -177,4 +176,5 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT #define USE_SERIALSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR + diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h index aee436548c..7d603cc116 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h @@ -49,7 +49,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index ab5d0bf5f3..5a3943e11a 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -87,7 +87,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c index 83090b5c94..fb1d9804fd 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.c +++ b/src/main/target/IFLIGHTF7_TWING/target.c @@ -37,11 +37,11 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index 2fb765bce7..fb74f2d530 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -100,7 +100,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 2b0f908a91..28e94d1e7f 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -47,6 +47,10 @@ #define MPU6500_CS_PIN PC4 #define MPU6500_SPI_BUS BUS_SPI1 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_BUS BUS_SPI1 #ifdef KAKUTEF4V2 # define USE_I2C @@ -75,7 +79,6 @@ # define USE_MAG #endif -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PB14 diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 0d42c7e2d6..97764f6e92 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -104,7 +104,6 @@ #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 -#define USE_OSD #ifndef KAKUTEF7HDV #define USE_MAX7456 @@ -179,4 +178,5 @@ #define MAX_PWM_OUTPUT_PORTS 6 -#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file +#define BNO055_I2C_BUS BUS_I2C1 + diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index 7ecc7a3d1e..6e7188f9f7 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -106,7 +106,6 @@ /* * OSD */ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 @@ -173,4 +172,5 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) \ No newline at end of file +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index 4f5eac5f7c..cde87e45d2 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -61,7 +61,6 @@ #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PC13 -#define USE_OSD #ifdef USE_MSP_DISPLAYPORT #undef USE_MSP_DISPLAYPORT #endif diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index adb4c09f1e..ae1eb7bb33 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -57,7 +57,7 @@ #define I2C2_SDA PB9 #define DEFAULT_I2C_BUS BUS_I2C1 -#else +#else #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 // SCL pad TX3 @@ -158,7 +158,6 @@ #define VBAT_SCALE_DEFAULT 1100 // ******* OSD ******** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -187,8 +186,8 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 8 -#define TARGET_MOTOR_COUNT 4 +#define MAX_PWM_OUTPUT_PORTS 4 +#define TARGET_MOTOR_COUNT 4 // ESC-related features #define USE_DSHOT @@ -199,4 +198,4 @@ #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS -#define BNO055_I2C_BUS DEFAULT_I2C_BUS \ No newline at end of file +#define BNO055_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 2c1cb07cf7..3efdde2835 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -156,7 +156,6 @@ #define VBAT_SCALE_DEFAULT 1100 // ******* OSD ******** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN @@ -207,4 +206,5 @@ #define BNO055_I2C_BUS DEFAULT_I2C_BUS -#endif \ No newline at end of file +#endif + diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index a70cc55583..c3dd007b4d 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -76,7 +76,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB10 diff --git a/src/main/target/MATEKF405CAN/config.c b/src/main/target/MATEKF405CAN/config.c index 34166b5512..b400704b00 100644 --- a/src/main/target/MATEKF405CAN/config.c +++ b/src/main/target/MATEKF405CAN/config.c @@ -21,6 +21,7 @@ #include "config/feature.h" #include "io/serial.h" #include "sensors/compass.h" +#include "fc/config.h" void targetConfiguration(void) @@ -28,5 +29,5 @@ void targetConfiguration(void) compassConfigMutable()->mag_align = CW0_DEG_FLIP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS; // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200; - + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h index b9dde1e434..b321826dd4 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -25,7 +25,6 @@ #define BEEPER PA8 #define BEEPER_INVERTED -#define BEEPER_PWM #define BEEPER_PWM_FREQUENCY 2500 // *************** SPI1 Gyro & ACC ******************* diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 75afdea322..805f6cea42 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -85,7 +85,6 @@ #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -#define USE_OSD #define USE_MAX7456 #define MAX7456_CS_PIN PB12 #define MAX7456_SPI_BUS BUS_SPI2 diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index ac6962a350..b73c6013c7 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -55,7 +55,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index 2da0393f54..8bb3e933ef 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -50,7 +50,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 331ad65821..b1b657e424 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -90,7 +90,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB10 diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index ca9dd843a6..83d7d669cb 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -169,7 +169,6 @@ #define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 10 -#define USE_OSD #define USE_DSHOT #define USE_SERIALSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index ecff68fa66..6f62d53406 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -95,7 +95,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -195,4 +194,5 @@ #define USE_SERIALSHOT #define USE_ESC_SENSOR -#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file +#define BNO055_I2C_BUS BUS_I2C1 + diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index 8f5b256026..bb2d33d094 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -98,7 +98,6 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 @@ -132,6 +131,11 @@ #define UART4_TX_PIN PD1 #define UART4_RX_PIN PD0 +#define USE_UART5 +#define UART5_TX_PIN NONE +#define UART5_RX_PIN PB8 +#define UART5_AF 7 + #define USE_UART6 #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 @@ -150,7 +154,7 @@ #define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad -#define SERIAL_PORT_COUNT 9 +#define SERIAL_PORT_COUNT 10 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MATEKH743/CMakeLists.txt b/src/main/target/MATEKH743/CMakeLists.txt index 40b0996e17..96a65ca5a4 100644 --- a/src/main/target/MATEKH743/CMakeLists.txt +++ b/src/main/target/MATEKH743/CMakeLists.txt @@ -1 +1 @@ -target_stm32h743xi(MATEKH743 SKIP_RELEASES) +target_stm32h743xi(MATEKH743) diff --git a/src/main/target/MATEKH743/config.c b/src/main/target/MATEKH743/config.c new file mode 100644 index 0000000000..1065971614 --- /dev/null +++ b/src/main/target/MATEKH743/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 5a77e56bcf..17288f9393 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -30,21 +30,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1 BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500, DEVHW_MPU6500, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN); const timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S12 + DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index ca99c9ddcc..84449f9f33 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -21,48 +21,14 @@ #define TARGET_BOARD_IDENTIFIER "H743" #define USBD_PRODUCT_STRING "MATEKH743" +#define USE_TARGET_CONFIG + #define LED0 PE3 #define LED1 PE4 #define BEEPER PA15 #define BEEPER_INVERTED - -// *************** UART ***************************** -#define USE_VCP - -#define USE_UART1 -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define USE_UART2 -#define UART2_TX_PIN PD5 -#define UART2_RX_PIN PD6 - -#define USE_UART3 -#define UART3_TX_PIN PD8 -#define UART3_RX_PIN PD9 - -#define USE_UART4 -#define UART4_TX_PIN PD1 -#define UART4_RX_PIN PD0 - -#define USE_UART6 -#define UART6_TX_PIN PC6 -#define UART6_RX_PIN PC7 - -#define USE_UART7 -#define UART7_TX_PIN PE8 -#define UART7_RX_PIN PE7 - -#define USE_UART8 -#define UART8_TX_PIN PE1 -#define UART8_RX_PIN PE0 - -#define SERIAL_PORT_COUNT 8 - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART6 +#define BEEPER_PWM_FREQUENCY 2500 // *************** IMU generic *********************** #define USE_DUAL_GYRO @@ -79,32 +45,11 @@ #define SPI1_MOSI_PIN PD7 #define USE_IMU_MPU6000 -#define USE_IMU_MPU6500 -#define IMU1_ALIGN CW90_DEG_FLIP +#define IMU1_ALIGN CW0_DEG_FLIP #define IMU1_SPI_BUS BUS_SPI1 #define IMU1_CS_PIN PC15 -#define IMU1_EXTI_PIN NONE - -// *************** SPI2 OSD *********************** -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_OSD -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI2 -#define MAX7456_CS_PIN PB12 - -// *************** SPI3 SD BLACKBOX******************* -/* -#define USE_SDCARD -#define USE_SDCARD_SDIO -#define SDCARD_SDIO_DMA DMA_TAG(2,3,4) -#define SDCARD_SDIO_4BIT -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -*/ +#define IMU1_EXTI_PIN PB2 // *************** SPI4 IMU2 ************************* #define USE_SPI_DEVICE_4 @@ -112,10 +57,33 @@ #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 -#define IMU2_ALIGN CW270_DEG_FLIP +#define USE_IMU_MPU6500 + +#define IMU2_ALIGN CW0_DEG_FLIP #define IMU2_SPI_BUS BUS_SPI4 #define IMU2_CS_PIN PE11 -#define IMU2_EXTI_PIN NONE +#define IMU2_EXTI_PIN PE15 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 SPARE for external RM3100 *********** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_MAG_RM3100 +#define RM3100_CS_PIN PE2 //CS2 pad +// PD4 //CS1 pad +#define RM3100_SPI_BUS BUS_SPI3 // *************** I2C /Baro/Mag ********************* #define USE_I2C @@ -147,7 +115,55 @@ #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C2 +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad +#define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad + +#define SERIAL_PORT_COUNT 9 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** SDIO SD BLACKBOX******************* +//#define USE_SDCARD +//#define USE_SDCARD_SDIO +//#define SDCARD_SDIO_DMA DMA_TAG(2,3,4) +//#define SDCARD_SDIO_4BIT +//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT // *************** ADC ***************************** #define USE_ADC @@ -168,8 +184,8 @@ // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX -#define PINIO1_PIN PE4 // VTX power switcher -#define PINIO2_PIN PE15 // 2xCamera switcher +#define PINIO1_PIN PD10 // VTX power switcher +#define PINIO2_PIN PD11 // 2xCamera switcher // *************** LEDSTRIP ************************ #define USE_LED_STRIP @@ -189,4 +205,5 @@ #define MAX_PWM_OUTPUT_PORTS 15 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT \ No newline at end of file +#define USE_SERIALSHOT + diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h index 9a40ea5050..764e889bb4 100755 --- a/src/main/target/NOX/target.h +++ b/src/main/target/NOX/target.h @@ -61,7 +61,6 @@ #define BMP280_CS_PIN PA9 // *************** SPI OSD ***************************** -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PA10 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 3338db998e..273f41097f 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -103,7 +103,6 @@ #define SDCARD_SPI_BUS BUS_SPI2 #define SDCARD_CS_PIN SPI2_NSS_PIN -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PB1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 4027144c8d..fd0991fb94 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -213,7 +213,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 50e10578b4..8296e531e5 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -126,7 +126,6 @@ #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index 241322b5d6..de7da72927 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -128,7 +128,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h index cdae1860b3..1a701ed987 100644 --- a/src/main/target/RUSH_BLADE_F7/target.h +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -141,7 +141,6 @@ #define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 10 -#define USE_OSD #define USE_DSHOT #define USE_SERIALSHOT #define USE_ESC_SENSOR diff --git a/src/main/target/SKYSTARSF405HD/CMakeLists.txt b/src/main/target/SKYSTARSF405HD/CMakeLists.txt new file mode 100644 index 0000000000..cc9515d9e0 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SKYSTARSF405HD) diff --git a/src/main/target/SKYSTARSF405HD/config.c b/src/main/target/SKYSTARSF405HD/config.c new file mode 100644 index 0000000000..6dd129e144 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of iNav. + * + * iNav is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * iNav is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with iNav. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_DJI_HD_OSD; +} diff --git a/src/main/target/SKYSTARSF405HD/target.c b/src/main/target/SKYSTARSF405HD/target.c new file mode 100644 index 0000000000..ecaeed2621 --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/target.c @@ -0,0 +1,35 @@ +/* + * This file is part of iNav. + * + * iNav is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * iNav is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with iNav. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_PPM, 0, 0), + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h new file mode 100644 index 0000000000..d6c562aa0b --- /dev/null +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -0,0 +1,166 @@ +/* + * This file is part of iNav. + * + * iNav is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * iNav is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with iNav. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SS4D" + +#define USBD_PRODUCT_STRING "SkystarsF405HD" + +#define LED0 PC14 // green +#define LED1 PC15 // blue + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG_FLIP + +// *************** M25P256 flash ******************** +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** OSD & baro *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +#define USE_BARO +#define USE_BARO_BMP280 +#define BMP280_SPI_BUS BUS_SPI2 +#define BMP280_CS_PIN PC5 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// *************** I2C **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define PCA9685_I2C_BUS DEFAULT_I2C_BUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define USE_LED_STRIP +#define WS2811_PIN PC8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIALSHOT + +#define MAX_PWM_OUTPUT_PORTS 4 diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 3920653df7..0258ac2974 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -73,7 +73,6 @@ #define USE_FLASH_M25P16 /*** OSD ***/ -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB10 @@ -171,4 +170,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define RANGEFINDER_I2C_BUS BUS_I2C1 \ No newline at end of file +#define RANGEFINDER_I2C_BUS BUS_I2C1 + diff --git a/src/main/target/SPEEDYBEEF7/target.h b/src/main/target/SPEEDYBEEF7/target.h index 71e855af0a..559ad0924e 100644 --- a/src/main/target/SPEEDYBEEF7/target.h +++ b/src/main/target/SPEEDYBEEF7/target.h @@ -73,7 +73,6 @@ // OSD -- SPI2 #define USE_SPI_DEVICE_2 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index efe89add1e..8722883d01 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -143,7 +143,6 @@ // PC4 - NC - Free for ADC12_IN14 / VTX CS // PC5 - NC - Free for ADC12_IN15 / VTX Enable / OSD VSYNC -//#define USE_OSD //#define USE_MAX7456 //#define USE_OSD_OVER_MSP_DISPLAYPORT diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index 9bbc2a7455..c41c61732a 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -168,8 +168,6 @@ #define CURRENT_METER_SCALE 300 -#define USE_OSD - #define USE_LED_STRIP #define WS2811_PIN PA1 diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index 5ac92a8358..0e82c20e93 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -59,4 +59,5 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_LEVEL].I = 10; pidProfileMutable()->bank_mc.pid[PID_LEVEL].D = 75; + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 741f224252..69c5bc3ab1 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -40,7 +40,6 @@ #if defined(YUPIF4MINI) // #define BEEPER_INVERTED #else -#define BEEPER_PWM #define BEEPER_INVERTED #define BEEPER_PWM_FREQUENCY 3150 #endif @@ -74,7 +73,6 @@ #define USE_BARO_MS5611 #define USE_BARO_BMP280 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA14 diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/YUPIF7/config.c index e61e532d56..9d5077edba 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/YUPIF7/config.c @@ -59,4 +59,5 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PID_LEVEL].I = 10; pidProfileMutable()->bank_mc.pid[PID_LEVEL].D = 75; + beeperConfigMutable()->pwmMode = true; } diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index a95336a738..6123af03ab 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -25,7 +25,6 @@ #define CAMERA_CONTROL_PIN PB7 #define BEEPER PB14 -#define BEEPER_PWM #define BEEPER_INVERTED #define BEEPER_PWM_FREQUENCY 3150 @@ -115,7 +114,6 @@ #define I2C_DEVICE (I2CDEV_1) // OSD -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI1 #define MAX7456_CS_PIN PA14 diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 4558922417..5ad4f85e6e 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -140,7 +140,6 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PB5 -#define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 diff --git a/src/main/target/common.h b/src/main/target/common.h index 068e6b8dde..c110a6112f 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -127,6 +127,7 @@ #define USE_PWM_DRIVER_PCA9685 +#define USE_OSD #define USE_FRSKYOSD #define USE_DJI_HD_OSD #define USE_SMARTPORT_MASTER diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 1091319057..d7603c9e6f 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -61,9 +61,16 @@ extern uint8_t __config_end; #undef USE_PWM_SERVO_DRIVER #endif +#ifndef BEEPER_PWM_FREQUENCY +#define BEEPER_PWM_FREQUENCY 2500 +#endif + +#define USE_ARM_MATH // try to use FPU functions + #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST) // This feature uses 'arm_math.h', which does not exist for x86. #undef USE_DYNAMIC_FILTERS +#undef USE_ARM_MATH #endif //Defines for compiler optimizations diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index b551f6b9d1..0bc65d3457 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -193,10 +193,10 @@ static void sendGpsAltitude(void) static void sendThrottleOrBatterySizeAsRpm(void) { - uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); + uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); @@ -265,11 +265,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 min = absgps / GPS_DEGREES_DIVIDER; // minutes left - if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { - result->dddmm = deg * 100 + min; - } else { - result->dddmm = deg * 60 + min; - } + result->dddmm = deg * ((FRSKY_FORMAT_DMS == telemetryConfig()->frsky_coordinate_format) ? (100) : (60)) + min; result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000; } diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index c2aca3ac51..85f6f85b96 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -147,10 +147,12 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) sbufWriteU16(dst, GPS_directionToHome); uint8_t gpsFlags = 0; - if(STATE(GPS_FIX)) + if (STATE(GPS_FIX)) { gpsFlags |= GPS_FLAGS_FIX; - if(STATE(GPS_FIX_HOME)) + } + if (STATE(GPS_FIX_HOME)) { gpsFlags |= GPS_FLAGS_FIX_HOME; + } sbufWriteU8(dst, gpsFlags); } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 06332a166c..2eb3723950 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -89,6 +89,7 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" +#define MAVLINK_COMM_NUM_BUFFERS 1 #include "common/mavlink.h" #pragma GCC diagnostic pop @@ -326,7 +327,14 @@ void checkMAVLinkTelemetryState(void) static void mavlinkSendMessage(void) { uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - if (telemetryConfig()->mavlink.version == 1) mavSendMsg.magic = MAVLINK_STX_MAVLINK1; + + mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0); + if (telemetryConfig()->mavlink.version == 1) { + chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + chan_state->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } + int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); for (int i = 0; i < msgLength; i++) { @@ -502,8 +510,9 @@ void mavlinkSendRCChannelsAndRSSI(void) GET_CHANNEL_VALUE(6), // chan8_raw RC channel 8 value, in microseconds GET_CHANNEL_VALUE(7), - // rssi Receive signal strength indicator, 0: 0%, 255: 100% - scaleRange(getRSSI(), 0, 1023, 0, 255)); + // rssi Receive signal strength indicator, 0: 0%, 254: 100% + //https://github.com/mavlink/mavlink/issues/1027 + scaleRange(getRSSI(), 0, 1023, 0, 254)); #undef GET_CHANNEL_VALUE mavlinkSendMessage(); diff --git a/src/utils/settings.rb b/src/utils/settings.rb index 877e325fcd..e0b7a75898 100755 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -907,8 +907,8 @@ class Generator unsigned = !$~[:unsigned].empty? bitsize = $~[:bitsize].to_i type_range = unsigned ? 0..(2**bitsize-1) : (-2**(bitsize-1)+1)..(2**(bitsize-1)-1) - min = type_range.min if min =~ /\AU?INT\d+_MIN\Z/ - max = type_range.max if max =~ /\AU?INT\d+_MAX\Z/ + min = type_range.min if min.to_s =~ /\AU?INT\d+_MIN\Z/ + max = type_range.max if max.to_s =~ /\AU?INT\d+_MAX\Z/ raise "Member #{name} default value has an invalid type, integer or symbol expected" unless default_value.is_a? Integer or default_value.is_a? Symbol raise "Member #{name} default value is outside type's storage range, min #{type_range.min}, max #{type_range.max}" unless default_value.is_a? Symbol or type_range === default_value raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index f685d908a8..0c72c6a759 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -38,7 +38,7 @@ def parse_settings_yaml(): with open(SETTINGS_YAML_PATH, "r") as settings_yaml: return yaml.load(settings_yaml, Loader=yaml.Loader) -def generate_md_table_from_yaml(settings_yaml): +def generate_md_from_yaml(settings_yaml): """Generate a sorted markdown table with description & default value for each setting""" params = {} @@ -84,20 +84,19 @@ def generate_md_table_from_yaml(settings_yaml): "max": member["max"] if "max" in member else "" } - # MD table header - md_table_lines = [ - "| Variable Name | Default Value | Min | Max | Description |\n", - "| ------------- | ------------- | --- | --- | ----------- |\n", - ] - - # Sort the settings by name and build the rows of the table + # Sort the settings by name and build the doc + output_lines = [] for param in sorted(params.items()): - md_table_lines.append("| {} | {} | {} | {} | {} |\n".format( - param[0], param[1]['default'], param[1]['min'], param[1]['max'], param[1]['description'] - )) + output_lines.extend([ + f"### {param[0]}\n\n", + f"{param[1]['description'] if param[1]['description'] else '_// TODO_'}\n\n", + "| Default | Min | Max |\n| --- | --- | --- |\n", + f"| {param[1]['default']} | {param[1]['min']} | {param[1]['max']} |\n\n", + "---\n\n" + ]) - # Return the assembled table - return md_table_lines + # Return the assembled doc body + return output_lines def write_settings_md(lines): """Write the contents of the CLI settings docs""" @@ -185,9 +184,9 @@ if __name__ == "__main__": defaults_match = check_defaults(settings_yaml) quit(0 if defaults_match else 1) - md_table_lines = generate_md_table_from_yaml(settings_yaml) - settings_md_lines = \ - ["# CLI Variable Reference\n", "\n" ] + \ - md_table_lines + \ - ["\n", "> Note: this table is autogenerated. Do not edit it manually."] - write_settings_md(settings_md_lines) + output_lines = generate_md_from_yaml(settings_yaml) + output_lines = [ + "# CLI Variable Reference\n\n", + "> Note: this document is autogenerated. Do not edit it manually.\n\n" + ] + output_lines + write_settings_md(output_lines)