From ca4dd4fda9aee08d7e95e2fb301618c668519e13 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 18 Apr 2021 21:42:41 +0100 Subject: [PATCH 001/107] Initial build --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 6 ++++++ src/main/io/osd.c | 25 ++++++++++++++++++------- src/main/io/osd.h | 1 + 4 files changed, 26 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 537f8239e3..72f7d03b44 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -428,6 +428,7 @@ | osd_sidebar_scroll_arrows | OFF | | | osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | +| osd_system_msg_display_time | 1000 | System message display cycle time for multiple messages (milliseconds). | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | IMPERIAL, METRIC, UK | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 95b3df1bfc..9967e32285 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3251,6 +3251,12 @@ groups: min: -36 max: 36 + - name: osd_system_msg_display_time + description: System message display cycle time for multiple messages (milliseconds). + field: system_msg_display_time + default_value: 1000 + min: 500 + max: 5000 - name: PG_SYSTEM_CONFIG type: systemConfig_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 95f85d2e84..eea2a6d580 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2751,7 +2751,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, - + .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, @@ -3560,6 +3560,18 @@ displayCanvas_t *osdGetDisplayPortCanvas(void) return NULL; } +timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages){ + uint8_t i = 0; + float factor = 1.0f; + while (i < messageCount) { + if ((float)strlen(messages[i]) / 15.0f > factor) { + factor = (float)strlen(messages[i]) / 15.0f; + } + i++; + } + return osdConfig()->system_msg_display_time * factor; +} + textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText) { textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; @@ -3594,7 +3606,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } #endif if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; if (message == failsafeInfoMessage) { // failsafeInfoMessage is not useful for recovering // a lost model, but might help avoiding a crash. @@ -3657,17 +3669,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); } } - // Pick one of the available messages. Each message lasts - // a second. + // Pick one of the available messages. Message duration variable based on longest message length if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; + message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), 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) { + if (OSD_ALTERNATING_CHOICES(osdConfig()->system_msg_display_time, 2) == 0) { const setting_t *setting = settingGet(invalidIndex); settingGetName(setting, messageBuf); for (int ii = 0; messageBuf[ii]; ii++) { @@ -3679,7 +3690,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } } else { - if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + if (OSD_ALTERNATING_CHOICES(osdConfig()->system_msg_display_time, 2) == 0) { message = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } else { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 89469b08a8..650e118788 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -362,6 +362,7 @@ typedef struct osdConfig_s { uint8_t pan_servo_index; // Index of the pan servo used for home direction offset int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) } osdConfig_t; From ff005cdbd9bb01a6a7da83f43077dff0aa38632f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 22 Apr 2021 23:26:01 +0100 Subject: [PATCH 002/107] Update Settings.md --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index a7025525ba..5933baa489 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -428,8 +428,8 @@ | osd_sidebar_scroll_arrows | OFF | | | osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | -| osd_system_msg_display_time | 1000 | System message display cycle time for multiple messages (milliseconds). | | osd_stats_min_voltage_unit | BATTERY | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. | +| osd_system_msg_display_time | 1000 | System message display cycle time for multiple messages (milliseconds). | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | IMPERIAL, METRIC, UK | From f41ed8a7eeb77bb7182503593f4479ece5bb8bb9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 24 Apr 2021 20:20:31 +0100 Subject: [PATCH 003/107] Update Settings.md --- docs/Settings.md | 1076 +++++++++++++++++++++++----------------------- 1 file changed, 538 insertions(+), 538 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5933baa489..f938492f3d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1,542 +1,542 @@ # CLI Variable Reference -| Variable Name | Default Value | Description | -| ------------- | ------------- | ----------- | -| 3d_deadband_high | 1514 | 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 | 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 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. | -| 3d_neutral | 1460 | Neutral (stop) throttle value for 3D mode | -| acc_event_threshold_high | 0 | 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 | 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 | 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 | 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 | | -| acc_notch_hz | 0 | | -| accgain_x | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| accgain_y | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | -| acczero_x | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| acczero_y | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | -| airmode_throttle_threshold | 1300 | 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 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 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_roll | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_yaw | 0 | 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 | Same as align_mag_roll, but for the pitch axis. | -| align_mag_roll | 0 | 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 | 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 | Defines the deadband of throttle during alt_hold [r/c points] | -| antigravity_accelerator | 1 | | -| antigravity_cutoff_lpf_hz | 15 | 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 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements | -| applied_defaults | 0 | Internal (configurator) hint. Should not be changed manually | -| baro_cal_tolerance | 150 | 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 | 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 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | -| battery_capacity_critical | 0 | 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 | 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 | Blackbox logging rate denominator. See blackbox_rate_num. | -| blackbox_rate_num | 1 | 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 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | -| current_adc_channel | _target default_ | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | -| current_meter_offset | _target default_ | This sets the output offset voltage of the current sensor in millivolts. | -| current_meter_scale | _target default_ | 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 | | -| d_boost_gyro_delta_lpf_hz | 80 | | -| d_boost_max_at_acceleration | 7500 | | -| deadband | 5 | 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_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | -| 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 | 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 | Sets the DShot beeper tone | -| dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | -| dterm_lpf2_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | -| 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 | 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 | 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 | | -| eleres_loc_delay | 240 | | -| eleres_loc_en | OFF | | -| eleres_loc_power | 7 | | -| eleres_signature | 0 | | -| eleres_telemetry_en | OFF | | -| eleres_telemetry_power | 7 | | -| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | -| failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | -| failsafe_fw_pitch_angle | 100 | 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 | 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 | 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 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | -| failsafe_lights_flash_period | 1000 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | -| failsafe_min_distance | 0 | 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 | 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 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | -| failsafe_stick_threshold | 50 | 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 | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_throttle_low_delay | 0 | 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 | 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. | -| flip_over_after_crash_power_factor | 65 | flip over after crash power factor | -| fpv_mix_degrees | 0 | | -| frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | -| frsky_default_latitude | 0 | 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 | 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 | 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 | 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 | FF to P gain (strength relationship) [%] | -| fw_autotune_overshoot_time | 100 | Time [ms] to detect sustained overshoot | -| fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection | -| fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | -| fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | -| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH | -| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL | -| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW | -| fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH | -| fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL | -| fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW | -| fw_i_level | 5 | Fixed-wing attitude stabilisation low-pass filter cutoff | -| fw_i_pitch | 7 | Fixed-wing rate stabilisation I-gain for PITCH | -| fw_i_roll | 7 | Fixed-wing rate stabilisation I-gain for ROLL | -| fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW | -| fw_iterm_limit_stick_position | 0.5 | 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 | 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_trim | 0 | 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 | 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 | Fixed-wing attitude stabilisation P-gain | -| fw_p_pitch | 5 | Fixed-wing rate stabilisation P-gain for PITCH | -| fw_p_roll | 5 | Fixed-wing rate stabilisation P-gain for ROLL | -| fw_p_yaw | 6 | Fixed-wing rate stabilisation P-gain for YAW | -| fw_reference_airspeed | 1500 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | -| fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | -| fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | -| fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | -| fw_yaw_iterm_freeze_bank_angle | 0 | 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 | 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_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | -| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | -| gyro_dyn_lpf_min_hz | 200 | 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_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | -| gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| gyro_notch_cutoff | 1 | | -| gyro_notch_hz | 0 | | -| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | -| gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_to_use | 0 | | -| 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 | 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 | 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 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | -| idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | -| imu2_align_pitch | 0 | Pitch alignment for Secondary IMU. 1/10 of a degree | -| imu2_align_roll | 0 | Roll alignment for Secondary IMU. 1/10 of a degree | -| imu2_align_yaw | 0 | Yaw alignment for Secondary IMU. 1/10 of a degree | -| imu2_gain_acc_x | 0 | Secondary IMU ACC calibration data | -| imu2_gain_acc_y | 0 | Secondary IMU ACC calibration data | -| imu2_gain_acc_z | 0 | Secondary IMU ACC calibration data | -| imu2_gain_mag_x | 0 | Secondary IMU MAG calibration data | -| imu2_gain_mag_y | 0 | Secondary IMU MAG calibration data | -| imu2_gain_mag_z | 0 | Secondary IMU MAG calibration data | -| imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality | -| imu2_radius_acc | 0 | Secondary IMU MAG calibration data | -| imu2_radius_mag | 0 | 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 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | -| imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | -| imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements | -| imu_dcm_ki_mag | 0 | Inertial Measurement Unit KI Gain for compass measurements | -| imu_dcm_kp | 2500 | Inertial Measurement Unit KP Gain for accelerometer measurements | -| imu_dcm_kp_mag | 10000 | 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 | Uncertainty value for barometric sensor [cm] | -| inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | -| inav_max_eph_epv | 1000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | -| inav_max_surface_altitude | 200 | 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 | Weight for accelerometer drift estimation | -| inav_w_xy_flow_p | 1.0 | | -| inav_w_xy_flow_v | 2.0 | | -| inav_w_xy_gps_p | 1.0 | Weight of GPS coordinates in estimated UAV position and speed. | -| inav_w_xy_gps_v | 2.0 | Weight of GPS velocity data in estimated UAV speed | -| inav_w_xy_res_v | 0.5 | Decay coefficient for estimated velocity when GPS reference for position is lost | -| inav_w_xyz_acc_p | 1.0 | | -| inav_w_z_baro_p | 0.35 | Weight of barometer measurements in estimated altitude and climb rate | -| inav_w_z_gps_p | 0.2 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | -| inav_w_z_gps_v | 0.1 | 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 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | -| inav_w_z_surface_p | 3.5 | | -| inav_w_z_surface_v | 6.1 | | -| iterm_windup | 50 | 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 | | -| log_level | ERROR | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | -| log_topics | 0 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | -| looptime | 1000 | 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 | Adjust how long time the Calibration of mag will last. | -| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | -| mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| mag_to_use | 0 | 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 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | -| maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | -| maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | -| magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | -| manual_pitch_rate | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | -| manual_rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | -| manual_rc_yaw_expo | 20 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | -| manual_roll_rate | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | -| manual_yaw_rate | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | -| mavlink_ext_status_rate | 2 | | -| mavlink_extra1_rate | 10 | | -| mavlink_extra2_rate | 2 | | -| mavlink_extra3_rate | 1 | | -| mavlink_pos_rate | 2 | | -| mavlink_rc_chan_rate | 5 | | -| mavlink_version | 2 | Version of MAVLink to use | -| max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | -| max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | -| max_check | 1900 | 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 | 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 | 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 | Multicopter Control Derivative gain for PITCH | -| mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL | -| mc_cd_yaw | 60 | Multicopter Control Derivative gain for YAW | -| mc_d_level | 75 | Multicopter attitude stabilisation HORIZON transition point | -| mc_d_pitch | 23 | Multicopter rate stabilisation D-gain for PITCH | -| mc_d_roll | 23 | Multicopter rate stabilisation D-gain for ROLL | -| mc_d_yaw | 0 | Multicopter rate stabilisation D-gain for YAW | -| mc_i_level | 15 | Multicopter attitude stabilisation low-pass filter cutoff | -| mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH | -| mc_i_roll | 30 | Multicopter rate stabilisation I-gain for ROLL | -| mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW | -| mc_iterm_relax | RP | | -| mc_iterm_relax_cutoff | 15 | | -| mc_p_level | 20 | Multicopter attitude stabilisation P-gain | -| mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH | -| mc_p_roll | 40 | Multicopter rate stabilisation P-gain for ROLL | -| mc_p_yaw | 85 | Multicopter rate stabilisation P-gain for YAW | -| min_check | 1100 | 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 | 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 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | -| moron_threshold | 32 | 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 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | -| motor_decel_time | 0 | 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 | 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 | 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 | 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 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | -| nav_auto_speed | 300 | 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 | 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_bank_angle | 35 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | -| nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error | -| nav_fw_cruise_speed | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | -| nav_fw_cruise_thr | 1400 | 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 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] | -| nav_fw_dive_angle | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_heading_p | 60 | P gain of Heading Hold controller (Fixedwing) | -| nav_fw_land_dive_angle | 2 | 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 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | -| nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | -| nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | -| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | -| nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | -| nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | -| nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | -| nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | -| nav_fw_launch_motor_delay | 500 | Delay between detected launch and launch sequence start and throttling up (ms) | -| nav_fw_launch_spinup_time | 100 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | -| nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | -| nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | -| nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | -| nav_fw_loiter_radius | 7500 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | -| nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | -| nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | -| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | -| nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_i | 2 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_p | 30 | P gain of heading PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_pidsum_limit | 350 | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_xy_d | 8 | 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 | 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 | 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 | D gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_i | 5 | I gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_p | 40 | P gain of altitude PID controller (Fixedwing) | -| nav_fw_yaw_deadband | 0 | 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 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] | -| nav_land_minalt_vspd | 50 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] | -| nav_land_slowdown_maxalt | 2000 | 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 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] | -| nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | -| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | -| nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode | -| nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) | -| nav_mc_bank_angle | 30 | 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 | max angle that MR is allowed to bank in BOOST mode | -| nav_mc_braking_boost_disengage_speed | 100 | BOOST will be disabled when speed goes below this value | -| nav_mc_braking_boost_factor | 100 | acceleration factor for BOOST phase | -| nav_mc_braking_boost_speed_threshold | 150 | BOOST can be enabled when speed is above this value | -| nav_mc_braking_boost_timeout | 750 | how long in ms BOOST phase can happen | -| nav_mc_braking_disengage_speed | 75 | braking is disabled when speed goes below this value | -| nav_mc_braking_speed_threshold | 100 | min speed in cm/s above which braking can happen | -| nav_mc_braking_timeout | 2000 | timeout in ms for braking | -| nav_mc_heading_p | 60 | P gain of Heading Hold controller (Multirotor) | -| nav_mc_hover_thr | 1500 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | -| nav_mc_pos_deceleration_time | 120 | 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 | Expo for PosHold control | -| nav_mc_pos_xy_p | 65 | 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 | P gain of altitude PID controller (Multirotor) | -| nav_mc_vel_xy_d | 100 | 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 | 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 | 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 | 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 | | -| nav_mc_vel_xy_ff | 40 | | -| nav_mc_vel_xy_i | 15 | 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 | 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 | D gain of velocity PID controller | -| nav_mc_vel_z_i | 50 | I gain of velocity PID controller | -| nav_mc_vel_z_p | 100 | P gain of velocity PID controller | -| nav_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 | 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 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | -| nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | -| nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | -| 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 | 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 | 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_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | -| nav_wp_safe_distance | 10000 | 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 | | -| osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) | -| osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) | -| osd_ahi_max_pitch | 20 | 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 | AHI vertical offset from center (pixel OSD only) | -| osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) | -| osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | -| osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | -| osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | -| osd_camera_fov_h | 135 | Horizontal field of view for the camera in degres | -| osd_camera_fov_v | 85 | Vertical field of view for the camera in degres | -| osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | -| osd_coordinate_digits | 9 | | -| osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | -| osd_crsf_lq_format | TYPE1 | To select LQ format | -| osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | -| osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | -| osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_esc_temp_alarm_min | -200 | 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 | Value above which the OSD g force indicator will blink (g) | -| osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | -| osd_gforce_axis_alarm_min | -5 | 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 | 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 | Left and right margins for the hud area | -| osd_hud_margin_v | 3 | Top and bottom margins for the hud area | -| osd_hud_radar_disp | 0 | 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 | 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 | In meters, radar aircrafts further away than this will not be displayed in the hud | -| osd_hud_radar_range_min | 3 | In meters, radar aircrafts closer than this will not be displayed in the hud | -| osd_hud_wp_disp | 0 | 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 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_left_sidebar_scroll | NONE | | -| osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. | -| osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | -| osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | -| osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | -| osd_pan_servo_index | 0 | 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 | 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 | 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 | Same as left_sidebar_scroll_step, but for the right sidebar | -| osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | -| osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | -| osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | -| osd_sidebar_scroll_arrows | OFF | | -| osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | -| osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | -| osd_stats_min_voltage_unit | BATTERY | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. | -| osd_system_msg_display_time | 1000 | System message display cycle time for multiple messages (milliseconds). | -| osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | -| osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | -| 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 | 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 | 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` | Mode assignment for PINIO#1 | -| pinio_box2 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pinio_box3 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pinio_box4 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | -| pitch_rate | 20 | 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 | | -| pitot_scale | 1.0 | | -| 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 | Stick deadband in [r/c points], applied after r/c deadband and expo | -| prearm_timeout | 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 | 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 | 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 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | -| rc_filter_frequency | 50 | 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 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | -| reboot_character | 82 | 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 | 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 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | -| rpm_gyro_min_hz | 100 | 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 | 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 channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | -| rssi_channel | 0 | RX channel containing the RSSI signal | -| rssi_max | 100 | 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 | 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 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | -| rx_max_usec | 2115 | 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 | 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 | | -| rx_spi_protocol | _target default_ | | -| rx_spi_rf_channel_count | 0 | | -| safehome_max_distance | 20000 | 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 | | -| 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_center_pulse | 1500 | Servo midpoint | -| servo_lpf_hz | 20 | 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 | 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 | 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 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | -| setpoint_kalman_w | 4 | 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 | 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` | 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 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | -| small_angle | 25 | 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 | | -| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | 0 = disabled. Used to bind the spektrum satellite to RX | -| srxl2_baud_fast | ON | | -| srxl2_unit_id | 1 | | -| stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | -| stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | -| stats_total_energy | 0 | | -| stats_total_time | 0 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | -| switch_disarm_delay | 250 | 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 | 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 | Throttle exposition value | -| thr_mid | 50 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | -| throttle_idle | 15 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | -| throttle_scale | 1.0 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | -| throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | -| tpa_breakpoint | 1500 | See tpa_rate. | -| tpa_rate | 0 | 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. | -| 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 | 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 channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | -| vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. | -| vbat_max_cell_voltage | 420 | 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 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | -| vbat_scale | _target default_ | 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 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | -| vtx_band | 4 | 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 | 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 | 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_power | 1 | 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 | 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 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | -| yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | +| 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_speed_source | GROUND | | | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | +| 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. | +| flip_over_after_crash_power_factor | 65 | 0 | 100 | flip over after crash power factor | +| 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_overshoot_time | 100 | 50 | 500 | Time [ms] to detect sustained overshoot | +| fw_autotune_threshold | 50 | 0 | 100 | Threshold [%] of max rate to consider overshoot/undershoot detection | +| fw_autotune_undershoot_time | 200 | 50 | 500 | Time [ms] to detect sustained undershoot | +| 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_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_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_lpf_hz | 60 | | 200 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | +| gyro_lpf_type | BIQUAD | | | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | +| gyro_notch_cutoff | 1 | 1 | 500 | | +| gyro_notch_hz | 0 | | 500 | | +| gyro_stage2_lowpass_hz | 0 | 0 | 500 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | +| gyro_stage2_lowpass_type | BIQUAD | | | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| 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 | | | | +| 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_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 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_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 (m/s) | +| 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_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_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_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_system_msg_display_time | 1000 | 500 | 5000 | System message display cycle time for multiple messages (milliseconds). | +| 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 | CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN | CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX | 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 | CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN | CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX | 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_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 | | | | +| 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 | CONTROL_RATE_CONFIG_TPA_MAX | 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. | +| 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 | CONTROL_RATE_CONFIG_YAW_RATE_MIN | CONTROL_RATE_CONFIG_YAW_RATE_MAX | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | > Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file From a2a54c187727d7df68daeb2b0ae27d956b940ba7 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 14 Nov 2021 17:06:00 +0000 Subject: [PATCH 004/107] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 77 ++++++++++++++++++++++------------------ 1 file changed, 42 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index a56b817151..34404a6fe2 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -185,57 +185,64 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXHEADINGHOLD; - if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { - activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; - activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; - } - - if (STATE(ALTITUDE_CONTROL)) { + if (STATE(MULTIROTOR)) { + if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) { + activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; + activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; + } + if (sensors(SENSOR_BARO) && sensors(SENSOR_RANGEFINDER) && sensors(SENSOR_OPFLOW)) { + activeBoxIds[activeBoxIdCount++] = BOXSURFACE; + } activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; } //Camstab mode is enabled always activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; -#ifdef USE_GPS - if (STATE(ALTITUDE_CONTROL) && (sensors(SENSOR_BARO) || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro)))) { + bool readyAltControl = false; + if (STATE(ALTITUDE_CONTROL) && sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; - activeBoxIds[activeBoxIdCount++] = BOXSURFACE; + readyAltControl = true; +#ifdef USE_GPS + } else if ((feature(FEATURE_GPS) && STATE(AIRPLANE) && positionEstimationConfig()->use_gps_no_baro)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; + readyAltControl = true; } - const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; - if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) { - if (!STATE(ROVER) && !STATE(BOAT)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + if (readyAltControl) { + const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; + if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) { + if (!STATE(ROVER) && !STATE(BOAT)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + } + if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; + } } - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; - } - } - if (navReadyMultirotor || navReadyOther) { - activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; - activeBoxIds[activeBoxIdCount++] = BOXNAVWP; - activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; - activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - activeBoxIds[activeBoxIdCount++] = BOXPLANWPMISSION; + if (navReadyMultirotor || navReadyOther) { + activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; + activeBoxIds[activeBoxIdCount++] = BOXNAVWP; + activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; + activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; + activeBoxIds[activeBoxIdCount++] = BOXPLANWPMISSION; - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; - activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; - activeBoxIds[activeBoxIdCount++] = BOXSOARING; + if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; + activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; + activeBoxIds[activeBoxIdCount++] = BOXSOARING; + } } - } #ifdef USE_MR_BRAKING_MODE - if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { - activeBoxIds[activeBoxIdCount++] = BOXBRAKING; + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + activeBoxIds[activeBoxIdCount++] = BOXBRAKING; + } +#endif +#endif // GPS } -#endif - -#endif if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { activeBoxIds[activeBoxIdCount++] = BOXMANUAL; From c73f45976b5d29c7d780da8df74bdc10a0fd4581 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 16:18:23 -0300 Subject: [PATCH 005/107] [gyro.c] --- docs/Settings.md | 40 ++++++++++++++++++++++++++++++ src/main/fc/settings.yaml | 23 +++++++++++++++++ src/main/sensors/gyro.c | 52 +++++++++++++++++++++++++-------------- src/main/sensors/gyro.h | 2 ++ 4 files changed, 98 insertions(+), 19 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d5409bfbb9..99cd64e996 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1602,6 +1602,36 @@ Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro --- +### gyro_zero_x + +Calculated gyro zero calibration of axis X + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### gyro_zero_y + +Calculated gyro zero calibration of axis Y + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + +### gyro_zero_z + +Calculated gyro zero calibration of axis Z + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -32768 | 32767 | + +--- + ### 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 @@ -2102,6 +2132,16 @@ _// TODO_ --- +### init_gyro_cal + +If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup. + +| Default | Min | Max | +| --- | --- | --- | +| ON | | | + +--- + ### 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) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 22dd0335b1..737574ca83 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -309,6 +309,29 @@ groups: condition: USE_GYRO_KALMAN min: 1 max: 16000 + - name: init_gyro_cal + description: "If defined to 'OFF', it will ignore the gyroscope calibration done at each startup. Instead, the gyroscope last calibration from when you calibrated will be used. It also means you don't have to keep the UAV stationary during a startup." + default_value: ON + field: init_gyro_cal_enabled + type: bool + - name: gyro_zero_x + description: "Calculated gyro zero calibration of axis X" + default_value: 0 + field: gyro_zero_cal[X] + min: INT16_MIN + max: INT16_MAX + - name: gyro_zero_y + description: "Calculated gyro zero calibration of axis Y" + default_value: 0 + field: gyro_zero_cal[Y] + min: INT16_MIN + max: INT16_MAX + - name: gyro_zero_z + description: "Calculated gyro zero calibration of axis Z" + default_value: 0 + field: gyro_zero_cal[Z] + min: INT16_MIN + max: INT16_MAX - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e178b0b4fb..58bcd2f758 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -99,7 +99,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -126,6 +126,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT, #endif + .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT, + .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT}, ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -355,27 +357,39 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe { fpVector3_t v; - // Consume gyro reading - v.v[X] = dev->gyroADCRaw[X]; - v.v[Y] = dev->gyroADCRaw[Y]; - v.v[Z] = dev->gyroADCRaw[Z]; + if (gyroConfig()->init_gyro_cal_enabled) { + // Consume gyro reading + v.v[X] = dev->gyroADCRaw[X]; + v.v[Y] = dev->gyroADCRaw[Y]; + v.v[Z] = dev->gyroADCRaw[Z]; - zeroCalibrationAddValueV(gyroCalibration, &v); + zeroCalibrationAddValueV(gyroCalibration, &v); - // Check if calibration is complete after this cycle - if (zeroCalibrationIsCompleteV(gyroCalibration)) { - zeroCalibrationGetZeroV(gyroCalibration, &v); - dev->gyroZero[X] = v.v[X]; - dev->gyroZero[Y] = v.v[Y]; - dev->gyroZero[Z] = v.v[Z]; + // Check if calibration is complete after this cycle + if (zeroCalibrationIsCompleteV(gyroCalibration)) { + zeroCalibrationGetZeroV(gyroCalibration, &v); + dev->gyroZero[X] = v.v[X]; + dev->gyroZero[Y] = v.v[Y]; + dev->gyroZero[Z] = v.v[Z]; + + // store the gyro calibration in the flash memory + gyroConfigMutable()->gyro_zero_cal[X] = dev->gyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = dev->gyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = dev->gyroZero[Z]; + saveConfigAndNotify(); - LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); - schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics - } - else { - dev->gyroZero[X] = 0; - dev->gyroZero[Y] = 0; - dev->gyroZero[Z] = 0; + LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); + schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics + } else { + dev->gyroZero[X] = 0; + dev->gyroZero[Y] = 0; + dev->gyroZero[Z] = 0; + } + } else { + gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended + dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; + dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; + dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5ed1988d1a..154ce13468 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -73,6 +73,8 @@ typedef struct gyroConfig_s { uint16_t kalman_q; uint8_t kalmanEnabled; #endif + bool init_gyro_cal_enabled; + int16_t gyro_zero_cal[XYZ_AXIS_COUNT]; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); From 67a7a66d1230708aabadaaa7d12d4763ce3eaee1 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 17:26:07 -0300 Subject: [PATCH 006/107] change saveConfigAndNotify by writeEEPROM --- src/main/sensors/gyro.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 58bcd2f758..209e43fc0e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -80,7 +80,7 @@ FILE_COMPILE_FOR_SPEED FASTRAM gyro_t gyro; // gyro sensor object -#define MAX_GYRO_COUNT 1 +#define MAX_GYRO_COUNT 1 STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT]; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT]; @@ -376,7 +376,8 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe gyroConfigMutable()->gyro_zero_cal[X] = dev->gyroZero[X]; gyroConfigMutable()->gyro_zero_cal[Y] = dev->gyroZero[Y]; gyroConfigMutable()->gyro_zero_cal[Z] = dev->gyroZero[Z]; - saveConfigAndNotify(); + // save gyro calibration + writeEEPROM(); LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics From 2684e023fcc98b4efc67ddac926ae178c05e0a9a Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 17:31:24 -0300 Subject: [PATCH 007/107] add readEEPROM function --- src/main/sensors/gyro.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 209e43fc0e..63474c3c5f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -378,6 +378,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe gyroConfigMutable()->gyro_zero_cal[Z] = dev->gyroZero[Z]; // save gyro calibration writeEEPROM(); + readEEPROM(); LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics From 472bf57fc6bc3c44dbdaf748b8829a5a56ab9464 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 18:35:07 -0300 Subject: [PATCH 008/107] move gyro storage calibration --- src/main/sensors/gyro.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 63474c3c5f..a884a3f3c5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -371,14 +371,6 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[X] = v.v[X]; dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; - - // store the gyro calibration in the flash memory - gyroConfigMutable()->gyro_zero_cal[X] = dev->gyroZero[X]; - gyroConfigMutable()->gyro_zero_cal[Y] = dev->gyroZero[Y]; - gyroConfigMutable()->gyro_zero_cal[Z] = dev->gyroZero[Z]; - // save gyro calibration - writeEEPROM(); - readEEPROM(); LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics @@ -429,6 +421,13 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC return true; } else { performGyroCalibration(gyroDev, gyroCal); + + // save gyro calibration + gyroConfigMutable()->gyro_zero_cal[X] = gyroDev->gyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = gyroDev->gyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = gyroDev->gyroZero[Z]; + writeEEPROM(); + readEEPROM(); // Reset gyro values to zero to prevent other code from using uncalibrated data gyroADCf[X] = 0.0f; From e2f4f078ee0e3eb331336f7c7f67600420779d5a Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 18:58:17 -0300 Subject: [PATCH 009/107] fix compilation --- src/main/sensors/gyro.c | 16 ++++++++-------- src/test/unit/sensor_gyro_unittest.cc | 1 + 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a884a3f3c5..a50a49ff46 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -371,6 +371,13 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[X] = v.v[X]; dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; + + // save gyro calibration + gyroConfigMutable()->gyro_zero_cal[X] = gyroDev->gyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = gyroDev->gyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = gyroDev->gyroZero[Z]; + writeEEPROM(); + readEEPROM(); LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics @@ -379,7 +386,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Y] = 0; dev->gyroZero[Z] = 0; } - } else { + } else { // skip gyro calibration and use values ​​read from storage gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; @@ -421,13 +428,6 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC return true; } else { performGyroCalibration(gyroDev, gyroCal); - - // save gyro calibration - gyroConfigMutable()->gyro_zero_cal[X] = gyroDev->gyroZero[X]; - gyroConfigMutable()->gyro_zero_cal[Y] = gyroDev->gyroZero[Y]; - gyroConfigMutable()->gyro_zero_cal[Z] = gyroDev->gyroZero[Z]; - writeEEPROM(); - readEEPROM(); // Reset gyro values to zero to prevent other code from using uncalibrated data gyroADCf[X] = 0.0f; diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index f80ecf7db3..855d51671a 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -146,4 +146,5 @@ timeDelta_t getLooptime(void) {return gyro.targetLooptime;} timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} +void saveConfigAndNotify(void) {} } From d5f8f4571047f0d74196c77d1f2180e7a47e8408 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 18:59:21 -0300 Subject: [PATCH 010/107] fix compilation --- src/test/unit/sensor_gyro_unittest.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 855d51671a..fc1202e767 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -146,5 +146,6 @@ timeDelta_t getLooptime(void) {return gyro.targetLooptime;} timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} -void saveConfigAndNotify(void) {} +void writeEEPROM(void) {} +void readEEPROM(void){} } From bb7acaca321921ca065c6db32295d34af9bbb1d3 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 19:17:29 -0300 Subject: [PATCH 011/107] fix compilation --- src/test/unit/sensor_gyro_unittest.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index fc1202e767..2d4896e4a8 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -32,6 +32,7 @@ extern "C" { #include "common/utils.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/logging_codes.h" + #include "fc/config.h" #include "io/beeper.h" #include "scheduler/scheduler.h" #include "sensors/gyro.h" From 9cfc87e95141012d378efecd578a93c91ed6e96a Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 19:47:18 -0300 Subject: [PATCH 012/107] fix compilation --- src/test/unit/flight_imu_unittest.cc | 4 ++++ src/test/unit/sensor_gyro_unittest.cc | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 25e2c438c3..efcf421ffe 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -29,6 +29,8 @@ extern "C" { #include "io/gps.h" + #include "fc/config.h" + #include "flight/imu.h" } @@ -137,4 +139,6 @@ void resetHeadingHoldTarget(int16_t heading) UNUSED(heading); } bool isGPSHeadingValid(void) { return true; } +void readEEPROM(void) {} +void writeEEPROM(void) {} } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 2d4896e4a8..6a741729f6 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -147,6 +147,6 @@ timeDelta_t getLooptime(void) {return gyro.targetLooptime;} timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} +void readEEPROM(void) {} void writeEEPROM(void) {} -void readEEPROM(void){} } From d98f05c6289d68d442688245903a1acd0636f3cb Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 20:02:41 -0300 Subject: [PATCH 013/107] fix compilation --- src/test/unit/flight_imu_unittest.cc | 2 -- src/test/unit/sensor_gyro_unittest.cc | 1 - 2 files changed, 3 deletions(-) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index efcf421ffe..aeb9799680 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -28,8 +28,6 @@ extern "C" { #include "fc/runtime_config.h" #include "io/gps.h" - - #include "fc/config.h" #include "flight/imu.h" } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 6a741729f6..cc8c80f796 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -32,7 +32,6 @@ extern "C" { #include "common/utils.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/logging_codes.h" - #include "fc/config.h" #include "io/beeper.h" #include "scheduler/scheduler.h" #include "sensors/gyro.h" From 8dd81be2dabd39f2290acb67e6dce96672b5b0b3 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 22 Nov 2021 20:23:08 -0300 Subject: [PATCH 014/107] add comment --- src/main/sensors/gyro.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a50a49ff46..a08fe44d9d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -388,6 +388,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe } } else { // skip gyro calibration and use values ​​read from storage gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended + // pass the calibration values dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; From 88e4b2b6af2b101fc9e199c19f2f548f3b1d14b7 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 23 Nov 2021 17:53:42 -0300 Subject: [PATCH 015/107] tentative to fix compilation --- src/test/unit/sensor_gyro_unittest.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index cc8c80f796..f80ecf7db3 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -146,6 +146,4 @@ timeDelta_t getLooptime(void) {return gyro.targetLooptime;} timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} -void readEEPROM(void) {} -void writeEEPROM(void) {} } From dec0ef81177b9a485159d4189c7b6299c52ab83a Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 24 Nov 2021 18:10:00 -0300 Subject: [PATCH 016/107] second tentative to fix compilation error --- src/test/unit/CMakeLists.txt | 2 +- src/test/unit/flight_imu_unittest.cc | 4 +--- src/test/unit/sensor_gyro_unittest.cc | 3 +++ 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index a7a20a5324..2aa9bcfb31 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" - "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" + "drivers/accgyro/accgyro_fake.c" "fc/config.h" "flight/imu.c" "sensors/boardalignment.c" "sensors/gyro.c") set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index aeb9799680..25e2c438c3 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -28,7 +28,7 @@ extern "C" { #include "fc/runtime_config.h" #include "io/gps.h" - + #include "flight/imu.h" } @@ -137,6 +137,4 @@ void resetHeadingHoldTarget(int16_t heading) UNUSED(heading); } bool isGPSHeadingValid(void) { return true; } -void readEEPROM(void) {} -void writeEEPROM(void) {} } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index f80ecf7db3..6a741729f6 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -32,6 +32,7 @@ extern "C" { #include "common/utils.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/logging_codes.h" + #include "fc/config.h" #include "io/beeper.h" #include "scheduler/scheduler.h" #include "sensors/gyro.h" @@ -146,4 +147,6 @@ timeDelta_t getLooptime(void) {return gyro.targetLooptime;} timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} +void readEEPROM(void) {} +void writeEEPROM(void) {} } From 6afe6accd29e15b9c231228aee0b3fa056af53ef Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 24 Nov 2021 18:27:11 -0300 Subject: [PATCH 017/107] third tentative to fix compilation error --- src/test/unit/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 2aa9bcfb31..1ae5038d74 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" - "drivers/accgyro/accgyro_fake.c" "fc/config.h" "flight/imu.c" "sensors/boardalignment.c" + "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" "sensors/gyro.c") set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") @@ -25,7 +25,7 @@ set_property(SOURCE rcdevice_unittest.cc PROPERTY depends set_property(SOURCE sensor_gyro_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" - "drivers/accgyro/accgyro_fake.c" "sensors/gyro.c" "sensors/boardalignment.c") + "drivers/accgyro/accgyro_fake.c" "fc/config.h" "sensors/gyro.c" "sensors/boardalignment.c") set_property(SOURCE telemetry_hott_unittest.cc PROPERTY depends "telemetry/hott.c" "common/gps_conversion.c" "common/string_light.c") From c7a158aa9d2d258a507ce83ac77752cf04280b87 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 24 Nov 2021 18:45:03 -0300 Subject: [PATCH 018/107] fourth tentative to fix compilation error --- src/test/unit/CMakeLists.txt | 2 +- src/test/unit/flight_imu_unittest.cc | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 1ae5038d74..ae62ebe853 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" - "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" + "drivers/accgyro/accgyro_fake.c" "fc/config.h" "flight/imu.c" "sensors/boardalignment.c" "sensors/gyro.c") set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 25e2c438c3..00389e78aa 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -28,6 +28,8 @@ extern "C" { #include "fc/runtime_config.h" #include "io/gps.h" + + #include "fc/config.h" #include "flight/imu.h" } @@ -137,4 +139,6 @@ void resetHeadingHoldTarget(int16_t heading) UNUSED(heading); } bool isGPSHeadingValid(void) { return true; } +void readEEPROM(void) {} +void writeEEPROM(void) {} } From af7e553cf056b3c7ecc5ed6fb2561a3a9f9c6d1d Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 24 Nov 2021 19:06:55 -0300 Subject: [PATCH 019/107] Thursday tentative to fix compilation error --- src/test/unit/flight_imu_unittest.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 00389e78aa..3b43323f5c 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -110,8 +110,6 @@ bool sensors(uint32_t mask) return false; }; uint32_t millis(void) { return 0; } -timeDelta_t getLooptime(void) { return gyro.targetLooptime; } -timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; } void schedulerResetTaskStatistics(cfTaskId_e) {} void sensorsSet(uint32_t) {} bool compassIsHealthy(void) { return true; } From e2400955fe678d54df38bfe0c698959bcb2834bf Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 24 Nov 2021 19:13:17 -0300 Subject: [PATCH 020/107] last attempt to fix compilation error --- src/test/unit/sensor_gyro_unittest.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 6a741729f6..87744b2eb9 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -143,8 +143,6 @@ timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; -timeDelta_t getLooptime(void) {return gyro.targetLooptime;} -timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} void readEEPROM(void) {} From e708327bb9ffa36fdffa1ad2317be3a5cc703d3b Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 24 Nov 2021 19:36:40 -0300 Subject: [PATCH 021/107] int32_t changed to timeDelta_t --- src/main/fc/config.c | 4 ++-- src/main/fc/config.h | 4 ++-- src/test/unit/flight_imu_unittest.cc | 2 ++ src/test/unit/sensor_gyro_unittest.cc | 2 ++ 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 02c8f1c8e6..b5d5a467ee 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -173,11 +173,11 @@ __attribute__((weak)) void targetConfiguration(void) #define SECOND_PORT_INDEX 1 #endif -uint32_t getLooptime(void) { +timeDelta_t getLooptime(void) { return gyroConfig()->looptime; } -uint32_t getGyroLooptime(void) { +timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index dda257670c..e327ba96af 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -137,5 +137,5 @@ void createDefaultConfig(void); void resetConfigs(void); void targetConfiguration(void); -uint32_t getLooptime(void); -uint32_t getGyroLooptime(void); +timeDelta_t getLooptime(void); +timeDelta_t getGyroLooptime(void); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 3b43323f5c..00389e78aa 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -110,6 +110,8 @@ bool sensors(uint32_t mask) return false; }; uint32_t millis(void) { return 0; } +timeDelta_t getLooptime(void) { return gyro.targetLooptime; } +timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; } void schedulerResetTaskStatistics(cfTaskId_e) {} void sensorsSet(uint32_t) {} bool compassIsHealthy(void) { return true; } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 87744b2eb9..6a741729f6 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -143,6 +143,8 @@ timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; +timeDelta_t getLooptime(void) {return gyro.targetLooptime;} +timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} void readEEPROM(void) {} From b1ad2b15a65b46866c0d23c2d8463a1af6871dc6 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 25 Nov 2021 16:28:05 -0300 Subject: [PATCH 022/107] force new build --- src/test/unit/sensor_gyro_unittest.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 6a741729f6..ebb5e12304 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -138,7 +138,6 @@ TEST(SensorGyro, Update) extern "C" { static timeMs_t milliTime = 0; - timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} From 1adb361669ec3ca17fd98e06ac35f2164d1dfc76 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 25 Nov 2021 17:31:06 -0300 Subject: [PATCH 023/107] timeDelta_t changed to uint32_t --- src/main/fc/config.c | 4 ++-- src/main/fc/config.h | 4 ++-- src/test/unit/flight_imu_unittest.cc | 4 ++-- src/test/unit/sensor_gyro_unittest.cc | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b5d5a467ee..02c8f1c8e6 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -173,11 +173,11 @@ __attribute__((weak)) void targetConfiguration(void) #define SECOND_PORT_INDEX 1 #endif -timeDelta_t getLooptime(void) { +uint32_t getLooptime(void) { return gyroConfig()->looptime; } -timeDelta_t getGyroLooptime(void) { +uint32_t getGyroLooptime(void) { return gyro.targetLooptime; } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e327ba96af..dda257670c 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -137,5 +137,5 @@ void createDefaultConfig(void); void resetConfigs(void); void targetConfiguration(void); -timeDelta_t getLooptime(void); -timeDelta_t getGyroLooptime(void); +uint32_t getLooptime(void); +uint32_t getGyroLooptime(void); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 00389e78aa..0c239dab8c 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -110,8 +110,8 @@ bool sensors(uint32_t mask) return false; }; uint32_t millis(void) { return 0; } -timeDelta_t getLooptime(void) { return gyro.targetLooptime; } -timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; } +uint32_t getLooptime(void) { return gyro.targetLooptime; } +uint32_t getGyroLooptime(void) { return gyro.targetLooptime; } void schedulerResetTaskStatistics(cfTaskId_e) {} void sensorsSet(uint32_t) {} bool compassIsHealthy(void) { return true; } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index ebb5e12304..772d6d5a18 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -142,8 +142,8 @@ timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; -timeDelta_t getLooptime(void) {return gyro.targetLooptime;} -timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} +uint32_t getLooptime(void) {return gyro.targetLooptime;} +uint32_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} void readEEPROM(void) {} From bca280b6c906098b2407a87f6dfe6d278ccbaef4 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 25 Nov 2021 21:31:39 -0300 Subject: [PATCH 024/107] use an external void to save the gyro calibration to storage --- CMakeLists.txt | 2 +- src/main/fc/config.h | 1 + src/main/fc/fc_core.c | 3 +++ src/main/sensors/gyro.c | 21 +++++++++++++++------ src/main/sensors/gyro.h | 1 + src/test/unit/CMakeLists.txt | 4 ++-- src/test/unit/flight_imu_unittest.cc | 8 ++------ src/test/unit/sensor_gyro_unittest.cc | 7 ++----- 8 files changed, 27 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f126830cfd..edb0b7640e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 5.0.0) +project(INAV VERSION 4.0.0) enable_language(ASM) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index dda257670c..94faba21b6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -29,6 +29,7 @@ #define MAX_NAME_LENGTH 16 #define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3e28f41be8..a400ff16f5 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -841,6 +841,9 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) { /* Update actual hardware readings */ gyroUpdate(); + + // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors + save_gyro_cal_externally(); #ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a08fe44d9d..c371e78762 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -82,6 +82,8 @@ FASTRAM gyro_t gyro; // gyro sensor object #define MAX_GYRO_COUNT 1 +static bool ok_to_save_gyro_cal = false; + STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT]; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT]; STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration[MAX_GYRO_COUNT]; @@ -372,12 +374,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; - // save gyro calibration - gyroConfigMutable()->gyro_zero_cal[X] = gyroDev->gyroZero[X]; - gyroConfigMutable()->gyro_zero_cal[Y] = gyroDev->gyroZero[Y]; - gyroConfigMutable()->gyro_zero_cal[Z] = gyroDev->gyroZero[Z]; - writeEEPROM(); - readEEPROM(); + ok_to_save_gyro_cal = true; LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics @@ -562,3 +559,15 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { } } } + +void save_gyro_cal_externally(void) { // fixes Test Unit compilation error + if (ok_to_save_gyro_cal) { + // save gyro calibration + gyroConfigMutable()->gyro_zero_cal[X] = gyroDev->gyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = gyroDev->gyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = gyroDev->gyroZero[Z]; + writeEEPROM(); + readEEPROM(); + ok_to_save_gyro_cal = false; + } +} \ No newline at end of file diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 154ce13468..d09ecac242 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -89,3 +89,4 @@ bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); void gyroUpdateDynamicLpf(float cutoffFreq); +void save_gyro_cal_externally(void); \ No newline at end of file diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index ae62ebe853..a7a20a5324 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" - "drivers/accgyro/accgyro_fake.c" "fc/config.h" "flight/imu.c" "sensors/boardalignment.c" + "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" "sensors/gyro.c") set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") @@ -25,7 +25,7 @@ set_property(SOURCE rcdevice_unittest.cc PROPERTY depends set_property(SOURCE sensor_gyro_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" - "drivers/accgyro/accgyro_fake.c" "fc/config.h" "sensors/gyro.c" "sensors/boardalignment.c") + "drivers/accgyro/accgyro_fake.c" "sensors/gyro.c" "sensors/boardalignment.c") set_property(SOURCE telemetry_hott_unittest.cc PROPERTY depends "telemetry/hott.c" "common/gps_conversion.c" "common/string_light.c") diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 0c239dab8c..25e2c438c3 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -28,8 +28,6 @@ extern "C" { #include "fc/runtime_config.h" #include "io/gps.h" - - #include "fc/config.h" #include "flight/imu.h" } @@ -110,8 +108,8 @@ bool sensors(uint32_t mask) return false; }; uint32_t millis(void) { return 0; } -uint32_t getLooptime(void) { return gyro.targetLooptime; } -uint32_t getGyroLooptime(void) { return gyro.targetLooptime; } +timeDelta_t getLooptime(void) { return gyro.targetLooptime; } +timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; } void schedulerResetTaskStatistics(cfTaskId_e) {} void sensorsSet(uint32_t) {} bool compassIsHealthy(void) { return true; } @@ -139,6 +137,4 @@ void resetHeadingHoldTarget(int16_t heading) UNUSED(heading); } bool isGPSHeadingValid(void) { return true; } -void readEEPROM(void) {} -void writeEEPROM(void) {} } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 772d6d5a18..579f1f9445 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -32,7 +32,6 @@ extern "C" { #include "common/utils.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/logging_codes.h" - #include "fc/config.h" #include "io/beeper.h" #include "scheduler/scheduler.h" #include "sensors/gyro.h" @@ -142,10 +141,8 @@ timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; -uint32_t getLooptime(void) {return gyro.targetLooptime;} -uint32_t getGyroLooptime(void) {return gyro.targetLooptime;} +timeDelta_t getLooptime(void) {return gyro.targetLooptime;} +timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} -void readEEPROM(void) {} -void writeEEPROM(void) {} } From 2c84497e3403b448cb4b446b8f25ed6d94adecd8 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 25 Nov 2021 21:32:15 -0300 Subject: [PATCH 025/107] use an external void to save the gyro calibration to storage --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index edb0b7640e..f126830cfd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 4.0.0) +project(INAV VERSION 5.0.0) enable_language(ASM) From a5d61e08e6e0bee6454152d70d6ff6a6246773fc Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 25 Nov 2021 21:53:28 -0300 Subject: [PATCH 026/107] move again --- src/main/fc/fc_core.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a400ff16f5..94f5165233 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -841,9 +841,6 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) { /* Update actual hardware readings */ gyroUpdate(); - - // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors - save_gyro_cal_externally(); #ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { @@ -887,6 +884,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(dT); + // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors + save_gyro_cal_externally(); + if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew); } From c9f4ae75ee956c2436dfc92f7a47e4a5c942631e Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 19:49:57 -0300 Subject: [PATCH 027/107] move function to config.c --- CMakeLists.txt | 2 +- src/main/fc/config.c | 12 ++++++++++++ src/main/fc/config.h | 2 ++ src/main/fc/fc_core.c | 2 +- src/main/sensors/gyro.c | 19 ++++--------------- src/main/sensors/gyro.h | 5 +++-- 6 files changed, 23 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f126830cfd..edb0b7640e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 5.0.0) +project(INAV VERSION 4.0.0) enable_language(ASM) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 02c8f1c8e6..c9bab62286 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -465,6 +465,18 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } +void setCalibrationGyroAndWriteEEPROM(void) { // fixes Test Unit compilation error + if (gyro.ok_to_save_cal) { + // save gyro calibration + gyroConfigMutable()->gyro_zero_cal[X] = gyro.getZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = gyro.getZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = gyro.getZero[Z]; + writeEEPROM(); + readEEPROM(); + gyro.ok_to_save_cal = false; + } +} + void beeperOffSet(uint32_t mask) { beeperConfigMutable()->beeper_off_flags |= mask; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 94faba21b6..a65f82a756 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -131,6 +131,8 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); +void setCalibrationGyroAndWriteEEPROM(void); + bool canSoftwareSerialBeUsed(void); void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 94f5165233..1fcf23a3c3 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -885,7 +885,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(dT); // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors - save_gyro_cal_externally(); + setCalibrationGyroAndWriteEEPROM(); if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c371e78762..62048af6f2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -82,8 +82,6 @@ FASTRAM gyro_t gyro; // gyro sensor object #define MAX_GYRO_COUNT 1 -static bool ok_to_save_gyro_cal = false; - STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT]; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT]; STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration[MAX_GYRO_COUNT]; @@ -374,7 +372,10 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; - ok_to_save_gyro_cal = true; + gyro.getZero[X] = dev->gyroZero[X]; + gyro.getZero[Y] = dev->gyroZero[Y]; + gyro.getZero[Z] = dev->gyroZero[Z]; + gyro.ok_to_save_cal = true; LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics @@ -558,16 +559,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); } } -} - -void save_gyro_cal_externally(void) { // fixes Test Unit compilation error - if (ok_to_save_gyro_cal) { - // save gyro calibration - gyroConfigMutable()->gyro_zero_cal[X] = gyroDev->gyroZero[X]; - gyroConfigMutable()->gyro_zero_cal[Y] = gyroDev->gyroZero[Y]; - gyroConfigMutable()->gyro_zero_cal[Z] = gyroDev->gyroZero[Z]; - writeEEPROM(); - readEEPROM(); - ok_to_save_gyro_cal = false; - } } \ No newline at end of file diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d09ecac242..73045cc3d6 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -42,6 +42,8 @@ typedef enum { typedef struct gyro_s { bool initialized; + bool ok_to_save_cal; + int16_t getZero[XYZ_AXIS_COUNT]; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; @@ -88,5 +90,4 @@ bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); -void gyroUpdateDynamicLpf(float cutoffFreq); -void save_gyro_cal_externally(void); \ No newline at end of file +void gyroUpdateDynamicLpf(float cutoffFreq); \ No newline at end of file From 2bd31e52140b604ba9580466afc9e522b86a3186 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 19:51:05 -0300 Subject: [PATCH 028/107] 4.0.0 to 5.0.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index edb0b7640e..f126830cfd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 4.0.0) +project(INAV VERSION 5.0.0) enable_language(ASM) From 4d32fcf35d4d883b6cd6ba500ebb87f4c7fd596d Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 20:07:58 -0300 Subject: [PATCH 029/107] test without function --- src/main/fc/fc_core.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 1fcf23a3c3..1e0f798393 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -885,7 +885,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(dT); // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors - setCalibrationGyroAndWriteEEPROM(); + //setCalibrationGyroAndWriteEEPROM(); if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew); From 1744ff09e04fdcb1cf0d5e5467b4f1a349a328e7 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 20:31:05 -0300 Subject: [PATCH 030/107] test without variables --- src/main/fc/fc_core.c | 2 +- src/main/sensors/gyro.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 1e0f798393..1fcf23a3c3 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -885,7 +885,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(dT); // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors - //setCalibrationGyroAndWriteEEPROM(); + setCalibrationGyroAndWriteEEPROM(); if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 62048af6f2..ee6f5dba51 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -371,12 +371,12 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[X] = v.v[X]; dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; - + /* gyro.getZero[X] = dev->gyroZero[X]; gyro.getZero[Y] = dev->gyroZero[Y]; gyro.getZero[Z] = dev->gyroZero[Z]; gyro.ok_to_save_cal = true; - + */ LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics } else { From df05f9fff401939844601dad3b472d0fa7753cb7 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 20:47:09 -0300 Subject: [PATCH 031/107] test without if condition --- src/main/sensors/gyro.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index ee6f5dba51..5a78dd0d68 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -357,7 +357,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe { fpVector3_t v; - if (gyroConfig()->init_gyro_cal_enabled) { + //if (gyroConfig()->init_gyro_cal_enabled) { // Consume gyro reading v.v[X] = dev->gyroADCRaw[X]; v.v[Y] = dev->gyroADCRaw[Y]; @@ -371,12 +371,12 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[X] = v.v[X]; dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; - /* + gyro.getZero[X] = dev->gyroZero[X]; gyro.getZero[Y] = dev->gyroZero[Y]; gyro.getZero[Z] = dev->gyroZero[Z]; gyro.ok_to_save_cal = true; - */ + LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics } else { @@ -384,13 +384,13 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Y] = 0; dev->gyroZero[Z] = 0; } - } else { // skip gyro calibration and use values ​​read from storage + /*} else { // skip gyro calibration and use values ​​read from storage gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended // pass the calibration values dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; - } + }*/ } /* From d7f008c7b96f50143e24e5d9228ec02b66c7ef91 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 21:04:45 -0300 Subject: [PATCH 032/107] verify condition with return --- src/main/sensors/gyro.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5a78dd0d68..52e5204eb1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -355,6 +355,15 @@ bool gyroIsCalibrationComplete(void) STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { + if (!gyroConfig()->init_gyro_cal_enabled) { + gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended + // pass the calibration values + dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; + dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; + dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; + return; // skip gyro calibration and use values ​​read from storage + } + fpVector3_t v; //if (gyroConfig()->init_gyro_cal_enabled) { From b3930f6cdd42786f14c1a29118de39db99005ad8 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 21:26:40 -0300 Subject: [PATCH 033/107] test init_gyro_cal_enabled with true verification in if condition --- src/main/sensors/gyro.c | 54 ++++++++++++++++++----------------------- 1 file changed, 23 insertions(+), 31 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 52e5204eb1..cb6ead3c5a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -355,7 +355,7 @@ bool gyroIsCalibrationComplete(void) STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { - if (!gyroConfig()->init_gyro_cal_enabled) { + if (gyroConfig()->init_gyro_cal_enabled) { gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended // pass the calibration values dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; @@ -366,40 +366,32 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe fpVector3_t v; - //if (gyroConfig()->init_gyro_cal_enabled) { - // Consume gyro reading - v.v[X] = dev->gyroADCRaw[X]; - v.v[Y] = dev->gyroADCRaw[Y]; - v.v[Z] = dev->gyroADCRaw[Z]; + // Consume gyro reading + v.v[X] = dev->gyroADCRaw[X]; + v.v[Y] = dev->gyroADCRaw[Y]; + v.v[Z] = dev->gyroADCRaw[Z]; - zeroCalibrationAddValueV(gyroCalibration, &v); + zeroCalibrationAddValueV(gyroCalibration, &v); - // Check if calibration is complete after this cycle - if (zeroCalibrationIsCompleteV(gyroCalibration)) { - zeroCalibrationGetZeroV(gyroCalibration, &v); - dev->gyroZero[X] = v.v[X]; - dev->gyroZero[Y] = v.v[Y]; - dev->gyroZero[Z] = v.v[Z]; + // Check if calibration is complete after this cycle + if (zeroCalibrationIsCompleteV(gyroCalibration)) { + zeroCalibrationGetZeroV(gyroCalibration, &v); + dev->gyroZero[X] = v.v[X]; + dev->gyroZero[Y] = v.v[Y]; + dev->gyroZero[Z] = v.v[Z]; - gyro.getZero[X] = dev->gyroZero[X]; - gyro.getZero[Y] = dev->gyroZero[Y]; - gyro.getZero[Z] = dev->gyroZero[Z]; - gyro.ok_to_save_cal = true; + gyro.getZero[X] = dev->gyroZero[X]; + gyro.getZero[Y] = dev->gyroZero[Y]; + gyro.getZero[Z] = dev->gyroZero[Z]; + gyro.ok_to_save_cal = true; - LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); - schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics - } else { - dev->gyroZero[X] = 0; - dev->gyroZero[Y] = 0; - dev->gyroZero[Z] = 0; - } - /*} else { // skip gyro calibration and use values ​​read from storage - gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended - // pass the calibration values - dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; - dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; - dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; - }*/ + LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); + schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics + } else { + dev->gyroZero[X] = 0; + dev->gyroZero[Y] = 0; + dev->gyroZero[Z] = 0; + } } /* From c19290e1b3b26ad317e65f17378a9247bb0becf1 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 21:42:54 -0300 Subject: [PATCH 034/107] use USE_IMU_FAKE to optimize the function --- src/main/sensors/gyro.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index cb6ead3c5a..444c38a076 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -355,7 +355,8 @@ bool gyroIsCalibrationComplete(void) STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { - if (gyroConfig()->init_gyro_cal_enabled) { +#ifndef USE_IMU_FAKE + if (!gyroConfig()->init_gyro_cal_enabled) { gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended // pass the calibration values dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; @@ -363,6 +364,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; return; // skip gyro calibration and use values ​​read from storage } +#endif fpVector3_t v; From bee6ac200ac5595fc985c55f0ae130dc6b4936bb Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 26 Nov 2021 21:59:30 -0300 Subject: [PATCH 035/107] pass gyro cal using array --- src/main/fc/config.c | 17 +++++++---------- src/main/fc/config.h | 3 ++- src/main/fc/fc_core.c | 3 --- src/main/sensors/gyro.c | 13 ++++++------- src/main/sensors/gyro.h | 2 -- 5 files changed, 15 insertions(+), 23 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c9bab62286..3eb51afe84 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -465,16 +465,13 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } -void setCalibrationGyroAndWriteEEPROM(void) { // fixes Test Unit compilation error - if (gyro.ok_to_save_cal) { - // save gyro calibration - gyroConfigMutable()->gyro_zero_cal[X] = gyro.getZero[X]; - gyroConfigMutable()->gyro_zero_cal[Y] = gyro.getZero[Y]; - gyroConfigMutable()->gyro_zero_cal[Z] = gyro.getZero[Z]; - writeEEPROM(); - readEEPROM(); - gyro.ok_to_save_cal = false; - } +void setCalibrationGyroAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) { + gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X]; + gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y]; + gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z]; + // save gyro calibration + writeEEPROM(); + readEEPROM(); } void beeperOffSet(uint32_t mask) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index a65f82a756..6ecdc7531e 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -19,6 +19,7 @@ #include #include +#include "common/axis.h" #include "common/time.h" #include "config/parameter_group.h" #include "drivers/adc.h" @@ -131,7 +132,7 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); -void setCalibrationGyroAndWriteEEPROM(void); +void setCalibrationGyroAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]); bool canSoftwareSerialBeUsed(void); void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 1fcf23a3c3..3e28f41be8 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -884,9 +884,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) annexCode(dT); - // saves gyro calibration in storage outside of "gyroUpdate" to avoid compilation errors - setCalibrationGyroAndWriteEEPROM(); - if (rxConfig()->rcFilterFrequency) { rcInterpolationApply(isRXDataNew); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 444c38a076..98476130bd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -355,7 +355,7 @@ bool gyroIsCalibrationComplete(void) STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { -#ifndef USE_IMU_FAKE +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error if (!gyroConfig()->init_gyro_cal_enabled) { gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended // pass the calibration values @@ -381,12 +381,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[X] = v.v[X]; dev->gyroZero[Y] = v.v[Y]; dev->gyroZero[Z] = v.v[Z]; - - gyro.getZero[X] = dev->gyroZero[X]; - gyro.getZero[Y] = dev->gyroZero[Y]; - gyro.getZero[Z] = dev->gyroZero[Z]; - gyro.ok_to_save_cal = true; - + +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error + setCalibrationGyroAndWriteEEPROM(dev->gyroZero); +#endif + LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics } else { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 73045cc3d6..6a81c9ba33 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -42,8 +42,6 @@ typedef enum { typedef struct gyro_s { bool initialized; - bool ok_to_save_cal; - int16_t getZero[XYZ_AXIS_COUNT]; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; From 0b338fc18a4be34d605443b6511c1ef1f697b2f9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 30 Nov 2021 11:31:47 +0000 Subject: [PATCH 036/107] first build --- docs/Settings.md | 10 ++++++ src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 5 +++ src/main/io/osd.c | 14 +++++--- src/main/io/osd.h | 1 + src/main/navigation/navigation.c | 45 ++++++++++++++++++++---- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_private.h | 11 +++--- 8 files changed, 72 insertions(+), 16 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index d5409bfbb9..1a529dc111 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3852,6 +3852,16 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT --- +### nav_wp_capture_altitude + +Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + ### nav_wp_load_on_boot If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index f8cdf92b25..96b95fd17e 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -194,6 +194,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = 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 CAPTURE ALTITUDE", SETTING_NAV_WP_CAPTURE_ALTITUDE), OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE), OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 22dd0335b1..ca833716cc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2415,6 +2415,11 @@ groups: field: general.waypoint_radius min: 10 max: 10000 + - name: nav_wp_capture_altitude + description: "Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on." + default_value: OFF + field: general.flags.waypoint_capture_altitude + type: bool - name: nav_wp_safe_distance description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check." default_value: 10000 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0c21c4c3ea..b0fa6a7237 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4212,13 +4212,17 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter 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 - timeMs_t currentTime = millis(); - int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)((currentTime - posControl.wpReachedTime)/1000); - if (holdTimeRemaining >=0) { + if (navConfig()->general.flags.waypoint_capture_altitude && !posControl.wpAltitudeReached) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); + } else { + // WP hold time countdown in seconds + timeMs_t currentTime = millis(); + int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime)); + holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0; tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); + + messages[messageCount++] = messageBuf; } - messages[messageCount++] = messageBuf; } else { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 75b8aefeff..17c5771c43 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -85,6 +85,7 @@ #define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" #define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" #define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" #define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" #define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a3302f3ab5..4f7894d9dc 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -58,7 +58,7 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" - +#define WP_ALTITUDE_MARGIN_CM 100 // WP altitude capture tolerance, used when WP altitude setting enforced when WP reached // Multirotors: #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) @@ -97,7 +97,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, 1); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 15); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -117,8 +117,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT, .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT, .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs - .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action + .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled + .waypoint_capture_altitude = SETTING_NAV_WP_CAPTURE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved }, // General navigation parameters @@ -241,6 +242,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); +bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); static bool isWaypointMissionValid(void); @@ -1480,6 +1482,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; + posControl.wpAltitudeReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS // We use p3 as the volatile jump counter (p2 is the static value) @@ -1581,9 +1584,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga { UNUSED(previousState); + if (navConfig()->general.flags.waypoint_capture_altitude) { + posControl.wpAltitudeReached = isWaypointAltitudeReached(); + } + switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_WAYPOINT: - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT + if (navConfig()->general.flags.waypoint_capture_altitude && !posControl.wpAltitudeReached) { + return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME; + } else { + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT + } case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: @@ -1612,6 +1623,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + if (navConfig()->general.flags.waypoint_capture_altitude && !posControl.wpAltitudeReached) { + // Adjust altitude to waypoint setting + if (STATE(AIRPLANE)) { + int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + } else { + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); + } + + posControl.wpAltitudeReached = isWaypointAltitudeReached(); + + if (posControl.wpAltitudeReached) { + posControl.wpReachedTime = millis(); + } else { + return NAV_FSM_EVENT_NONE; + } + } + timeMs_t currentTime = millis(); if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 || @@ -1619,7 +1648,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SUCCESS; } - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } @@ -2134,6 +2162,11 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp return isWaypointPositionReached(&waypoint->pos, isWaypointHome); } +bool isWaypointAltitudeReached(void) +{ + return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < WP_ALTITUDE_MARGIN_CM; +} + static void updateHomePositionCompatibility(void) { geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); @@ -2595,7 +2628,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti lastUpdateTimeUs = currentTimeUs; posControl.desiredState.pos.z = altitudeToUse; } - else { + else { // ROC_TO_ALT_NORMAL /* * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 2894b22b53..3b1f7309b8 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -224,6 +224,7 @@ typedef struct navConfig_s { uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled uint8_t mission_planner_reset; // Allow WP Mission Planner reset using mode toggle (resets WPs to 0) uint8_t waypoint_mission_restart; // Waypoint mission restart action + uint8_t waypoint_capture_altitude; // Forces waypoint altitude to be acheived } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c20b0fbef6..2b3deeec50 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -373,12 +373,13 @@ typedef struct { int8_t loadedMultiMissionStartWP; // selected multi mission start WP int8_t loadedMultiMissionWPCount; // number of WPs in selected multi 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 - float wpDistance; // Distance to active WP - timeMs_t wpReachedTime; // Time the waypoint was reached + float wpInitialAltitude; // Altitude at start of WP + float wpInitialDistance; // Distance when starting flight to WP + float wpDistance; // Distance to active WP + timeMs_t wpReachedTime; // Time the waypoint was reached + bool wpAltitudeReached; // WP altitude achieved /* Internals & statistics */ int16_t rcAdjustment[4]; From 2b7918bf53a9e8176ba1c580d9c39320b651d64d Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 3 Dec 2021 17:19:31 +0000 Subject: [PATCH 037/107] 1st cut --- src/main/CMakeLists.txt | 2 + src/main/fc/fc_init.c | 10 + src/main/fc/fc_tasks.c | 6 + src/main/fc/settings.yaml | 4 +- src/main/io/displayport_hdzero_osd.c | 426 +++++++++++++++++++++++++++ src/main/io/displayport_hdzero_osd.h | 34 +++ src/main/io/osd.c | 20 +- src/main/io/osd.h | 32 ++ src/main/io/serial.h | 3 +- src/main/target/common.h | 1 + 10 files changed, 525 insertions(+), 13 deletions(-) create mode 100644 src/main/io/displayport_hdzero_osd.c create mode 100644 src/main/io/displayport_hdzero_osd.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 47edcaeee4..89aaaaf27c 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -502,6 +502,8 @@ main_sources(COMMON_SRC io/displayport_msp.h io/displayport_oled.c io/displayport_oled.h + io/displayport_hdzero_osd.c + io/displayport_hdzero_osd.h io/displayport_srxl.c io/displayport_srxl.h io/displayport_hott.c diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 536d235d33..0408203209 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -109,6 +109,7 @@ #include "io/displayport_frsky_osd.h" #include "io/displayport_msp.h" #include "io/displayport_max7456.h" +#include "io/displayport_hdzero_osd.h" #include "io/displayport_srxl.h" #include "io/flashfs.h" #include "io/gps.h" @@ -289,6 +290,10 @@ void init(void) djiOsdSerialInit(); #endif +#ifdef USE_HDZERO_OSD + hdzeroOsdSerialInit(); +#endif + #if defined(USE_SMARTPORT_MASTER) smartportMasterInit(); #endif @@ -553,6 +558,11 @@ void init(void) osdDisplayPort = frskyOSDDisplayPortInit(osdConfig()->video_system); } #endif +#ifdef USE_HDZERO_OSD + if (!osdDisplayPort) { + osdDisplayPort = hdzeroOsdDisplayPortInit(); + } +#endif #if defined(USE_MAX7456) // If there is a max7456 chip for the OSD and we have no // external OSD initialized, use it. diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index e4ef2a2132..a5b60a577e 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -40,6 +40,8 @@ #include "fc/fc_core.h" #include "fc/fc_msp.h" #include "fc/fc_tasks.h" + +#include "../io/displayport_hdzero_osd.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -108,6 +110,10 @@ void taskHandleSerial(timeUs_t currentTimeUs) // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task djiOsdSerialProcess(); #endif + +#ifdef USE_HDZERO_OSD + hdzeroOsdSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); +#endif } void taskUpdateBattery(timeUs_t currentTimeUs) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8cc050ea46..0082bfc173 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -75,7 +75,7 @@ tables: values: ["BATTERY", "CELL"] enum: osd_stats_min_voltage_unit_e - name: osd_video_system - values: ["AUTO", "PAL", "NTSC"] + values: ["AUTO", "PAL", "NTSC", "HD"] enum: videoSystem_e - name: osd_telemetry values: ["OFF", "ON","TEST"] @@ -3091,7 +3091,7 @@ groups: type: uint8_t default_value: "OFF" - name: osd_video_system - description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`" + description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, and `HD`" default_value: "AUTO" table: osd_video_system field: video_system diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c new file mode 100644 index 0000000000..f8a93c5d15 --- /dev/null +++ b/src/main/io/displayport_hdzero_osd.c @@ -0,0 +1,426 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + * + * This file 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 this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include + +#include "platform.h" + +// TODO: Remove these before committing +#define USE_OSD +#define USE_HDZERO_OSD + +#define STATS + +#if defined(USE_OSD) && defined(USE_HDZERO_OSD) + +#include "common/utils.h" +#include "common/printf.h" +#include "common/time.h" + +#include "drivers/display.h" +#include "drivers/display_font_metadata.h" +#include "drivers/osd_symbols.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +#include "displayport_hdzero_osd.h" + +#define MSP_HEARTBEAT 0 +#define MSP_RELEASE 1 +#define MSP_CLEAR_SCREEN 2 +#define MSP_WRITE_STRING 3 +#define MSP_DRAW_SCREEN 4 +#define MSP_SET_HD 5 + +#define FONT_PAGE_ATTRIBUTE 0x01 + +static mspPort_t hdzeroMspPort; +static displayPort_t hdzeroOsdDisplayPort; +static bool hdzeroVtxReady; + +// HD screen size +#define ROWS 18 +#define COLS 50 +#define SCREENSIZE (ROWS*COLS) +static uint8_t screen[SCREENSIZE]; +static uint8_t fontPage[SCREENSIZE/8+1]; // page bits for each character (to address 512 char font) + +static int setHdMode(displayPort_t *displayPort); +static void resync(displayPort_t *displayPort); +static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr); +static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr); + +extern uint8_t cliMode; + +static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len) +{ + UNUSED(displayPort); + + if (cliMode) + { + return 0; + } + + return mspSerialPushPort(cmd, subcmd, len, &hdzeroMspPort, MSP_V1); +} + +static int heartbeat(displayPort_t *displayPort) +{ + uint8_t subcmd[] = { MSP_HEARTBEAT }; + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int release(displayPort_t *displayPort) +{ + uint8_t subcmd[] = { MSP_RELEASE }; + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int clearScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + + memset(screen, SYM_BLANK, sizeof(screen)); + memset(fontPage, 0, sizeof(fontPage)); + return 1; +} + +#ifdef STATS +static void displayFrequency(displayPort_t *displayPort) +{ + static timeMs_t lastTime = 0; + timeMs_t now = millis(); + char buff[6]; + + int freq = 1000/(now-lastTime); + lastTime = now; + + tfp_sprintf(buff, "%02dHZ", freq); + writeString(displayPort, 46, 17, buff, 0); +} +#endif + +/* + * Write one line at a time, skipping blank lines + */ +static int drawScreen(displayPort_t *displayPort) // 62hz +{ + int charsOut = 0; + static uint8_t row = 0, clearSent = 0; + uint8_t subcmd[COLS + 4]; + uint16_t lineIdx, idx, end; + uint8_t len, col, page, aPage; + +#ifdef STATS + if (row == 0) + { + displayFrequency(displayPort); + } +#endif + + uint8_t linesToPrint = 3; // 62 / (18/3) = 10Hz fullpage refresh min + do + { + // Find a row with something to print + do + { + // Strip leading and trailing spaces for the selected row + lineIdx = row*COLS; + idx = lineIdx; + end = idx + COLS - 1; + + while ((screen[idx] == SYM_BLANK || screen[end] == SYM_BLANK) && idx <= end) + { + if (screen[idx] == SYM_BLANK) + { + idx++; + } + + if (screen[end] == SYM_BLANK) + { + end--; + } + } + } + while (idx > end && ++row < ROWS); + + while (idx <= end) + { + if (!clearSent) + { + // Start the transaction + subcmd[0] = MSP_CLEAR_SCREEN; + charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + clearSent = 1; + } + + // Split the line up into strings from the same font page and output them. + // (note spaces are printed to save overhead on small elements) + len = 4; + col = idx-lineIdx; + page = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; + + do + { + subcmd[len++] = screen[idx++]; + aPage = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; + } + while (idx <= end && (aPage == page || screen[idx] == SYM_BLANK)); + + subcmd[0] = MSP_WRITE_STRING; + subcmd[1] = row; + subcmd[2] = col; + subcmd[3] = page; + charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, len); + } + } + while (++row < ROWS && --linesToPrint); + + if (row >= ROWS) + { + // End the transaction if required and reset the counters + if (clearSent > 0) + { + subcmd[0] = MSP_DRAW_SCREEN; + charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + } + row = clearSent = 0; + } + + return charsOut; +} + +static int setHdMode(displayPort_t *displayPort) +{ + uint8_t subcmd[3]; + subcmd[0] = MSP_SET_HD; + subcmd[1] = 0; // future font index + subcmd[2] = 1; // 0 SD 1 HD + + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); +} + +static int grab(displayPort_t *displayPort) +{ + return heartbeat(displayPort); +} + +static int screenSize(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return SCREENSIZE; +} + +// Intercept writeString and write to a buffer instead (1st page of font file) +static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr) +{ + UNUSED(displayPort); + UNUSED(attr); + + uint16_t pos = (row * COLS) + col; + if (pos >= SCREENSIZE) + { + return 0; + } + + uint16_t len = strlen(string); + + // Allow word wrap and truncate of past the screen end + uint16_t end = pos + len - 1; + if (end >= SCREENSIZE) + { + len = end-SCREENSIZE; + } + + // Copy the string into the screen buffer + memcpy(screen + pos, string, len); + + // Clear the page bits for all the characters in the string + for (uint16_t i = 0; i < len; i++) + { + uint16_t idx = pos+i; + fontPage[idx >> 3] &= ~(1 << (idx & 0x07)); + } + + return (int)len; +} + +// Write character to screen and page buffers (supports 512 char fonts) +static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) +{ + UNUSED(displayPort); + UNUSED(attr); + + uint16_t pos = (row * COLS) + col; + if (pos >= SCREENSIZE) + { + return 0; + } + + // Copy character into screen buffer + screen[pos] = c; + + uint16_t idx = pos >> 3; + uint8_t bitmask = 1 << (pos & 0x07); + + // Save index of the character's font page + if (c & 0x0100) + { + fontPage[idx] |= bitmask; + } + else + { + fontPage[idx] &= ~bitmask; + } + + return 1; +} + +static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr) +{ + UNUSED(displayPort); + + uint16_t pos = (row * COLS) + col; + if (pos >= SCREENSIZE) + { + *c = SYM_BLANK; + } + else + { + uint16_t chr = (fontPage[pos >> 3] >> (pos & 0x07)) & FONT_PAGE_ATTRIBUTE; + *c = (chr << 8) | screen[pos]; + } + + if (attr) + { + *attr = TEXT_ATTRIBUTES_NONE; + } + + return true; +} + +static bool isTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + +static void resync(displayPort_t *displayPort) +{ + displayPort->rows = ROWS; + displayPort->cols = COLS; + setHdMode(displayPort); +} + +static uint32_t txBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return mspSerialTxBytesFree(); +} + +static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + + textAttributes_t attr = TEXT_ATTRIBUTES_NONE; + //TEXT_ATTRIBUTES_ADD_INVERTED(attr); + //TEXT_ATTRIBUTES_ADD_SOLID_BG(attr); + return attr; +} + +// TODO: Is this needed? +static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) +{ + UNUSED(displayPort); + + metadata->charCount = 512; + metadata->version = 3; + + return true; +} + +static bool isReady(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return hdzeroVtxReady; +} + +static const displayPortVTable_t hdzeroOsdVTable = { + .grab = grab, + .release = release, + .clearScreen = clearScreen, + .drawScreen = drawScreen, + .screenSize = screenSize, + .writeString = writeString, + .writeChar = writeChar, + .readChar = readChar, + .isTransferInProgress = isTransferInProgress, + .heartbeat = heartbeat, + .resync = resync, + .txBytesFree = txBytesFree, + .supportedTextAttributes = supportedTextAttributes, + .getFontMetadata = getFontMetadata, + .isReady = isReady, +}; + +void hdzeroOsdSerialInit(void) +{ + memset(&hdzeroMspPort, 0, sizeof(mspPort_t)); + + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD); + + if (portConfig) + { + serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, + NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + + if (port) + { + resetMspPort(&hdzeroMspPort, port); + } + } +} + +displayPort_t *hdzeroOsdDisplayPortInit(void) +{ + memset(screen, SYM_BLANK, sizeof(screen)); + memset(fontPage, 0, sizeof(fontPage)); + displayInit(&hdzeroOsdDisplayPort, &hdzeroOsdVTable); + return &hdzeroOsdDisplayPort; +} + +void hdzeroOsdSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn) +{ + if (hdzeroMspPort.port) + { + // Process normal MSP command + mspSerialProcessOnePort(&hdzeroMspPort, evaluateNonMspData, mspProcessCommandFn); + hdzeroVtxReady = true; + } +} + +#endif // USE_HDZERO_OSD diff --git a/src/main/io/displayport_hdzero_osd.h b/src/main/io/displayport_hdzero_osd.h new file mode 100644 index 0000000000..4b12deb688 --- /dev/null +++ b/src/main/io/displayport_hdzero_osd.h @@ -0,0 +1,34 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + * + * This file 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 this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "drivers/osd.h" +#include "msp/msp_serial.h" + +typedef struct displayPort_s displayPort_t; + +void hdzeroOsdSerialInit(void); +displayPort_t *hdzeroOsdDisplayPortInit(void); +void hdzeroOsdSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0c21c4c3ea..1bf3c4bee5 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -969,7 +969,7 @@ static void osdFormatMessage(char *buff, size_t size, const char *message, bool strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength)); } // Ensure buff is zero terminated - buff[size - 1] = '\0'; + buff[size] = '\0'; } /** @@ -3160,11 +3160,11 @@ void osdDrawNextElement(void) } while(!osdDrawSingleElement(elementIndex) && index != elementIndex); // Draw artificial horizon + tracking telemtry last - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - if (osdConfig()->telemetry>0){ - osdDisplayTelemetry(); + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } -} PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, @@ -3562,7 +3562,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse) if (!osdDisplayPortToUse) return; - BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31)); + BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); osdDisplayPort = osdDisplayPortToUse; @@ -4121,11 +4121,11 @@ void osdUpdate(timeUs_t currentTimeUs) osdUpdateStats(); } - if ((counter & DRAW_FREQ_DENOM) == 0) { - // redraw values in buffer + if (counter % DRAW_FREQ_DENOM == 0) { + // redraw values in buffer one at a time osdRefresh(currentTimeUs); - } else { - // rest of time redraw screen + } else if (counter % DRAW_FREQ_DENOM == 1) { + // redraw screen displayDrawScreen(osdDisplayPort); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 75b8aefeff..aa11ecc753 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -29,20 +29,52 @@ #endif #define OSD_LAYOUT_COUNT (OSD_ALTERNATE_LAYOUT_COUNT + 1) +#if 1 + +// 00vb yyyy yyxx xxxx +// (visible)(blink)(yCoord)(xCoord) + +#define OSD_VISIBLE_FLAG 0x2000 +#define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) + +#define OSD_POS(x,y) ((x) | ((y) << 6)) +#define OSD_POSSD(x,y) (((x)<<1) | ((y) << 6)) +#define OSD_X(x) ((x) & 0x003F) +#define OSD_Y(x) (((x) >> 6) & 0x003F) +#define OSD_POS_MAX 0xFFF + +#else + +// 0000 vbyy yyyx xxxx +// (visible)(blink)(yCoord)(xCoord) + #define OSD_VISIBLE_FLAG 0x0800 #define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) + #define OSD_POS(x,y) ((x) | ((y) << 5)) #define OSD_X(x) ((x) & 0x001F) #define OSD_Y(x) (((x) >> 5) & 0x001F) #define OSD_POS_MAX 0x3FF + +#endif + #define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) +#ifdef USE_HDZERO_OSD // TODO: Fix me #define OSD_HOMING_LIM_H1 6 #define OSD_HOMING_LIM_H2 16 #define OSD_HOMING_LIM_H3 38 #define OSD_HOMING_LIM_V1 5 #define OSD_HOMING_LIM_V2 10 #define OSD_HOMING_LIM_V3 15 +#else +#define OSD_HOMING_LIM_H1 6 +#define OSD_HOMING_LIM_H2 16 +#define OSD_HOMING_LIM_H3 38 +#define OSD_HOMING_LIM_V1 5 +#define OSD_HOMING_LIM_V2 10 +#define OSD_HOMING_LIM_V3 15 +#endif // Message defines to be use in OSD and/or telemetry exports #define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!" diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 20d389048c..3b2b5d68a6 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -55,7 +55,8 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 - FUNCTION_IMU2 = (1 << 24), // 16777216 + FUNCTION_IMU2 = (1 << 24), // 16777216 + FUNCTION_HDZERO_OSD = (1 << 25), // 33554432 } serialPortFunction_e; typedef enum { diff --git a/src/main/target/common.h b/src/main/target/common.h index 6817ec7e37..b97507e836 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -130,6 +130,7 @@ #define USE_OSD #define USE_FRSKYOSD #define USE_DJI_HD_OSD +#define USE_HDZERO_OSD #define USE_SMARTPORT_MASTER #define NAV_NON_VOLATILE_WAYPOINT_CLI From 19894a5a8025535aeaced1e766f0c0dda6dce6d9 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Sat, 4 Dec 2021 20:16:33 -0300 Subject: [PATCH 038/107] add ins gravity --- docs/Settings.md | 10 +++++ src/main/fc/config.c | 11 +++++- src/main/fc/config.h | 3 +- src/main/fc/settings.yaml | 6 +++ src/main/navigation/navigation.c | 1 + .../navigation/navigation_pos_estimator.c | 37 ++++++++++++------ src/main/sensors/gyro.c | 39 ++++++++++++------- src/main/sensors/gyro.h | 1 + 8 files changed, 80 insertions(+), 28 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 99cd64e996..f1b4b1cd88 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2142,6 +2142,16 @@ If defined to 'OFF', it will ignore the gyroscope calibration done at each start --- +### ins_gravity_cmss + +Calculated 1G of Acc axis Z to use in INS + +| Default | Min | Max | +| --- | --- | --- | +| 0.0 | 0 | 1000 | + +--- + ### 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) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3eb51afe84..5d090ced44 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -465,11 +465,18 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } -void setCalibrationGyroAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) { +void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) { gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X]; gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y]; gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z]; - // save gyro calibration + // save the calibration + writeEEPROM(); + readEEPROM(); +} + +void setGravityCalibrationAndWriteEEPROM(float getGravity) { + gyroConfigMutable()->gravity_cmss_cal = getGravity; + // save the calibration writeEEPROM(); readEEPROM(); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 6ecdc7531e..ed9deaabd2 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -132,7 +132,8 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); -void setCalibrationGyroAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]); +void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]); +void setGravityCalibrationAndWriteEEPROM(float getGravity); bool canSoftwareSerialBeUsed(void); void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 737574ca83..bd290f64e3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -332,6 +332,12 @@ groups: field: gyro_zero_cal[Z] min: INT16_MIN max: INT16_MAX + - name: ins_gravity_cmss + description: "Calculated 1G of Acc axis Z to use in INS" + default_value: 0.0 + field: gravity_cmss_cal + min: 0 + max: 1000 - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a3302f3ab5..546421ff1a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -63,6 +63,7 @@ #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) #define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set + // Planes: #define FW_RTH_CLIMB_OVERSHOOT_CM 100 #define FW_RTH_CLIMB_MARGIN_MIN_CM 100 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index e11ab12beb..8dc1c6f67f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -51,6 +51,7 @@ #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" +#include "sensors/gyro.h" #include "sensors/pitotmeter.h" #include "sensors/opflow.h" @@ -353,11 +354,19 @@ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs) */ static void restartGravityCalibration(void) { + if (!gyroConfig()->init_gyro_cal_enabled) { + return; + } + zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false); } static bool gravityCalibrationComplete(void) { + if (!gyroConfig()->init_gyro_cal_enabled) { + return true; + } + return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } @@ -393,9 +402,9 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.lastUpdateTime = currentTimeUs; if (!isImuReady()) { - posEstimator.imu.accelNEU.x = 0; - posEstimator.imu.accelNEU.y = 0; - posEstimator.imu.accelNEU.z = 0; + posEstimator.imu.accelNEU.x = 0.0f; + posEstimator.imu.accelNEU.y = 0.0f; + posEstimator.imu.accelNEU.z = 0.0f; restartGravityCalibration(); } @@ -424,13 +433,19 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.accelNEU.z = accelBF.z; /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ - if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { - zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); + if (gyroConfig()->init_gyro_cal_enabled) { + if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { + zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); - if (gravityCalibrationComplete()) { - zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); - LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); + if (gravityCalibrationComplete()) { + zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); + setGravityCalibrationAndWriteEEPROM(posEstimator.imu.calibratedGravityCMSS); + LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); + } } + } else { + posEstimator.imu.gravityCalibration.params.state = ZERO_CALIBRATION_DONE; + posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } /* If calibration is incomplete - report zero acceleration */ @@ -438,9 +453,9 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; } else { - posEstimator.imu.accelNEU.x = 0; - posEstimator.imu.accelNEU.y = 0; - posEstimator.imu.accelNEU.z = 0; + posEstimator.imu.accelNEU.x = 0.0f; + posEstimator.imu.accelNEU.y = 0.0f; + posEstimator.imu.accelNEU.z = 0.0f; } /* Update blackbox values */ diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 98476130bd..f440ab6cbb 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -99,7 +99,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -128,6 +128,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, #endif .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT, .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT}, + .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT, ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -340,6 +341,10 @@ void gyroStartCalibration(void) if (!gyro.initialized) { return; } + + if (!gyroConfig()->init_gyro_cal_enabled) { + return; + } zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } @@ -349,23 +354,16 @@ bool gyroIsCalibrationComplete(void) if (!gyro.initialized) { return true; } + + if (!gyroConfig()->init_gyro_cal_enabled) { + return true; + } return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { -#ifndef USE_IMU_FAKE // fixes Test Unit compilation error - if (!gyroConfig()->init_gyro_cal_enabled) { - gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; // calibration ended - // pass the calibration values - dev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; - dev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; - dev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; - return; // skip gyro calibration and use values ​​read from storage - } -#endif - fpVector3_t v; // Consume gyro reading @@ -383,7 +381,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Z] = v.v[Z]; #ifndef USE_IMU_FAKE // fixes Test Unit compilation error - setCalibrationGyroAndWriteEEPROM(dev->gyroZero); + setGyroCalibrationAndWriteEEPROM(dev->gyroZero); #endif LOG_D(GYRO, "Gyro calibration complete (%d, %d, %d)", dev->gyroZero[X], dev->gyroZero[Y], dev->gyroZero[Z]); @@ -407,8 +405,21 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate) static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf) { + // range: +/- 8192; +/- 2000 deg/sec if (gyroDev->readFn(gyroDev)) { + +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error + if (!gyroConfig()->init_gyro_cal_enabled) { + // marks that the gyro calibration has ended + gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; + // pass the calibration values + gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; + gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; + gyroDev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z]; + } +#endif + if (zeroCalibrationIsCompleteV(gyroCal)) { int32_t gyroADCtmp[XYZ_AXIS_COUNT]; @@ -508,7 +519,7 @@ void FAST_CODE NOINLINE gyroUpdate() // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] float gyroADCf = gyro.gyroADCf[axis]; - DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); + //DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); /* * First gyro LPF is the only filter applied with the full gyro sampling speed diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6a81c9ba33..f7794f7a62 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -75,6 +75,7 @@ typedef struct gyroConfig_s { #endif bool init_gyro_cal_enabled; int16_t gyro_zero_cal[XYZ_AXIS_COUNT]; + float gravity_cmss_cal; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); From b6483bb457a0919bee8b4c892e6870bc1db96f18 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Sat, 4 Dec 2021 20:22:27 -0300 Subject: [PATCH 039/107] add USE_IMU_FAKE --- src/main/navigation/navigation_pos_estimator.c | 4 ++-- src/main/sensors/gyro.c | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 8dc1c6f67f..d4d1e86a25 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -362,11 +362,11 @@ static void restartGravityCalibration(void) } static bool gravityCalibrationComplete(void) -{ +{ if (!gyroConfig()->init_gyro_cal_enabled) { return true; } - + return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f440ab6cbb..b0ce5e56b3 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -341,10 +341,12 @@ void gyroStartCalibration(void) if (!gyro.initialized) { return; } - + +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error if (!gyroConfig()->init_gyro_cal_enabled) { return; } +#endif zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } @@ -355,9 +357,11 @@ bool gyroIsCalibrationComplete(void) return true; } +#ifndef USE_IMU_FAKE // fixes Test Unit compilation error if (!gyroConfig()->init_gyro_cal_enabled) { return true; } +#endif return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } @@ -519,7 +523,7 @@ void FAST_CODE NOINLINE gyroUpdate() // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] float gyroADCf = gyro.gyroADCf[axis]; - //DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); + DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); /* * First gyro LPF is the only filter applied with the full gyro sampling speed From 84c422210e119e528e1c600e066f5df85e4202a7 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Mon, 13 Dec 2021 09:21:57 +0000 Subject: [PATCH 040/107] Update DJI OSD module to map HD coordinate system to SD. --- src/main/io/osd_dji_hd.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index dd317cdaea..b8ffb444c4 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -357,33 +357,35 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature)); if (inavOSDIdx >= 0 && itemIsSupported) { - // Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV - // However visibility is different. INAV has 3 layouts, while BF only has visibility profiles - // Here we use only one OSD layout mapped to first OSD BF profile + // Position & visibility are encoded in 16 bits, and is the same between BF/DJI. + // However INAV supports co-ords of 0-63 and has 3 layouts, while BF has co-ords 0-31 and visibility profiles. + // Re-encode for co-ords of 0-31 and map the layout to all three BF profiles. uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx]; + uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPosHD), OSD_Y(itemPosHD)); // Workarounds for certain OSD element positions // INAV calculates these dynamically, while DJI expects the config to have defined coordinates switch(inavOSDIdx) { case OSD_CROSSHAIRS: - itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(13, 6); + itemPosSD = (itemPosSD & (~OSD_POS_MAX)) | OSD_POS_SD(13, 6); break; case OSD_ARTIFICIAL_HORIZON: - itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 2); + itemPosSD = (itemPosSD & (~OSD_POS_MAX)) | OSD_POS_SD(14, 2); break; case OSD_HORIZON_SIDEBARS: - itemPos = (itemPos & (~OSD_POS_MAX)) | OSD_POS(14, 5); + itemPosSD = (itemPosSD & (~OSD_POS_MAX)) | OSD_POS_SD(14, 5); break; } // Enforce visibility in 3 BF OSD profiles if (OSD_VISIBLE(itemPos)) { - itemPos |= 0x3000; + itemPosSD |= OSD_VISIBLE_FLAG_SD; + itemPosSD |= 0x3000; } - sbufWriteU16(dst, itemPos); + sbufWriteU16(dst, itemPosSD); } else { // Hide OSD elements unsupported by INAV From 8108397022e39546a535e2d75d0b5c23ac4e69eb Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Mon, 13 Dec 2021 09:22:59 +0000 Subject: [PATCH 041/107] Include SD defines to support DJI module --- src/main/io/osd.h | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index aa11ecc753..be0651e50d 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -38,11 +38,14 @@ #define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) #define OSD_POS(x,y) ((x) | ((y) << 6)) -#define OSD_POSSD(x,y) (((x)<<1) | ((y) << 6)) #define OSD_X(x) ((x) & 0x003F) #define OSD_Y(x) (((x) >> 6) & 0x003F) #define OSD_POS_MAX 0xFFF +// For DJI compatibility +#define OSD_VISIBLE_FLAG_SD 0x0800 +#define OSD_POS_SD(x,y) ((x) | ((y) << 5)) + #else // 0000 vbyy yyyx xxxx @@ -60,21 +63,12 @@ #define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) -#ifdef USE_HDZERO_OSD // TODO: Fix me #define OSD_HOMING_LIM_H1 6 #define OSD_HOMING_LIM_H2 16 #define OSD_HOMING_LIM_H3 38 #define OSD_HOMING_LIM_V1 5 #define OSD_HOMING_LIM_V2 10 #define OSD_HOMING_LIM_V3 15 -#else -#define OSD_HOMING_LIM_H1 6 -#define OSD_HOMING_LIM_H2 16 -#define OSD_HOMING_LIM_H3 38 -#define OSD_HOMING_LIM_V1 5 -#define OSD_HOMING_LIM_V2 10 -#define OSD_HOMING_LIM_V3 15 -#endif // Message defines to be use in OSD and/or telemetry exports #define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!" From c56ab0cc598567a22bb05e5ed0286514a8ec501d Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 14 Dec 2021 18:17:26 +0000 Subject: [PATCH 042/107] Fix syntax error in dji code changes. --- src/main/io/osd_dji_hd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b8ffb444c4..1c6fbf0f68 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -361,7 +361,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) // However INAV supports co-ords of 0-63 and has 3 layouts, while BF has co-ords 0-31 and visibility profiles. // Re-encode for co-ords of 0-31 and map the layout to all three BF profiles. uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx]; - uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPosHD), OSD_Y(itemPosHD)); + uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPos), OSD_Y(itemPos)); // Workarounds for certain OSD element positions // INAV calculates these dynamically, while DJI expects the config to have defined coordinates From 446339d360a2e0b4cf7a296beec3cc5bfd3a9fc5 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 14 Dec 2021 18:18:06 +0000 Subject: [PATCH 043/107] Remove debugging code --- src/main/io/displayport_hdzero_osd.c | 40 +++++----------------------- 1 file changed, 7 insertions(+), 33 deletions(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index f8a93c5d15..4862288c04 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -28,12 +28,6 @@ #include "platform.h" -// TODO: Remove these before committing -#define USE_OSD -#define USE_HDZERO_OSD - -#define STATS - #if defined(USE_OSD) && defined(USE_HDZERO_OSD) #include "common/utils.h" @@ -109,25 +103,12 @@ static int clearScreen(displayPort_t *displayPort) return 1; } -#ifdef STATS -static void displayFrequency(displayPort_t *displayPort) -{ - static timeMs_t lastTime = 0; - timeMs_t now = millis(); - char buff[6]; - - int freq = 1000/(now-lastTime); - lastTime = now; - - tfp_sprintf(buff, "%02dHZ", freq); - writeString(displayPort, 46, 17, buff, 0); -} -#endif - /* - * Write one line at a time, skipping blank lines + * Write up to three populated rows at a time, skipping blank lines. + * This gives a refresh rate to the VTX of approximately 10 to 62Hz + * depending on how much data is displayed. */ -static int drawScreen(displayPort_t *displayPort) // 62hz +static int drawScreen(displayPort_t *displayPort) // 62.5hz { int charsOut = 0; static uint8_t row = 0, clearSent = 0; @@ -135,14 +116,7 @@ static int drawScreen(displayPort_t *displayPort) // 62hz uint16_t lineIdx, idx, end; uint8_t len, col, page, aPage; -#ifdef STATS - if (row == 0) - { - displayFrequency(displayPort); - } -#endif - - uint8_t linesToPrint = 3; // 62 / (18/3) = 10Hz fullpage refresh min + uint8_t rowsToPrint = 3; do { // Find a row with something to print @@ -198,7 +172,7 @@ static int drawScreen(displayPort_t *displayPort) // 62hz charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, len); } } - while (++row < ROWS && --linesToPrint); + while (++row < ROWS && --rowsToPrint); if (row >= ROWS) { @@ -235,7 +209,7 @@ static int screenSize(const displayPort_t *displayPort) return SCREENSIZE; } -// Intercept writeString and write to a buffer instead (1st page of font file) +// Intercept writeString and write to a buffer instead (1st page of font file only) static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr) { UNUSED(displayPort); From 70cc857a43a6713dd95bc99fb30666ec81a12dd1 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sat, 18 Dec 2021 11:32:04 +0000 Subject: [PATCH 044/107] Final tidy up. --- src/main/io/displayport_hdzero_osd.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index 4862288c04..c442a00c71 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -63,11 +63,6 @@ static bool hdzeroVtxReady; static uint8_t screen[SCREENSIZE]; static uint8_t fontPage[SCREENSIZE/8+1]; // page bits for each character (to address 512 char font) -static int setHdMode(displayPort_t *displayPort); -static void resync(displayPort_t *displayPort); -static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr); -static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr); - extern uint8_t cliMode; static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len) From e09924edfa4e551a3b00ee095a6e2e38b6075098 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sat, 18 Dec 2021 11:32:32 +0000 Subject: [PATCH 045/107] Final tidy up. --- src/main/io/osd.h | 25 ++++--------------------- 1 file changed, 4 insertions(+), 21 deletions(-) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index be0651e50d..3b562701b2 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -29,37 +29,20 @@ #endif #define OSD_LAYOUT_COUNT (OSD_ALTERNATE_LAYOUT_COUNT + 1) -#if 1 - // 00vb yyyy yyxx xxxx // (visible)(blink)(yCoord)(xCoord) #define OSD_VISIBLE_FLAG 0x2000 #define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) -#define OSD_POS(x,y) ((x) | ((y) << 6)) -#define OSD_X(x) ((x) & 0x003F) -#define OSD_Y(x) (((x) >> 6) & 0x003F) +#define OSD_POS(x,y) (((x) & 0x3F) | (((y) & 0x3F) << 6)) +#define OSD_X(x) ((x) & 0x3F) +#define OSD_Y(x) (((x) >> 6) & 0x3F) #define OSD_POS_MAX 0xFFF // For DJI compatibility #define OSD_VISIBLE_FLAG_SD 0x0800 -#define OSD_POS_SD(x,y) ((x) | ((y) << 5)) - -#else - -// 0000 vbyy yyyx xxxx -// (visible)(blink)(yCoord)(xCoord) - -#define OSD_VISIBLE_FLAG 0x0800 -#define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) - -#define OSD_POS(x,y) ((x) | ((y) << 5)) -#define OSD_X(x) ((x) & 0x001F) -#define OSD_Y(x) (((x) >> 5) & 0x001F) -#define OSD_POS_MAX 0x3FF - -#endif +#define OSD_POS_SD(x,y) (((x) & 0x1F) | (((y) & 0x1F) << 5)) #define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) From 545843a9a10906646d17842c93459a5470e2cbba Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sun, 19 Dec 2021 09:06:35 +0000 Subject: [PATCH 046/107] Update DJI OSD module to map HD coordinate system to SD. --- src/main/io/osd_dji_hd.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 1c6fbf0f68..37e61e40dd 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -367,22 +367,21 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst) // INAV calculates these dynamically, while DJI expects the config to have defined coordinates switch(inavOSDIdx) { case OSD_CROSSHAIRS: - itemPosSD = (itemPosSD & (~OSD_POS_MAX)) | OSD_POS_SD(13, 6); + itemPosSD = OSD_POS_SD(15, 8); break; case OSD_ARTIFICIAL_HORIZON: - itemPosSD = (itemPosSD & (~OSD_POS_MAX)) | OSD_POS_SD(14, 2); + itemPosSD = OSD_POS_SD(9, 8); break; case OSD_HORIZON_SIDEBARS: - itemPosSD = (itemPosSD & (~OSD_POS_MAX)) | OSD_POS_SD(14, 5); + itemPosSD = OSD_POS_SD(16, 7); break; } // Enforce visibility in 3 BF OSD profiles if (OSD_VISIBLE(itemPos)) { - itemPosSD |= OSD_VISIBLE_FLAG_SD; - itemPosSD |= 0x3000; + itemPosSD |= (0x3000 | OSD_VISIBLE_FLAG_SD); } sbufWriteU16(dst, itemPosSD); From 0d4a24f6a0bf195a92f35ca90fb3916f92802c5e Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sun, 19 Dec 2021 15:13:02 +0000 Subject: [PATCH 047/107] Adhere to coding standards --- src/main/io/displayport_hdzero_osd.c | 292 ++++++++++++--------------- 1 file changed, 132 insertions(+), 160 deletions(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index c442a00c71..d1de7f5c37 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -28,6 +28,8 @@ #include "platform.h" +#define USE_OSD +#define USE_HDZERO_OSD #if defined(USE_OSD) && defined(USE_HDZERO_OSD) #include "common/utils.h" @@ -61,7 +63,7 @@ static bool hdzeroVtxReady; #define COLS 50 #define SCREENSIZE (ROWS*COLS) static uint8_t screen[SCREENSIZE]; -static uint8_t fontPage[SCREENSIZE/8+1]; // page bits for each character (to address 512 char font) +static uint8_t fontPage[SCREENSIZE / 8 + 1]; // page bits for each character (to address 512 char font) extern uint8_t cliMode; @@ -70,9 +72,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int UNUSED(displayPort); if (cliMode) - { return 0; - } return mspSerialPushPort(cmd, subcmd, len, &hdzeroMspPort, MSP_V1); } @@ -91,11 +91,11 @@ static int release(displayPort_t *displayPort) static int clearScreen(displayPort_t *displayPort) { - UNUSED(displayPort); + UNUSED(displayPort); - memset(screen, SYM_BLANK, sizeof(screen)); - memset(fontPage, 0, sizeof(fontPage)); - return 1; + memset(screen, SYM_BLANK, sizeof(screen)); + memset(fontPage, 0, sizeof(fontPage)); + return 1; } /* @@ -105,92 +105,75 @@ static int clearScreen(displayPort_t *displayPort) */ static int drawScreen(displayPort_t *displayPort) // 62.5hz { - int charsOut = 0; - static uint8_t row = 0, clearSent = 0; - uint8_t subcmd[COLS + 4]; - uint16_t lineIdx, idx, end; - uint8_t len, col, page, aPage; + static uint8_t row = 0, clearSent = 0; + uint8_t subcmd[COLS + 4], len, col, page, aPage, rowsToPrint; + uint16_t lineIdx, idx, end; + int charsOut = 0; - uint8_t rowsToPrint = 3; - do - { - // Find a row with something to print - do - { - // Strip leading and trailing spaces for the selected row - lineIdx = row*COLS; - idx = lineIdx; - end = idx + COLS - 1; + rowsToPrint = 3; + do { + // Find a row with something to print + do { + // Strip leading and trailing spaces for the selected row + lineIdx = row * COLS; + idx = lineIdx; + end = idx + COLS - 1; - while ((screen[idx] == SYM_BLANK || screen[end] == SYM_BLANK) && idx <= end) - { - if (screen[idx] == SYM_BLANK) - { - idx++; - } + while ((screen[idx] == SYM_BLANK || screen[end] == SYM_BLANK) && idx <= end) { + if (screen[idx] == SYM_BLANK) + idx++; + if (screen[end] == SYM_BLANK) + end--; + } + } while (idx > end && ++row < ROWS); - if (screen[end] == SYM_BLANK) - { - end--; - } - } - } - while (idx > end && ++row < ROWS); + while (idx <= end) { + if (!clearSent) { + // Start the transaction + subcmd[0] = MSP_CLEAR_SCREEN; + charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + clearSent = 1; + } - while (idx <= end) - { - if (!clearSent) - { - // Start the transaction - subcmd[0] = MSP_CLEAR_SCREEN; - charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); - clearSent = 1; - } + // Split the line up into strings from the same font page and output them. + // (note spaces are printed to save overhead on small elements) + len = 4; + col = idx - lineIdx; + page = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; - // Split the line up into strings from the same font page and output them. - // (note spaces are printed to save overhead on small elements) - len = 4; - col = idx-lineIdx; - page = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; + do { + subcmd[len++] = screen[idx++]; + aPage = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; + } while (idx <= end && (aPage == page || screen[idx] == SYM_BLANK)); - do - { - subcmd[len++] = screen[idx++]; - aPage = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; - } - while (idx <= end && (aPage == page || screen[idx] == SYM_BLANK)); + subcmd[0] = MSP_WRITE_STRING; + subcmd[1] = row; + subcmd[2] = col; + subcmd[3] = page; + charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, len); + } + } while (++row < ROWS && --rowsToPrint); - subcmd[0] = MSP_WRITE_STRING; - subcmd[1] = row; - subcmd[2] = col; - subcmd[3] = page; - charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, len); - } - } - while (++row < ROWS && --rowsToPrint); + if (row >= ROWS) { + // End the transaction if required and reset the counters + if (clearSent > 0) { + subcmd[0] = MSP_DRAW_SCREEN; + charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + } + row = clearSent = 0; + } - if (row >= ROWS) - { - // End the transaction if required and reset the counters - if (clearSent > 0) - { - subcmd[0] = MSP_DRAW_SCREEN; - charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); - } - row = clearSent = 0; - } - - return charsOut; + return charsOut; } static int setHdMode(displayPort_t *displayPort) { - uint8_t subcmd[3]; - subcmd[0] = MSP_SET_HD; - subcmd[1] = 0; // future font index - subcmd[2] = 1; // 0 SD 1 HD + uint8_t subcmd[3]; + subcmd[0] = MSP_SET_HD; + subcmd[1] = 0; // future font index + subcmd[2] = 1; // 0 SD 1 HD - return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); } static int grab(displayPort_t *displayPort) @@ -200,7 +183,8 @@ static int grab(displayPort_t *displayPort) static int screenSize(const displayPort_t *displayPort) { - UNUSED(displayPort); + UNUSED(displayPort); + return SCREENSIZE; } @@ -210,32 +194,29 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con UNUSED(displayPort); UNUSED(attr); - uint16_t pos = (row * COLS) + col; - if (pos >= SCREENSIZE) - { - return 0; - } + uint16_t i, pos, len, end, idx; - uint16_t len = strlen(string); + pos = (row * COLS) + col; + if (pos >= SCREENSIZE) + return 0; - // Allow word wrap and truncate of past the screen end - uint16_t end = pos + len - 1; - if (end >= SCREENSIZE) - { - len = end-SCREENSIZE; - } + len = strlen(string); - // Copy the string into the screen buffer - memcpy(screen + pos, string, len); + // Allow word wrap and truncate of past the screen end + end = pos + len - 1; + if (end >= SCREENSIZE) + len = end - SCREENSIZE; - // Clear the page bits for all the characters in the string - for (uint16_t i = 0; i < len; i++) - { - uint16_t idx = pos+i; - fontPage[idx >> 3] &= ~(1 << (idx & 0x07)); - } + // Copy the string into the screen buffer + memcpy(screen + pos, string, len); - return (int)len; + // Clear the page bits for all the characters in the string + for (i = 0; i < len; i++) { + idx = pos + i; + fontPage[idx >> 3] &= ~(1 << (idx & 0x07)); + } + + return (int) len; } // Write character to screen and page buffers (supports 512 char fonts) @@ -244,70 +225,66 @@ static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1 UNUSED(displayPort); UNUSED(attr); - uint16_t pos = (row * COLS) + col; - if (pos >= SCREENSIZE) - { - return 0; - } + uint16_t pos, idx; + uint8_t bitmask; - // Copy character into screen buffer - screen[pos] = c; + pos = (row * COLS) + col; + if (pos >= SCREENSIZE) + return 0; - uint16_t idx = pos >> 3; - uint8_t bitmask = 1 << (pos & 0x07); + // Copy character into screen buffer + screen[pos] = c; - // Save index of the character's font page - if (c & 0x0100) - { - fontPage[idx] |= bitmask; - } - else - { - fontPage[idx] &= ~bitmask; - } + idx = pos >> 3; + bitmask = 1 << (pos & 0x07); - return 1; + // Save index of the character's font page + if (c & 0x0100) + fontPage[idx] |= bitmask; + else + fontPage[idx] &= ~bitmask; + + return (int) 1; } static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr) { UNUSED(displayPort); - uint16_t pos = (row * COLS) + col; - if (pos >= SCREENSIZE) - { - *c = SYM_BLANK; - } - else - { - uint16_t chr = (fontPage[pos >> 3] >> (pos & 0x07)) & FONT_PAGE_ATTRIBUTE; - *c = (chr << 8) | screen[pos]; - } + uint16_t pos, chr; - if (attr) - { - *attr = TEXT_ATTRIBUTES_NONE; - } + pos = (row * COLS) + col; + if (pos >= SCREENSIZE) + *c = SYM_BLANK; + else { + chr = (fontPage[pos >> 3] >> (pos & 0x07)) & FONT_PAGE_ATTRIBUTE; + *c = (chr << 8) | screen[pos]; + } - return true; + if (attr) + *attr = TEXT_ATTRIBUTES_NONE; + + return true; } static bool isTransferInProgress(const displayPort_t *displayPort) { UNUSED(displayPort); + return false; } static void resync(displayPort_t *displayPort) { - displayPort->rows = ROWS; - displayPort->cols = COLS; - setHdMode(displayPort); + displayPort->rows = ROWS; + displayPort->cols = COLS; + setHdMode(displayPort); } static uint32_t txBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); + return mspSerialTxBytesFree(); } @@ -321,13 +298,12 @@ static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort return attr; } -// TODO: Is this needed? static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) { - UNUSED(displayPort); + UNUSED(displayPort); metadata->charCount = 512; - metadata->version = 3; + metadata->version = 1; return true; } @@ -335,7 +311,8 @@ static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t static bool isReady(displayPort_t *displayPort) { UNUSED(displayPort); - return hdzeroVtxReady; + + return hdzeroVtxReady; } static const displayPortVTable_t hdzeroOsdVTable = { @@ -353,42 +330,37 @@ static const displayPortVTable_t hdzeroOsdVTable = { .txBytesFree = txBytesFree, .supportedTextAttributes = supportedTextAttributes, .getFontMetadata = getFontMetadata, - .isReady = isReady, + .isReady = isReady, }; void hdzeroOsdSerialInit(void) { - memset(&hdzeroMspPort, 0, sizeof(mspPort_t)); + memset(&hdzeroMspPort, 0, sizeof(mspPort_t)); serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD); - if (portConfig) - { - serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, - NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); - + if (portConfig) { + serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, NULL, NULL, + baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); if (port) - { resetMspPort(&hdzeroMspPort, port); - } } } -displayPort_t *hdzeroOsdDisplayPortInit(void) +displayPort_t* hdzeroOsdDisplayPortInit(void) { - memset(screen, SYM_BLANK, sizeof(screen)); - memset(fontPage, 0, sizeof(fontPage)); + memset(screen, SYM_BLANK, sizeof(screen)); + memset(fontPage, 0, sizeof(fontPage)); displayInit(&hdzeroOsdDisplayPort, &hdzeroOsdVTable); return &hdzeroOsdDisplayPort; } void hdzeroOsdSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn) { - if (hdzeroMspPort.port) - { - // Process normal MSP command - mspSerialProcessOnePort(&hdzeroMspPort, evaluateNonMspData, mspProcessCommandFn); - hdzeroVtxReady = true; + if (hdzeroMspPort.port) { + // Process normal MSP command + mspSerialProcessOnePort(&hdzeroMspPort, evaluateNonMspData, mspProcessCommandFn); + hdzeroVtxReady = true; } } From 6dd2e50428678421a3dc3ec5c9299da2271ba836 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sun, 19 Dec 2021 15:34:21 +0000 Subject: [PATCH 048/107] Set font version to min required of 3. --- src/main/io/displayport_hdzero_osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index d1de7f5c37..b6f05d8317 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -303,7 +303,7 @@ static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t UNUSED(displayPort); metadata->charCount = 512; - metadata->version = 1; + metadata->version = 3; return true; } From 74cf8a47395185ed1d9ab47afe030bc6ac1e889b Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Mon, 20 Dec 2021 10:43:36 +0000 Subject: [PATCH 049/107] Remove test #defines --- src/main/io/displayport_hdzero_osd.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index b6f05d8317..6483610b2e 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -28,8 +28,6 @@ #include "platform.h" -#define USE_OSD -#define USE_HDZERO_OSD #if defined(USE_OSD) && defined(USE_HDZERO_OSD) #include "common/utils.h" From 93686b3219d590a137d98f1b267d4919f83e25a8 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 21 Dec 2021 17:51:12 -0300 Subject: [PATCH 050/107] an attempt to fix the error --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index f1b4b1cd88..69b46b2968 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2144,7 +2144,7 @@ If defined to 'OFF', it will ignore the gyroscope calibration done at each start ### ins_gravity_cmss -Calculated 1G of Acc axis Z to use in INS +Calculated 1G of acc axis Z to use in INS | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bd290f64e3..96fd33dffd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -333,7 +333,7 @@ groups: min: INT16_MIN max: INT16_MAX - name: ins_gravity_cmss - description: "Calculated 1G of Acc axis Z to use in INS" + description: "Calculated 1G of acc axis Z to use in INS" default_value: 0.0 field: gravity_cmss_cal min: 0 From cd7751d245ed41337bb29bfdfd4e1a5d959fd1be Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 21 Dec 2021 17:57:23 -0300 Subject: [PATCH 051/107] new tentative --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/utils/update_cli_docs.py | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 69b46b2968..f1b4b1cd88 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2144,7 +2144,7 @@ If defined to 'OFF', it will ignore the gyroscope calibration done at each start ### ins_gravity_cmss -Calculated 1G of acc axis Z to use in INS +Calculated 1G of Acc axis Z to use in INS | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 96fd33dffd..bd290f64e3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -333,7 +333,7 @@ groups: min: INT16_MIN max: INT16_MAX - name: ins_gravity_cmss - description: "Calculated 1G of acc axis Z to use in INS" + description: "Calculated 1G of Acc axis Z to use in INS" default_value: 0.0 field: gravity_cmss_cal min: 0 diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index 0c72c6a759..6d68a249bf 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -63,6 +63,8 @@ def generate_md_from_yaml(settings_yaml): # Replace booleans with "ON"/"OFF" if type(member[key]) == bool: member[key] = "ON" if member[key] else "OFF" + member["min"] = "OFF" + member["max"] = "ON" # Replace zero placeholder with actual zero elif member[key] == ":zero": member[key] = 0 From 4585464a74288818f79b2299651f02c147113dfe Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 21 Dec 2021 18:05:02 -0300 Subject: [PATCH 052/107] update docs --- docs/Settings.md | 134 +++++++++++++++++++++++------------------------ 1 file changed, 67 insertions(+), 67 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index f1b4b1cd88..5c8e547388 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -388,7 +388,7 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -458,7 +458,7 @@ Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if t | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -518,7 +518,7 @@ This option is only available on certain architectures (F3 CPUs at the moment). | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -638,7 +638,7 @@ Disarms the motors independently of throttle value. Setting to OFF reverts to th | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -648,7 +648,7 @@ OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values a | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -698,7 +698,7 @@ Show inflight adjustments in craft name field | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -708,7 +708,7 @@ Re-purpose the craft name field for messages. | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -728,7 +728,7 @@ Whether using DShot motors as beepers is enabled | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -788,7 +788,7 @@ Enable/disable dynamic gyro notch also known as Matrix Filter | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -838,7 +838,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -868,7 +868,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -888,7 +888,7 @@ Enable when BLHeli32 Auto Telemetry function is used. Disable in every other cas | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -938,7 +938,7 @@ Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The tar | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -988,7 +988,7 @@ If set to `OFF` the failsafe procedure won't be triggered and the mission will c | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -1058,7 +1058,7 @@ Auto-arm fixed wing aircraft on throttle above min_check, and disarming with sti | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1118,7 +1118,7 @@ S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw acc | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1438,7 +1438,7 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -1448,7 +1448,7 @@ Enable automatic configuration of UBlox GPS receivers. | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -1498,7 +1498,7 @@ Enable use of Galileo satellites. This is at the expense of other regional const | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1598,7 +1598,7 @@ Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1638,7 +1638,7 @@ Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1818,7 +1818,7 @@ If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1828,7 +1828,7 @@ If set to ON, Secondary IMU data will be used for Analog OSD heading | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1838,7 +1838,7 @@ If set to ON, Secondary IMU data will be used for Angle, Horizon and all other m | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1908,7 +1908,7 @@ Defines if inav will dead-reckon over short GPS outages. May also be useful for | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1918,7 +1918,7 @@ Automatic setting of magnetic declination based on GPS position. When used manua | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -1988,7 +1988,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -1998,7 +1998,7 @@ Defined if iNav should use velocity data provided by GPS module for doing positi | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -2138,7 +2138,7 @@ If defined to 'OFF', it will ignore the gyroscope calibration done at each start | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -2168,7 +2168,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -2818,7 +2818,7 @@ Use if you need to inverse yaw motor direction. | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -2898,7 +2898,7 @@ If set to ON, iNav disarms the FC after landing | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -2928,7 +2928,7 @@ Enable the possibility to manually increase the throttle in auto throttle contro | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3328,7 +3328,7 @@ Stops motor when Soaring mode enabled. | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3718,7 +3718,7 @@ When ON, NAV engine will slow down when switching to the next waypoint. This pri | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -3738,7 +3738,7 @@ With Reset ON WP Mission Planner waypoint count can be reset to 0 by toggling th | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -3788,7 +3788,7 @@ If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3848,7 +3848,7 @@ If set to ON, aircraft will execute initial climb regardless of position sensor | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3868,7 +3868,7 @@ If set to ON drone will return tail-first. Obviously meaningless for airplanes. | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3878,7 +3878,7 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3888,7 +3888,7 @@ If set to OFF, the FC remembers your throttle stick position when enabling ALTHO | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3908,7 +3908,7 @@ If set to ON, waypoints will be automatically loaded from EEPROM to the FC durin | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3978,7 +3978,7 @@ Shows a border/corners around the AHI region (pixel OSD only) | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -3988,7 +3988,7 @@ When set to `ON`, the AHI position is adjusted by `osd_camera_uptilt`. For examp | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4018,7 +4018,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4218,7 +4218,7 @@ Use wind estimation for remaining flight time/distance estimation | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -4228,7 +4228,7 @@ If enabled the OSD automatically switches to the first layout during failsafe | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4238,7 +4238,7 @@ Force OSD to work in grid mode even if the OSD device supports pixel level acces | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4278,7 +4278,7 @@ Should home position coordinates be displayed on the arming screen. | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -4298,7 +4298,7 @@ To 3D-display the home point location in the hud | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4308,7 +4308,7 @@ To display little arrows around the crossair showing where the home point is in | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4568,7 +4568,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4828,7 +4828,7 @@ Selection of rangefinder hardware. | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4968,7 +4968,7 @@ S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -4988,7 +4988,7 @@ Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rot | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -5188,7 +5188,7 @@ Reverse the serial inversion of the serial RX protocol. When this value is OFF, | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -5258,7 +5258,7 @@ Enable Kalman filter on the gyro data | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -5348,7 +5348,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -5358,7 +5358,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -5408,7 +5408,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -5428,7 +5428,7 @@ General switch of the statistics recording feature (a.k.a. odometer) | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -5478,7 +5478,7 @@ S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -5488,7 +5488,7 @@ Determines if the telemetry protocol default signal inversion is reversed. This | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -5498,7 +5498,7 @@ Which aux channel to use to change serial output & baud rate (MSP / Telemetry). | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- @@ -5588,7 +5588,7 @@ On tricopter mix only, if this is set to ON, servo will always be correcting reg | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -5718,7 +5718,7 @@ Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- @@ -5768,7 +5768,7 @@ Enable workaround for early AKK SAudio-enabled VTX bug. | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- From dfb4d07c10cc41e2ad66fd397fb656c34e7c2ca3 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 21 Dec 2021 18:10:43 -0300 Subject: [PATCH 053/107] update docs again --- docs/Settings.md | 134 +++++++++++++++++------------------ src/utils/update_cli_docs.py | 2 - 2 files changed, 67 insertions(+), 69 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5c8e547388..f1b4b1cd88 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -388,7 +388,7 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -458,7 +458,7 @@ Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if t | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -518,7 +518,7 @@ This option is only available on certain architectures (F3 CPUs at the moment). | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -638,7 +638,7 @@ Disarms the motors independently of throttle value. Setting to OFF reverts to th | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -648,7 +648,7 @@ OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values a | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -698,7 +698,7 @@ Show inflight adjustments in craft name field | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -708,7 +708,7 @@ Re-purpose the craft name field for messages. | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -728,7 +728,7 @@ Whether using DShot motors as beepers is enabled | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -788,7 +788,7 @@ Enable/disable dynamic gyro notch also known as Matrix Filter | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -838,7 +838,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -868,7 +868,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -888,7 +888,7 @@ Enable when BLHeli32 Auto Telemetry function is used. Disable in every other cas | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -938,7 +938,7 @@ Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The tar | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -988,7 +988,7 @@ If set to `OFF` the failsafe procedure won't be triggered and the mission will c | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -1058,7 +1058,7 @@ Auto-arm fixed wing aircraft on throttle above min_check, and disarming with sti | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1118,7 +1118,7 @@ S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw acc | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1438,7 +1438,7 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -1448,7 +1448,7 @@ Enable automatic configuration of UBlox GPS receivers. | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -1498,7 +1498,7 @@ Enable use of Galileo satellites. This is at the expense of other regional const | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1598,7 +1598,7 @@ Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1638,7 +1638,7 @@ Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1818,7 +1818,7 @@ If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1828,7 +1828,7 @@ If set to ON, Secondary IMU data will be used for Analog OSD heading | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1838,7 +1838,7 @@ If set to ON, Secondary IMU data will be used for Angle, Horizon and all other m | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1908,7 +1908,7 @@ Defines if inav will dead-reckon over short GPS outages. May also be useful for | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1918,7 +1918,7 @@ Automatic setting of magnetic declination based on GPS position. When used manua | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -1988,7 +1988,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -1998,7 +1998,7 @@ Defined if iNav should use velocity data provided by GPS module for doing positi | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -2138,7 +2138,7 @@ If defined to 'OFF', it will ignore the gyroscope calibration done at each start | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -2168,7 +2168,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -2818,7 +2818,7 @@ Use if you need to inverse yaw motor direction. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -2898,7 +2898,7 @@ If set to ON, iNav disarms the FC after landing | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -2928,7 +2928,7 @@ Enable the possibility to manually increase the throttle in auto throttle contro | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3328,7 +3328,7 @@ Stops motor when Soaring mode enabled. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3718,7 +3718,7 @@ When ON, NAV engine will slow down when switching to the next waypoint. This pri | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -3738,7 +3738,7 @@ With Reset ON WP Mission Planner waypoint count can be reset to 0 by toggling th | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -3788,7 +3788,7 @@ If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3848,7 +3848,7 @@ If set to ON, aircraft will execute initial climb regardless of position sensor | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3868,7 +3868,7 @@ If set to ON drone will return tail-first. Obviously meaningless for airplanes. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3878,7 +3878,7 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3888,7 +3888,7 @@ If set to OFF, the FC remembers your throttle stick position when enabling ALTHO | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3908,7 +3908,7 @@ If set to ON, waypoints will be automatically loaded from EEPROM to the FC durin | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3978,7 +3978,7 @@ Shows a border/corners around the AHI region (pixel OSD only) | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -3988,7 +3988,7 @@ When set to `ON`, the AHI position is adjusted by `osd_camera_uptilt`. For examp | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4018,7 +4018,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4218,7 +4218,7 @@ Use wind estimation for remaining flight time/distance estimation | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -4228,7 +4228,7 @@ If enabled the OSD automatically switches to the first layout during failsafe | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4238,7 +4238,7 @@ Force OSD to work in grid mode even if the OSD device supports pixel level acces | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4278,7 +4278,7 @@ Should home position coordinates be displayed on the arming screen. | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -4298,7 +4298,7 @@ To 3D-display the home point location in the hud | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4308,7 +4308,7 @@ To display little arrows around the crossair showing where the home point is in | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4568,7 +4568,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4828,7 +4828,7 @@ Selection of rangefinder hardware. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4968,7 +4968,7 @@ S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -4988,7 +4988,7 @@ Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rot | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -5188,7 +5188,7 @@ Reverse the serial inversion of the serial RX protocol. When this value is OFF, | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -5258,7 +5258,7 @@ Enable Kalman filter on the gyro data | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -5348,7 +5348,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -5358,7 +5358,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -5408,7 +5408,7 @@ _// TODO_ | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -5428,7 +5428,7 @@ General switch of the statistics recording feature (a.k.a. odometer) | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -5478,7 +5478,7 @@ S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -5488,7 +5488,7 @@ Determines if the telemetry protocol default signal inversion is reversed. This | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -5498,7 +5498,7 @@ Which aux channel to use to change serial output & baud rate (MSP / Telemetry). | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| OFF | | | --- @@ -5588,7 +5588,7 @@ On tricopter mix only, if this is set to ON, servo will always be correcting reg | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -5718,7 +5718,7 @@ Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- @@ -5768,7 +5768,7 @@ Enable workaround for early AKK SAudio-enabled VTX bug. | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| ON | | | --- diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index 6d68a249bf..0c72c6a759 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -63,8 +63,6 @@ def generate_md_from_yaml(settings_yaml): # Replace booleans with "ON"/"OFF" if type(member[key]) == bool: member[key] = "ON" if member[key] else "OFF" - member["min"] = "OFF" - member["max"] = "ON" # Replace zero placeholder with actual zero elif member[key] == ":zero": member[key] = 0 From 684fc96046d57682cff14e766dc0276858d62d4d Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Wed, 22 Dec 2021 11:52:31 +0000 Subject: [PATCH 054/107] Update settings documentation --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 287c379a9b..8333281840 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4614,7 +4614,7 @@ IMPERIAL, METRIC, UK ### osd_video_system -Video system used. Possible values are `AUTO`, `PAL` and `NTSC` +Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, and `HD` | Default | Min | Max | | --- | --- | --- | From b316d74c977071174caca71be9423b7bf866a702 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= Date: Thu, 23 Dec 2021 12:31:09 +0100 Subject: [PATCH 055/107] Merge pull request #7630 from OptimusTi/RSSI%-Rework-for-CRSF Rework of RSSI% for CRSF/ExpLRS --- docs/Settings.md | 20 ++++++++++++++++++++ src/main/cms/cms_menu_osd.c | 1 + src/main/fc/settings.yaml | 14 ++++++++++++++ src/main/io/osd.c | 2 ++ src/main/io/osd.h | 2 ++ src/main/rx/crsf.c | 12 ++++++++++-- 6 files changed, 49 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 287c379a9b..3df4b32afc 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4492,6 +4492,26 @@ RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm --- +### osd_rssi_dbm_max + +RSSI dBm upper end of curve. Perfect rssi (max) = 100% + +| Default | Min | Max | +| --- | --- | --- | +| -30 | -50 | 0 | + +--- + +### osd_rssi_dbm_min + +RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0% + +| Default | Min | Max | +| --- | --- | --- | +| -120 | -130 | 0 | + +--- + ### osd_sidebar_height Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index f95b7bef2a..1cff131e75 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -146,6 +146,7 @@ static const OSD_Entry menuCrsfRxEntries[]= OSD_SETTING_ENTRY("LQ FORMAT", SETTING_OSD_CRSF_LQ_FORMAT), OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM), OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM), + OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN), OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM), OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ), OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d27ffc3cbd..be23f2ac42 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3222,6 +3222,20 @@ groups: field: rssi_dbm_alarm min: -130 max: 0 + - name: osd_rssi_dbm_max + condition: USE_SERIALRX_CRSF + description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%" + default_value: -30 + field: rssi_dbm_max + min: -50 + max: 0 + - name: osd_rssi_dbm_min + condition: USE_SERIALRX_CRSF + description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%" + default_value: -120 + field: rssi_dbm_min + 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" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index cb46c7d713..3aa472ce4f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3192,6 +3192,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .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, + .rssi_dbm_max = SETTING_OSD_RSSI_DBM_MAX_DEFAULT, + .rssi_dbm_min = SETTING_OSD_RSSI_DBM_MIN_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 75b8aefeff..ded916981f 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -323,6 +323,8 @@ typedef struct osdConfig_s { int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO int16_t baro_temp_alarm_min; diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index f01dcb965e..01b9a72abd 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -36,6 +36,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/serial_uart.h" #include "io/serial.h" +#include "io/osd.h" #include "rx/rx.h" #include "rx/crsf.h" @@ -241,8 +242,15 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower]; rxLinkStatistics.activeAnt = linkStats->activeAntenna; - if (rxLinkStatistics.uplinkLQ > 0) - lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rxLinkStatistics.uplinkRSSI, -120, -30), -120, -30, 0, RSSI_MAX_VALUE)); + if (rxLinkStatistics.uplinkLQ > 0) { + int16_t uplinkStrength; // RSSI dBm converted to % + uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (100 * sq((osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI)))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); + if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max ) + uplinkStrength = 99; + else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min) + uplinkStrength = 0; + lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(uplinkStrength, 0, 99, 0, RSSI_MAX_VALUE)); + } else lqTrackerSet(rxRuntimeConfig->lqTracker, 0); From 9bbd9741f597a7a80701f3e76b158176de444aff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= Date: Thu, 23 Dec 2021 12:41:51 +0100 Subject: [PATCH 056/107] Merge pull request #7597 from iNavFlight/dzikuvx-matrix-filter-rework Matrix filter rework --- src/main/flight/dynamic_gyro_notch.c | 46 ++++++----- src/main/flight/dynamic_gyro_notch.h | 13 ++-- src/main/flight/gyroanalyse.c | 109 +++++++++++++++++---------- src/main/flight/gyroanalyse.h | 15 +++- src/main/sensors/gyro.c | 4 +- src/main/sensors/gyro.h | 5 ++ 6 files changed, 122 insertions(+), 70 deletions(-) diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c index 7e6c12afbc..fa4c48e913 100644 --- a/src/main/flight/dynamic_gyro_notch.c +++ b/src/main/flight/dynamic_gyro_notch.c @@ -26,6 +26,8 @@ #ifdef USE_DYNAMIC_FILTERS +FILE_COMPILE_FOR_SPEED + #include #include "dynamic_gyro_notch.h" #include "fc/config.h" @@ -33,7 +35,12 @@ #include "sensors/gyro.h" void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { - state->filtersApplyFn = nullFilterApply; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->filtersApplyFn[axis][i] = nullFilterApply; + } + } state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; state->enabled = gyroConfig()->dynamicGyroNotchEnabled; @@ -45,27 +52,32 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { //Any initial notch Q is valid sice it will be updated immediately after - biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + biquadFilterInit(&state->filters[axis][i], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[axis][i] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + } - state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; } } -void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) { +void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]) { - state->frequency[axis] = frequency; - - DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency); - - if (state->enabled) { - biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); + if (axis == FD_ROLL) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, i, frequency[i]); + } } + if (state->enabled) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + // Filter update happens only if peak was detected + if (frequency[i] > 0.0f) { + biquadFilterUpdate(&state->filters[axis][i], frequency[i], state->looptime, state->dynNotchQ, FILTER_NOTCH); + } + } + } } float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) { @@ -75,9 +87,9 @@ float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, flo * We always apply all filters. If a filter dimension is disabled, one of * the function pointers will be a null apply function */ - output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output); - output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output); - output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output); + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + output = state->filtersApplyFn[axis][i]((filter_t *)&state->filters[axis][i], output); + } return output; } diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index 87d923fb06..c5be6a8137 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -27,23 +27,22 @@ #include #include "common/axis.h" #include "common/filter.h" +#include "sensors/gyro.h" #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 typedef struct dynamicGyroNotchState_s { - uint16_t frequency[XYZ_AXIS_COUNT]; + // uint16_t frequency[XYZ_AXIS_COUNT]; float dynNotchQ; float dynNotch1Ctr; float dynNotch2Ctr; uint32_t looptime; uint8_t enabled; - /* - * Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type - */ - biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT]; - filterApplyFnPtr filtersApplyFn; + + biquadFilter_t filters[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; } dynamicGyroNotchState_t; void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state); -void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency); +void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]); float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input); diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 2037b4712c..7596583273 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -61,7 +61,7 @@ enum { // NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // smoothing frequency for FFT centre frequency -#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 +#define DYN_NOTCH_SMOOTH_FREQ_HZ 25 /* * Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution. @@ -93,11 +93,12 @@ void gyroDataAnalyseStateInit( const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // any init value - state->centerFreq[axis] = state->maxFrequency; - state->prevCenterFreq[axis] = state->maxFrequency; + + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->centerFrequency[axis][i] = state->maxFrequency; + pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f); + } - biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs); } } @@ -135,18 +136,6 @@ void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); -static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) { - uint8_t peakBinIndex = state->fftStartBin; - float peakValue = 0; - for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) { - if (state->fftData[i] > peakValue) { - peakValue = state->fftData[i]; - peakBinIndex = i; - } - } - return peakBinIndex; -} - static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) { float preciseBin = peakBinIndex; @@ -190,38 +179,78 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) // 8us arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); - //Find peak frequency - uint8_t peakBin = findPeakBinIndex(state); + //Zero the data structure + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->peaks[i].bin = 0; + state->peaks[i].value = 0.0f; + } - // Failsafe to ensure the last bin is not a peak bin - peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1); + // Find peaks + for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT - 1; bin++) { + /* + * Peak is defined if the current bin is greater than the previous bin and the next bin + */ + if ( + state->fftData[bin] > state->fftData[bin - 1] && + state->fftData[bin] > state->fftData[bin + 1] + ) { + /* + * We are only interested in N biggest peaks + * Check previously found peaks and update the structure if necessary + */ + for (int p = 0; p < DYN_NOTCH_PEAK_COUNT; p++) { + if (state->fftData[bin] > state->peaks[p].value) { + for (int k = DYN_NOTCH_PEAK_COUNT - 1; k > p; k--) { + state->peaks[k] = state->peaks[k - 1]; + } + state->peaks[p].bin = bin; + state->peaks[p].value = state->fftData[bin]; + break; + } + } + bin++; // If bin is peak, next bin can't be peak => jump it + } + } - /* - * Calculate center frequency using the parabola method - */ - float preciseBin = computeParabolaMean(state, peakBin); - float peakFrequency = preciseBin * state->fftResolution; + // Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0) + for (int p = DYN_NOTCH_PEAK_COUNT - 1; p > 0; p--) { + for (int k = 0; k < p; k++) { + // Swap peaks but ignore swapping void peaks (bin = 0). This leaves + // void peaks at the end of peaks array without moving them + if (state->peaks[k].bin > state->peaks[k + 1].bin && state->peaks[k + 1].bin != 0) { + peak_t temp = state->peaks[k]; + state->peaks[k] = state->peaks[k + 1]; + state->peaks[k + 1] = temp; + } + } + } - peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency); - peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency); - - state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; - state->centerFreq[state->updateAxis] = peakFrequency; break; } case STEP_UPDATE_FILTERS_AND_HANNING: { - // 7us - // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) - if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { - /* - * Filters will be updated inside dynamicGyroNotchFiltersUpdate() - */ - state->filterUpdateExecute = true; - state->filterUpdateAxis = state->updateAxis; - state->filterUpdateFrequency = state->centerFreq[state->updateAxis]; + + /* + * Update frequencies + */ + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + + if (state->peaks[i].bin > 0) { + const int bin = constrain(state->peaks[i].bin, state->fftStartBin, FFT_BIN_COUNT - 1); + float frequency = computeParabolaMean(state, bin) * state->fftResolution; + + state->centerFrequency[state->updateAxis][i] = pt1FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency); + } else { + state->centerFrequency[state->updateAxis][i] = 0.0f; + } } + /* + * Filters will be updated inside dynamicGyroNotchFiltersUpdate() + */ + state->filterUpdateExecute = true; + state->filterUpdateAxis = state->updateAxis; + //Switch to the next axis state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index 0d95be42cc..2cfd19389f 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -31,6 +31,11 @@ */ #define FFT_WINDOW_SIZE 64 +typedef struct peak_s { + int bin; + float value; +} peak_t; + typedef struct gyroAnalyseState_s { // accumulator for oversampled data => no aliasing and less noise float currentSample[XYZ_AXIS_COUNT]; @@ -40,7 +45,6 @@ typedef struct gyroAnalyseState_s { float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; // update state machine step information - uint8_t updateTicks; uint8_t updateStep; uint8_t updateAxis; @@ -48,12 +52,15 @@ typedef struct gyroAnalyseState_s { float fftData[FFT_WINDOW_SIZE]; float rfftData[FFT_WINDOW_SIZE]; - biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; - uint16_t centerFreq[XYZ_AXIS_COUNT]; - uint16_t prevCenterFreq[XYZ_AXIS_COUNT]; + pt1Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + + peak_t peaks[DYN_NOTCH_PEAK_COUNT]; + bool filterUpdateExecute; uint8_t filterUpdateAxis; uint16_t filterUpdateFrequency; + uint16_t fftSamplingRateHz; uint8_t fftStartBin; float fftResolution; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8b14932ce2..d5ba164ef2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -479,8 +479,8 @@ void FAST_CODE NOINLINE gyroFilter() if (gyroAnalyseState.filterUpdateExecute) { dynamicGyroNotchFiltersUpdate( &dynamicGyroNotchState, - gyroAnalyseState.filterUpdateAxis, - gyroAnalyseState.filterUpdateFrequency + gyroAnalyseState.filterUpdateAxis, + gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] ); } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 44d567e570..18a3fe4ecb 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -24,6 +24,11 @@ #include "config/parameter_group.h" #include "drivers/sensor.h" +/* + * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters + */ +#define DYN_NOTCH_PEAK_COUNT 3 + typedef enum { GYRO_NONE = 0, GYRO_AUTODETECT, From 28279e5c934d5d94a383b26c45f583c307399aa9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 23 Dec 2021 12:46:37 +0100 Subject: [PATCH 057/107] Update docs --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3df4b32afc..84d0528a3e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5414,7 +5414,7 @@ Total flight distance [in meters]. The value is updated on every disarm when "st ### stats_total_energy -_// TODO_ +Total energy consumption [in mWh]. The value is updated on every disarm when "stats" are enabled. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index be23f2ac42..d2e57316b1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3572,6 +3572,7 @@ groups: default_value: 0 max: INT32_MAX - name: stats_total_energy + description: "Total energy consumption [in mWh]. The value is updated on every disarm when \"stats\" are enabled." max: INT32_MAX condition: USE_ADC default_value: 0 From 0404709cf910da4c95f87c91e175b78764a6f6bc Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Thu, 23 Dec 2021 18:57:32 -0300 Subject: [PATCH 058/107] update docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index f1b4b1cd88..5be5bc8be8 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2138,7 +2138,7 @@ If defined to 'OFF', it will ignore the gyroscope calibration done at each start | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- From a2d7e80c11623495f9de377717de3a9168975446 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 24 Dec 2021 19:02:44 +0000 Subject: [PATCH 059/107] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 53 +++++++++++++++++++++------------------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 34404a6fe2..946b419cf9 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -185,6 +185,9 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXHEADINGHOLD; + //Camstab mode is enabled always + activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; + if (STATE(MULTIROTOR)) { if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) { activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; @@ -196,45 +199,43 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; } - //Camstab mode is enabled always - activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; - - bool readyAltControl = false; + bool navReadyAltControl = false; if (STATE(ALTITUDE_CONTROL) && sensors(SENSOR_BARO)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; - readyAltControl = true; + navReadyAltControl = true; + } #ifdef USE_GPS - } else if ((feature(FEATURE_GPS) && STATE(AIRPLANE) && positionEstimationConfig()->use_gps_no_baro)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; - readyAltControl = true; + if ((feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro))) { + navReadyAltControl = true; } - if (readyAltControl) { - const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navReadyOther = !STATE(MULTIROTOR) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; - if (navFlowDeadReckoning || navReadyMultirotor || navReadyOther) { - if (!STATE(ROVER) && !STATE(BOAT)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; - } - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; - } - } + const bool navReadyBaseline = sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && navReadyBaseline; + const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; - if (navReadyMultirotor || navReadyOther) { + if (navReadyAltControl && (navReadyMultirotor || navReadyBaseline || navFlowDeadReckoning)) { + if (!STATE(ROVER) && !STATE(BOAT)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + } + if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; + } + } + + if (navReadyMultirotor || navReadyBaseline) { + if (navReadyAltControl) { activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; activeBoxIds[activeBoxIdCount++] = BOXPLANWPMISSION; - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; - activeBoxIds[activeBoxIdCount++] = BOXSOARING; } + } else if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; + activeBoxIds[activeBoxIdCount++] = BOXSOARING; } + } #ifdef USE_MR_BRAKING_MODE if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { @@ -242,6 +243,8 @@ void initActiveBoxIds(void) } #endif #endif // GPS + if (navReadyAltControl) { + activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; } if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { From 9a48088952ac48df433376e6a293fb37506bff14 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 24 Dec 2021 22:57:29 +0000 Subject: [PATCH 060/107] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 946b419cf9..953ec75a14 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -200,7 +200,7 @@ void initActiveBoxIds(void) } bool navReadyAltControl = false; - if (STATE(ALTITUDE_CONTROL) && sensors(SENSOR_BARO)) { + if (sensors(SENSOR_BARO)) { navReadyAltControl = true; } #ifdef USE_GPS @@ -208,21 +208,21 @@ void initActiveBoxIds(void) navReadyAltControl = true; } - const bool navReadyBaseline = sensors(SENSOR_ACC) && feature(FEATURE_GPS); - const bool navReadyMultirotor = STATE(MULTIROTOR) && (getHwCompassStatus() != HW_SENSOR_NONE) && navReadyBaseline; + bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS); + if (STATE(MULTIROTOR)) { + navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE; + } const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; - if (navReadyAltControl && (navReadyMultirotor || navReadyBaseline || navFlowDeadReckoning)) { - if (!STATE(ROVER) && !STATE(BOAT)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; - } + if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; } } - if (navReadyMultirotor || navReadyBaseline) { - if (navReadyAltControl) { + if (navReadyPosControl) { + if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) { activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; @@ -231,7 +231,9 @@ void initActiveBoxIds(void) if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; } - } else if (STATE(AIRPLANE)) { + } + + if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; activeBoxIds[activeBoxIdCount++] = BOXSOARING; } @@ -243,7 +245,7 @@ void initActiveBoxIds(void) } #endif #endif // GPS - if (navReadyAltControl) { + if (STATE(ALTITUDE_CONTROL) && navReadyAltControl) { activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; } From 3a5a3c0ada092dfbb6b5efbec3b2e389a854afd3 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 24 Dec 2021 23:43:33 +0000 Subject: [PATCH 061/107] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 953ec75a14..aab73d38be 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -199,20 +199,15 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; } - bool navReadyAltControl = false; - if (sensors(SENSOR_BARO)) { - navReadyAltControl = true; - } + bool navReadyAltControl = sensors(SENSOR_BARO); #ifdef USE_GPS - if ((feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro))) { - navReadyAltControl = true; - } + navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro)); + const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS); if (STATE(MULTIROTOR)) { navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE; } - const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) { activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; @@ -228,12 +223,10 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; activeBoxIds[activeBoxIdCount++] = BOXPLANWPMISSION; - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; - } } if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; activeBoxIds[activeBoxIdCount++] = BOXSOARING; } From 44e51726e3d3840f43627ef5e715488e880ada60 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 24 Dec 2021 23:55:37 +0000 Subject: [PATCH 062/107] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index aab73d38be..fe9b4c9eb2 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -233,9 +233,9 @@ void initActiveBoxIds(void) } #ifdef USE_MR_BRAKING_MODE - if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { - activeBoxIds[activeBoxIdCount++] = BOXBRAKING; - } + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + activeBoxIds[activeBoxIdCount++] = BOXBRAKING; + } #endif #endif // GPS if (STATE(ALTITUDE_CONTROL) && navReadyAltControl) { From c754b221f9c07e8a962592530731042e279a1411 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= Date: Mon, 27 Dec 2021 14:58:22 +0100 Subject: [PATCH 063/107] Merge pull request #7683 from iNavFlight/bkleiner-h7-spi-sdcard h7: switch spi to LL implementation --- cmake/stm32h7.cmake | 4 +- .../Src/stm32h7xx_ll_spi.c | 2 + src/main/drivers/bus_spi.h | 7 +- src/main/drivers/bus_spi_hal.c | 239 ------------------ src/main/drivers/bus_spi_hal_ll.c | 96 +++++-- src/main/target/KAKUTEH7/target.h | 11 +- 6 files changed, 92 insertions(+), 267 deletions(-) delete mode 100644 src/main/drivers/bus_spi_hal.c diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake index c936bf0dbc..70400fb9f0 100644 --- a/cmake/stm32h7.cmake +++ b/cmake/stm32h7.cmake @@ -114,7 +114,7 @@ set(STM32H7_HAL_SRC # stm32h7xx_ll_rng.c # stm32h7xx_ll_rtc.c stm32h7xx_ll_sdmmc.c -# stm32h7xx_ll_spi.c + stm32h7xx_ll_spi.c # stm32h7xx_ll_swpmi.c stm32h7xx_ll_tim.c # stm32h7xx_ll_usart.c @@ -148,7 +148,7 @@ main_sources(STM32H7_SRC drivers/adc_stm32h7xx.c drivers/bus_i2c_hal.c drivers/dma_stm32h7xx.c - drivers/bus_spi_hal.c + drivers/bus_spi_hal_ll.c drivers/memprot.h drivers/memprot_hal.c drivers/memprot_stm32h7xx.c diff --git a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c index 44a95c1256..05a6e5e993 100644 --- a/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c +++ b/lib/main/STM32H7/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_ll_spi.c @@ -28,8 +28,10 @@ #ifdef USE_FULL_ASSERT #include "stm32_assert.h" #else +#ifndef assert_param #define assert_param(expr) ((void)0U) #endif +#endif /** @addtogroup STM32H7xx_LL_Driver * @{ diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 24dc486038..2f5f8e0be7 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -84,9 +84,4 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); SPIDevice spiDeviceByInstance(SPI_TypeDef *instance); -SPI_TypeDef * spiInstanceByDevice(SPIDevice device); - -#if defined(USE_HAL_DRIVER) -SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance); -DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size); -#endif +SPI_TypeDef * spiInstanceByDevice(SPIDevice device); \ No newline at end of file diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c deleted file mode 100644 index 748dd25daa..0000000000 --- a/src/main/drivers/bus_spi_hal.c +++ /dev/null @@ -1,239 +0,0 @@ -/* - * 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 -#include - -#include - -#include "build/debug.h" -#include "drivers/bus_spi.h" -#include "dma.h" -#include "drivers/io.h" -#include "io_impl.h" -#include "drivers/nvic.h" -#include "rcc.h" - -#ifndef SPI1_NSS_PIN -#define SPI1_NSS_PIN NONE -#endif -#ifndef SPI2_NSS_PIN -#define SPI2_NSS_PIN NONE -#endif -#ifndef SPI3_NSS_PIN -#define SPI3_NSS_PIN NONE -#endif -#ifndef SPI4_NSS_PIN -#define SPI4_NSS_PIN NONE -#endif - -#if defined(USE_SPI_DEVICE_1) -static const uint32_t spiDivisorMapFast[] = { - SPI_BAUDRATEPRESCALER_256, // SPI_CLOCK_INITIALIZATON 421.875 KBits/s - SPI_BAUDRATEPRESCALER_32, // SPI_CLOCK_SLOW 843.75 KBits/s - SPI_BAUDRATEPRESCALER_16, // SPI_CLOCK_STANDARD 6.75 MBits/s - SPI_BAUDRATEPRESCALER_8, // SPI_CLOCK_FAST 13.5 MBits/s - SPI_BAUDRATEPRESCALER_4 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s -}; -#endif - -#if defined(USE_SPI_DEVICE_2) || defined(USE_SPI_DEVICE_3) || defined(USE_SPI_DEVICE_4) -static const uint32_t spiDivisorMapSlow[] = { - SPI_BAUDRATEPRESCALER_256, // SPI_CLOCK_INITIALIZATON 210.937 KBits/s - SPI_BAUDRATEPRESCALER_64, // SPI_CLOCK_SLOW 843.75 KBits/s - SPI_BAUDRATEPRESCALER_8, // SPI_CLOCK_STANDARD 6.75 MBits/s - SPI_BAUDRATEPRESCALER_4, // SPI_CLOCK_FAST 13.5 MBits/s - SPI_BAUDRATEPRESCALER_2 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s -}; -#endif - -static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { -#ifdef USE_SPI_DEVICE_1 - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, -#else - { .dev = NULL }, // No SPI1 -#endif -#ifdef USE_SPI_DEVICE_2 - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, -#else - { .dev = NULL }, // No SPI2 -#endif -#ifdef USE_SPI_DEVICE_3 - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, -#else - { .dev = NULL }, // No SPI3 -#endif -#ifdef USE_SPI_DEVICE_4 - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } -#else - { .dev = NULL } // No SPI4 -#endif -}; - -static SPI_HandleTypeDef spiHandle[SPIDEV_COUNT]; - -SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) -{ - if (instance == SPI1) - return SPIDEV_1; - - if (instance == SPI2) - return SPIDEV_2; - - if (instance == SPI3) - return SPIDEV_3; - - if (instance == SPI4) - return SPIDEV_4; - - return SPIINVALID; -} - -void spiTimeoutUserCallback(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - if (device == SPIINVALID) { - return; - } - - spiHardwareMap[device].errorCount++; -} - -bool spiInitDevice(SPIDevice device, bool leadingEdge) -{ - spiDevice_t *spi = &spiHardwareMap[device]; - - if (!spi->dev) { - return false; - } - - if (spi->initDone) { - return true; - } - - // Enable SPI clock - RCC_ClockCmd(spi->rcc, ENABLE); - RCC_ResetCmd(spi->rcc, DISABLE); - - IOInit(IOGetByTag(spi->sck), OWNER_SPI, RESOURCE_SPI_SCK, device + 1); - IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1); - IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); - - if (leadingEdge) { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); - } else { - IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); - } - IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); - IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); - - if (spi->nss) { - IOInit(IOGetByTag(spi->nss), OWNER_SPI, RESOURCE_SPI_CS, device + 1); - IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); - } - - SPI_HandleTypeDef * hspi = &spiHandle[device]; - memset(hspi, 0, sizeof(SPI_HandleTypeDef)); - hspi->Instance = spi->dev; - - HAL_SPI_DeInit(hspi); - - hspi->Init.Mode = SPI_MODE_MASTER; - hspi->Init.Direction = SPI_DIRECTION_2LINES; - hspi->Init.DataSize = SPI_DATASIZE_8BIT; - hspi->Init.NSS = SPI_NSS_SOFT; - hspi->Init.FirstBit = SPI_FIRSTBIT_MSB; - hspi->Init.CRCPolynomial = 7; - hspi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; - hspi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - hspi->Init.TIMode = SPI_TIMODE_DISABLED; - hspi->Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA; - hspi->Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE; /* Recommanded setting to avoid glitches */ - - if (leadingEdge) { - hspi->Init.CLKPolarity = SPI_POLARITY_LOW; - hspi->Init.CLKPhase = SPI_PHASE_1EDGE; - } - else { - hspi->Init.CLKPolarity = SPI_POLARITY_HIGH; - hspi->Init.CLKPhase = SPI_PHASE_2EDGE; - } - - if (spi->nss) { - IOHi(IOGetByTag(spi->nss)); - } - - HAL_SPI_Init(hspi); - - spi->initDone = true; - return true; -} - -uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) -{ - uint8_t rxData; - spiTransfer(instance, &rxData, &txByte, 1); - return rxData; -} - -/** - * Return true if the bus is currently in the middle of a transmission. - */ -bool spiIsBusBusy(SPI_TypeDef *instance) -{ - SPIDevice device = spiDeviceByInstance(instance); - return (spiHandle[device].State == HAL_SPI_STATE_BUSY); -} - -bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, int len) -{ - SPIDevice device = spiDeviceByInstance(instance); - SPI_HandleTypeDef * hspi = &spiHandle[device]; - HAL_StatusTypeDef status; - - #define SPI_DEFAULT_TIMEOUT 10 - - if (!rxData) { - status = HAL_SPI_Transmit(hspi, txData, len, SPI_DEFAULT_TIMEOUT); - } else if(!txData) { - status = HAL_SPI_Receive(hspi, rxData, len, SPI_DEFAULT_TIMEOUT); - } else { - status = HAL_SPI_TransmitReceive(hspi, txData, rxData, len, SPI_DEFAULT_TIMEOUT); - } - - if(status != HAL_OK) { - spiTimeoutUserCallback(instance); - } - - return true; -} - -void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed) -{ - SPIDevice device = spiDeviceByInstance(instance); - SPI_HandleTypeDef * hspi = &spiHandle[device]; - - HAL_SPI_DeInit(hspi); - hspi->Init.BaudRatePrescaler = spiHardwareMap[device].divisorMap[speed]; - HAL_SPI_Init(hspi); -} - -SPI_TypeDef * spiInstanceByDevice(SPIDevice device) -{ - return spiHardwareMap[device].dev; -} diff --git a/src/main/drivers/bus_spi_hal_ll.c b/src/main/drivers/bus_spi_hal_ll.c index 9c02ae2ccc..25662f2f2d 100644 --- a/src/main/drivers/bus_spi_hal_ll.c +++ b/src/main/drivers/bus_spi_hal_ll.c @@ -88,6 +88,30 @@ static const uint32_t spiDivisorMapSlow[] = { }; #endif +#if defined(STM32H7) +static spiDevice_t spiHardwareMap[SPIDEV_COUNT] = { +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#else + { .dev = NULL }, // No SPI1 +#endif +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1L(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI2 +#endif +#ifdef USE_SPI_DEVICE_3 + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1L(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI3 +#endif +#ifdef USE_SPI_DEVICE_4 + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#else + { .dev = NULL } // No SPI4 +#endif +}; +#else static spiDevice_t spiHardwareMap[] = { #ifdef USE_SPI_DEVICE_1 { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, @@ -110,6 +134,7 @@ static spiDevice_t spiHardwareMap[] = { { .dev = NULL } // No SPI4 #endif }; +#endif SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { @@ -187,12 +212,21 @@ bool spiInitDevice(SPIDevice device, bool leadingEdge) .CRCPoly = 7, .CRCCalculation = SPI_CRCCALCULATION_DISABLE, }; + +#if defined(STM32H7) + // Prevent glitching when SPI is disabled + LL_SPI_EnableGPIOControl(spi->dev); + + LL_SPI_SetFIFOThreshold(spi->dev, LL_SPI_FIFO_TH_01DATA); + LL_SPI_Init(spi->dev, &init); +#else LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF); LL_SPI_Init(spi->dev, &init); LL_SPI_Enable(spi->dev); SET_BIT(spi->dev->CR2, SPI_RXFIFO_THRESHOLD); +#endif if (spi->nss) { IOHi(IOGetByTag(spi->nss)); @@ -204,26 +238,11 @@ bool spiInitDevice(SPIDevice device, bool leadingEdge) uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) { - uint16_t spiTimeout = 1000; - - while (!LL_SPI_IsActiveFlag_TXE(instance)) { - if ((spiTimeout--) == 0) { - spiTimeoutUserCallback(instance); - return 0xFF; - } + uint8_t value = 0xFF; + if (!spiTransfer(instance, &value, &txByte, 1)) { + return 0xFF; } - - LL_SPI_TransmitData8(instance, txByte); - - spiTimeout = 1000; - while (!LL_SPI_IsActiveFlag_RXNE(instance)) { - if ((spiTimeout--) == 0) { - spiTimeoutUserCallback(instance); - return 0xFF; - } - } - - return (uint8_t)LL_SPI_ReceiveData8(instance); + return value; } /** @@ -231,11 +250,49 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) */ bool spiIsBusBusy(SPI_TypeDef *instance) { +#if defined(STM32H7) + UNUSED(instance); + // H7 doesnt really have a busy flag. its should be done when the transfer is. + return false; +#else return (LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY) || LL_SPI_IsActiveFlag_BSY(instance); +#endif } bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, int len) { +#if defined(STM32H7) + LL_SPI_SetTransferSize(instance, len); + LL_SPI_Enable(instance); + LL_SPI_StartMasterTransfer(instance); + while (len) { + int spiTimeout = 1000; + while(!LL_SPI_IsActiveFlag_TXP(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return false; + } + } + uint8_t b = txData ? *(txData++) : 0xFF; + LL_SPI_TransmitData8(instance, b); + + spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_RXP(instance)) { + if ((spiTimeout--) == 0) { + spiTimeoutUserCallback(instance); + return false; + } + } + b = LL_SPI_ReceiveData8(instance); + if (rxData) { + *(rxData++) = b; + } + --len; + } + while (!LL_SPI_IsActiveFlag_EOT(instance)); + LL_SPI_ClearFlag_TXTF(instance); + LL_SPI_Disable(instance); +#else SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD); while (len) { @@ -262,6 +319,7 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *rxData, const uint8_t *txData, } --len; } +#endif return true; } diff --git a/src/main/target/KAKUTEH7/target.h b/src/main/target/KAKUTEH7/target.h index ade11c62aa..9b8727abd1 100644 --- a/src/main/target/KAKUTEH7/target.h +++ b/src/main/target/KAKUTEH7/target.h @@ -41,6 +41,15 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI1 +#define SDCARD_CS_PIN PA4 +#define SDCARD_DETECT_INVERTED +#define SDCARD_DETECT_PIN PA3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + // *************** SPI2 *********************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 @@ -148,7 +157,7 @@ #define USE_LED_STRIP #define WS2811_PIN PD12 -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE 250 #define USE_SERIAL_4WAY_BLHELI_INTERFACE From 7afbf9f055ef506ebc94ae7991e7aab08943241d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 27 Dec 2021 14:59:32 +0100 Subject: [PATCH 064/107] Version bump --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index edb0b7640e..316531a8b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 4.0.0) +project(INAV VERSION 4.1.0) enable_language(ASM) From befe029329d1fc97f7d97718516125661a63a07b Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Tue, 28 Dec 2021 17:04:57 -0300 Subject: [PATCH 065/107] [cli.c] Fix external Flash Memory --- src/main/fc/cli.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5d75c0342a..8c9cbf8466 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2248,6 +2248,11 @@ static void cliFlashInfo(char *cmdline) UNUSED(cmdline); const flashGeometry_t *layout = flashGetGeometry(); + + if (layout->totalSize == 0) { + cliPrintLine("Flash not available"); + return; + } cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u", layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize); @@ -2277,6 +2282,13 @@ static void cliFlashErase(char *cmdline) { UNUSED(cmdline); + const flashGeometry_t *layout = flashGetGeometry(); + + if (layout->totalSize == 0) { + cliPrintLine("Flash not available"); + return; + } + cliPrintLine("Erasing..."); flashfsEraseCompletely(); From 2305491ed9b134f3e453944773bb2361e71c8d56 Mon Sep 17 00:00:00 2001 From: yajo10 Date: Mon, 3 Jan 2022 17:48:44 +0100 Subject: [PATCH 066/107] Don't start motor with high throttle in the mid of the stick --- src/main/flight/mixer.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 43d399a13f..9243d4d50d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -540,6 +540,15 @@ void FAST_CODE mixTable() motorValueWhenStopped = getReversibleMotorsThrottleDeadband(); mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax); + + if(feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { + /* + * We need to start the throttle output from stick input to start in the middle of the stick at the low and. + * Without this, it's starting at the high side. + */ + int throttleDistanceToMax = throttleRangeMax - rcCommand[THROTTLE]; + mixerThrottleCommand = throttleRangeMin + throttleDistanceToMax; + } } else { mixerThrottleCommand = rcCommand[THROTTLE]; throttleRangeMin = throttleIdleValue; From 7b532788804116eda6cb7861cc9fcbb40719fb0c Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Wed, 5 Jan 2022 07:59:31 +0000 Subject: [PATCH 067/107] Optimise data transmissions --- src/main/fc/fc_tasks.c | 2 +- src/main/io/displayport_hdzero_osd.c | 409 +++++++++++++-------------- src/main/io/displayport_hdzero_osd.h | 2 +- 3 files changed, 203 insertions(+), 210 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d56bad6099..50d3c92e05 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -112,7 +112,7 @@ void taskHandleSerial(timeUs_t currentTimeUs) #endif #ifdef USE_HDZERO_OSD - hdzeroOsdSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); + hdzeroOsdSerialProcess(mspFcProcessCommand); #endif } diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index 6483610b2e..68c40e7c21 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -28,11 +28,14 @@ #include "platform.h" +FILE_COMPILE_FOR_SPEED + #if defined(USE_OSD) && defined(USE_HDZERO_OSD) #include "common/utils.h" #include "common/printf.h" #include "common/time.h" +#include "common/bitarray.h" #include "drivers/display.h" #include "drivers/display_font_metadata.h" @@ -43,25 +46,28 @@ #include "displayport_hdzero_osd.h" -#define MSP_HEARTBEAT 0 -#define MSP_RELEASE 1 +#define FONT_VERSION 3 + #define MSP_CLEAR_SCREEN 2 #define MSP_WRITE_STRING 3 #define MSP_DRAW_SCREEN 4 -#define MSP_SET_HD 5 +#define MSP_SET_OPTIONS 5 +#define MAX_UPDATES 5 +#define VTX_TIMEOUT 1000 // 1 second timer -#define FONT_PAGE_ATTRIBUTE 0x01 - -static mspPort_t hdzeroMspPort; -static displayPort_t hdzeroOsdDisplayPort; -static bool hdzeroVtxReady; +static mspProcessCommandFnPtr mspProcessCommand; +static mspPort_t hdZeroMspPort; +static displayPort_t hdZeroOsdDisplayPort; +static bool vtxReady, vtxReset; +static timeMs_t vtxHeartbeat; // HD screen size #define ROWS 18 #define COLS 50 #define SCREENSIZE (ROWS*COLS) -static uint8_t screen[SCREENSIZE]; -static uint8_t fontPage[SCREENSIZE / 8 + 1]; // page bits for each character (to address 512 char font) +static uint8_t screen[SCREENSIZE] ALIGNED(4); +static BITARRAY_DECLARE(fontPage, SCREENSIZE) ALIGNED(4); // font page for each character on the screen +static BITARRAY_DECLARE(dirty, SCREENSIZE) ALIGNED(4); // change status for each character on the screen extern uint8_t cliMode; @@ -69,207 +75,156 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int { UNUSED(displayPort); - if (cliMode) - return 0; + int sent = 0; - return mspSerialPushPort(cmd, subcmd, len, &hdzeroMspPort, MSP_V1); -} - -static int heartbeat(displayPort_t *displayPort) -{ - uint8_t subcmd[] = { MSP_HEARTBEAT }; - return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); -} - -static int release(displayPort_t *displayPort) -{ - uint8_t subcmd[] = { MSP_RELEASE }; - return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); -} - -static int clearScreen(displayPort_t *displayPort) -{ - UNUSED(displayPort); - - memset(screen, SYM_BLANK, sizeof(screen)); - memset(fontPage, 0, sizeof(fontPage)); - return 1; -} - -/* - * Write up to three populated rows at a time, skipping blank lines. - * This gives a refresh rate to the VTX of approximately 10 to 62Hz - * depending on how much data is displayed. - */ -static int drawScreen(displayPort_t *displayPort) // 62.5hz -{ - static uint8_t row = 0, clearSent = 0; - uint8_t subcmd[COLS + 4], len, col, page, aPage, rowsToPrint; - uint16_t lineIdx, idx, end; - int charsOut = 0; - - rowsToPrint = 3; - do { - // Find a row with something to print - do { - // Strip leading and trailing spaces for the selected row - lineIdx = row * COLS; - idx = lineIdx; - end = idx + COLS - 1; - - while ((screen[idx] == SYM_BLANK || screen[end] == SYM_BLANK) && idx <= end) { - if (screen[idx] == SYM_BLANK) - idx++; - if (screen[end] == SYM_BLANK) - end--; - } - } while (idx > end && ++row < ROWS); - - while (idx <= end) { - if (!clearSent) { - // Start the transaction - subcmd[0] = MSP_CLEAR_SCREEN; - charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); - clearSent = 1; - } - - // Split the line up into strings from the same font page and output them. - // (note spaces are printed to save overhead on small elements) - len = 4; - col = idx - lineIdx; - page = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; - - do { - subcmd[len++] = screen[idx++]; - aPage = (fontPage[idx >> 3] >> (idx & 0x07)) & FONT_PAGE_ATTRIBUTE; - } while (idx <= end && (aPage == page || screen[idx] == SYM_BLANK)); - - subcmd[0] = MSP_WRITE_STRING; - subcmd[1] = row; - subcmd[2] = col; - subcmd[3] = page; - charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, len); - } - } while (++row < ROWS && --rowsToPrint); - - if (row >= ROWS) { - // End the transaction if required and reset the counters - if (clearSent > 0) { - subcmd[0] = MSP_DRAW_SCREEN; - charsOut += output(displayPort, MSP_DISPLAYPORT, subcmd, 1); - } - row = clearSent = 0; + if (!cliMode && vtxReady) { + sent = mspSerialPushPort(cmd, subcmd, len, &hdZeroMspPort, MSP_V1); +#ifdef STATS + dataSent += sent; +#endif } - return charsOut; + return sent; +} + +static void checkVtxPresent(void) +{ + if (vtxReady && (millis()-vtxHeartbeat) > VTX_TIMEOUT) { + vtxReady = false; + } } static int setHdMode(displayPort_t *displayPort) { - uint8_t subcmd[3]; - subcmd[0] = MSP_SET_HD; - subcmd[1] = 0; // future font index - subcmd[2] = 1; // 0 SD 1 HD - + checkVtxPresent(); + uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, 1 }; // font selection, mode (SD/HD) return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); } -static int grab(displayPort_t *displayPort) +static void hdZeroInit(void) { - return heartbeat(displayPort); + memset(screen, SYM_BLANK, sizeof(screen)); + BITARRAY_CLR_ALL(fontPage); + BITARRAY_CLR_ALL(dirty); } -static int screenSize(const displayPort_t *displayPort) +static int clearScreen(displayPort_t *displayPort) { - UNUSED(displayPort); + uint8_t subcmd[] = { MSP_CLEAR_SCREEN }; - return SCREENSIZE; -} - -// Intercept writeString and write to a buffer instead (1st page of font file only) -static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr) -{ - UNUSED(displayPort); - UNUSED(attr); - - uint16_t i, pos, len, end, idx; - - pos = (row * COLS) + col; - if (pos >= SCREENSIZE) - return 0; - - len = strlen(string); - - // Allow word wrap and truncate of past the screen end - end = pos + len - 1; - if (end >= SCREENSIZE) - len = end - SCREENSIZE; - - // Copy the string into the screen buffer - memcpy(screen + pos, string, len); - - // Clear the page bits for all the characters in the string - for (i = 0; i < len; i++) { - idx = pos + i; - fontPage[idx >> 3] &= ~(1 << (idx & 0x07)); - } - - return (int) len; -} - -// Write character to screen and page buffers (supports 512 char fonts) -static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) -{ - UNUSED(displayPort); - UNUSED(attr); - - uint16_t pos, idx; - uint8_t bitmask; - - pos = (row * COLS) + col; - if (pos >= SCREENSIZE) - return 0; - - // Copy character into screen buffer - screen[pos] = c; - - idx = pos >> 3; - bitmask = 1 << (pos & 0x07); - - // Save index of the character's font page - if (c & 0x0100) - fontPage[idx] |= bitmask; - else - fontPage[idx] &= ~bitmask; - - return (int) 1; + hdZeroInit(); + setHdMode(displayPort); + return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); } static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t *c, textAttributes_t *attr) { UNUSED(displayPort); - uint16_t pos, chr; - - pos = (row * COLS) + col; - if (pos >= SCREENSIZE) - *c = SYM_BLANK; - else { - chr = (fontPage[pos >> 3] >> (pos & 0x07)) & FONT_PAGE_ATTRIBUTE; - *c = (chr << 8) | screen[pos]; + uint16_t pos = (row * COLS) + col; + if (pos >= SCREENSIZE) { + return false; } - if (attr) + *c = screen[pos]; + if (bitArrayGet(fontPage, pos)) + { + *c |= 0x100; + } + + if (attr) { *attr = TEXT_ATTRIBUTES_NONE; + } return true; } -static bool isTransferInProgress(const displayPort_t *displayPort) +static int setChar(const uint16_t pos, const uint16_t c) +{ + if (pos < SCREENSIZE) { + uint8_t ch = c & 0xFF; + bool page = (c >> 8); + if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) { + screen[pos] = ch; + (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos); + bitArraySet(dirty, pos); + } + } + return 0; +} + +static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) { UNUSED(displayPort); + UNUSED(attr); - return false; + return setChar((row * COLS) + col, c); +} + +static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr) +{ + UNUSED(displayPort); + UNUSED(attr); + + uint16_t pos = (row * COLS) + col; + while (*string) + { + setChar(pos++, *string++); + } + return 0; +} + +/** + * Write only changed characters to the VTX + */ +static int drawScreen(displayPort_t *displayPort) // 62.5hz or 16ms +{ + uint8_t subcmd[COLS + 4]; + uint8_t updateCount = 0; + + subcmd[0] = MSP_WRITE_STRING; + + int next = BITARRAY_FIND_FIRST_SET(dirty, 0); + while (next >= 0 && updateCount < MAX_UPDATES) { + // Look for sequential dirty characters on the same line for the same font page + int pos = next; + uint8_t row = pos / COLS; + uint8_t col = pos % COLS; + int endOfLine = row * COLS + COLS; + bool page = bitArrayGet(fontPage, pos); + + uint8_t len = 4; + do { + bitArrayClr(dirty, pos); + subcmd[len++] = screen[pos++]; + + if (bitArrayGet(dirty, pos)) { + next = pos; + } + } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page); + + subcmd[1] = row; + subcmd[2] = col; + subcmd[3] = page; + output(displayPort, MSP_DISPLAYPORT, subcmd, len); + updateCount++; + next = BITARRAY_FIND_FIRST_SET(dirty, pos); + } + + if (updateCount > 0) + { + subcmd[0] = MSP_DRAW_SCREEN; + output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + } + + checkVtxPresent(); + + if (vtxReset) { + clearScreen(displayPort); + vtxReset = false; + } + + return 0; } static void resync(displayPort_t *displayPort) @@ -279,38 +234,60 @@ static void resync(displayPort_t *displayPort) setHdMode(displayPort); } +static int screenSize(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return SCREENSIZE; +} + static uint32_t txBytesFree(const displayPort_t *displayPort) { UNUSED(displayPort); - return mspSerialTxBytesFree(); } -static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort) -{ - UNUSED(displayPort); - - textAttributes_t attr = TEXT_ATTRIBUTES_NONE; - //TEXT_ATTRIBUTES_ADD_INVERTED(attr); - //TEXT_ATTRIBUTES_ADD_SOLID_BG(attr); - return attr; -} - static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) { UNUSED(displayPort); - metadata->charCount = 512; - metadata->version = 3; - + metadata->version = FONT_VERSION; return true; } +static textAttributes_t supportedTextAttributes(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return TEXT_ATTRIBUTES_NONE; +} + +static bool isTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + static bool isReady(displayPort_t *displayPort) { UNUSED(displayPort); + return vtxReady; +} - return hdzeroVtxReady; +static int grab(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int heartbeat(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int release(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; } static const displayPortVTable_t hdzeroOsdVTable = { @@ -333,32 +310,48 @@ static const displayPortVTable_t hdzeroOsdVTable = { void hdzeroOsdSerialInit(void) { - memset(&hdzeroMspPort, 0, sizeof(mspPort_t)); + memset(&hdZeroMspPort, 0, sizeof(mspPort_t)); serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD); - if (portConfig) { serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); - if (port) - resetMspPort(&hdzeroMspPort, port); + if (port) { + resetMspPort(&hdZeroMspPort, port); + } } } displayPort_t* hdzeroOsdDisplayPortInit(void) { - memset(screen, SYM_BLANK, sizeof(screen)); - memset(fontPage, 0, sizeof(fontPage)); - displayInit(&hdzeroOsdDisplayPort, &hdzeroOsdVTable); - return &hdzeroOsdDisplayPort; + hdZeroInit(); + displayInit(&hdZeroOsdDisplayPort, &hdzeroOsdVTable); + return &hdZeroOsdDisplayPort; } -void hdzeroOsdSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn) +/* + * Intercept MSP processor. + * VTX sends an MSP command every 125ms or so. + * VTX will have be marked as not ready if no commands received within VTX_TIMEOUT. + */ +static mspResult_e hdZeroProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { - if (hdzeroMspPort.port) { - // Process normal MSP command - mspSerialProcessOnePort(&hdzeroMspPort, evaluateNonMspData, mspProcessCommandFn); - hdzeroVtxReady = true; + if (!vtxReady) { + vtxReset = true; + } + + vtxReady = true; + vtxHeartbeat = millis(); + + // Process MSP command + return mspProcessCommand(cmd, reply, mspPostProcessFn); +} + +void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) +{ + if (hdZeroMspPort.port) { + mspProcessCommand = mspProcessCommandFn; + mspSerialProcessOnePort(&hdZeroMspPort, MSP_SKIP_NON_MSP_DATA, hdZeroProcessMspCommand); } } diff --git a/src/main/io/displayport_hdzero_osd.h b/src/main/io/displayport_hdzero_osd.h index 4b12deb688..73cf5a71dd 100644 --- a/src/main/io/displayport_hdzero_osd.h +++ b/src/main/io/displayport_hdzero_osd.h @@ -31,4 +31,4 @@ typedef struct displayPort_s displayPort_t; void hdzeroOsdSerialInit(void); displayPort_t *hdzeroOsdDisplayPortInit(void); -void hdzeroOsdSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); +void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); From 97033dc7147f5ab44c4a65036f26202a7d490744 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Wed, 5 Jan 2022 15:44:13 +0000 Subject: [PATCH 068/107] Increase writes per tick to 10 to cater for AHI. --- src/main/io/displayport_hdzero_osd.c | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index 68c40e7c21..fad3334eb6 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -52,7 +52,7 @@ FILE_COMPILE_FOR_SPEED #define MSP_WRITE_STRING 3 #define MSP_DRAW_SCREEN 4 #define MSP_SET_OPTIONS 5 -#define MAX_UPDATES 5 +#define MAX_UPDATES 10 #define VTX_TIMEOUT 1000 // 1 second timer static mspProcessCommandFnPtr mspProcessCommand; @@ -65,9 +65,9 @@ static timeMs_t vtxHeartbeat; #define ROWS 18 #define COLS 50 #define SCREENSIZE (ROWS*COLS) -static uint8_t screen[SCREENSIZE] ALIGNED(4); -static BITARRAY_DECLARE(fontPage, SCREENSIZE) ALIGNED(4); // font page for each character on the screen -static BITARRAY_DECLARE(dirty, SCREENSIZE) ALIGNED(4); // change status for each character on the screen +static uint8_t screen[SCREENSIZE]; +static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen +static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen extern uint8_t cliMode; @@ -79,9 +79,6 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int if (!cliMode && vtxReady) { sent = mspSerialPushPort(cmd, subcmd, len, &hdZeroMspPort, MSP_V1); -#ifdef STATS - dataSent += sent; -#endif } return sent; From ac307dbe63e4ceb852975522a6b2c118216bd59e Mon Sep 17 00:00:00 2001 From: Olivier C Date: Mon, 3 Jan 2022 19:51:07 +0100 Subject: [PATCH 069/107] Changing the UP and DOWN symbols for the radar relative altitude display The OSD font was changed before the PR was merged From eff0aa3179e647d8fa245fc98838054ace758bde Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Fri, 7 Jan 2022 17:44:24 -0300 Subject: [PATCH 070/107] Change Power to Square --- src/main/io/gps_naza.c | 7 +------ src/main/navigation/navigation_multicopter.c | 2 +- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/io/gps_naza.c b/src/main/io/gps_naza.c index 9abef4c878..1b1a35e258 100644 --- a/src/main/io/gps_naza.c +++ b/src/main/io/gps_naza.c @@ -189,17 +189,12 @@ static bool NAZA_parse_gps(void) uint16_t pdop = decodeShort(_buffernaza.nav.pdop, mask); // pdop - //uint16_t vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop - //uint16_t ndop = decodeShort(_buffernaza.nav.ndop, mask); - //uint16_t edop = decodeShort(_buffernaza.nav.edop, mask); - //gpsSol.hdop = sqrtf(powf(ndop,2)+powf(edop,2)); - //gpsSol.vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop gpsSol.hdop = gpsConstrainEPE(pdop); // PDOP 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 = fast_fsqrtf(powf(gpsSol.velNED[X], 2) + powf(gpsSol.velNED[Y], 2)); //cm/s + gpsSol.groundSpeed = fast_fsqrtf(sq(gpsSol.velNED[X]) + sq(gpsSol.velNED[Y])); //cm/s // calculate gps heading from VELNE gpsSol.groundCourse = (uint16_t)(fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[Y], gpsSol.velNED[X])) + 3600.0f, 3600.0f)); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2247d05835..44945ff031 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -493,7 +493,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 = fast_fsqrtf(powf(setpointX, 2)+powf(setpointY, 2)); + const float setpointXY = fast_fsqrtf(sq(setpointX) + sq(setpointY)); // Calculate velocity error const float velErrorX = setpointX - measurementX; From 084c289101181c653476a826c27ea2afc039d89d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 8 Jan 2022 09:40:42 +0000 Subject: [PATCH 071/107] Update navigation.c --- src/main/navigation/navigation.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b7070de8cc..8f541f464e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -819,7 +819,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, } }, @@ -837,7 +836,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, } }, From 29f7cc6b1e6c91d420190358d926bbc6908c701b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 9 Jan 2022 10:46:51 +0100 Subject: [PATCH 072/107] Lower the Airmode throttle threshold --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d2e57316b1..1576b58317 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1648,7 +1648,7 @@ groups: table: airmodeHandlingType - name: airmode_throttle_threshold description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used" - default_value: 1300 + default_value: 1150 field: airmodeThrottleThreshold min: 1000 max: 2000 From 9e0eb3227cba56e9ae6995027e724fdb341d6ceb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 9 Jan 2022 10:49:01 +0100 Subject: [PATCH 073/107] Docs update --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 84d0528a3e..aef9e2ad28 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -188,7 +188,7 @@ Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THR | Default | Min | Max | | --- | --- | --- | -| 1300 | 1000 | 2000 | +| 1150 | 1000 | 2000 | --- From 446f146c746de0f0326812be1c6a327f80d0093f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 9 Jan 2022 21:14:50 +0000 Subject: [PATCH 074/107] Update Settings.md --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index cdfb49bb03..28cfba4bf8 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3858,7 +3858,7 @@ Forces craft to achieve the set WP altitude as well as position before moving to | Default | Min | Max | | --- | --- | --- | -| OFF | | | +| OFF | OFF | ON | --- From 4793f66eb45369458f1510198b071cef26503b98 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 9 Jan 2022 22:16:48 +0000 Subject: [PATCH 075/107] Update Settings.md --- docs/Settings.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 8073301585..f11fcd5e6f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4582,6 +4582,16 @@ Auto swap display time interval between disarm stats pages (seconds). Reverts to --- +### osd_system_msg_display_time + +System message display cycle time for multiple messages (milliseconds). + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 500 | 5000 | + +--- + ### osd_telemetry To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` From 4f04c87d1e4d9c016aee85103af7a7ddc55b2e81 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 9 Jan 2022 23:33:15 +0000 Subject: [PATCH 076/107] Change capture to enforce --- docs/Settings.md | 2 +- src/main/cms/cms_menu_navigation.c | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/io/osd.c | 2 +- src/main/navigation/navigation.c | 10 +++++----- src/main/navigation/navigation.h | 2 +- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 28cfba4bf8..9c62e2030f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3852,7 +3852,7 @@ Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: AT --- -### nav_wp_capture_altitude +### nav_wp_enforce_altitude Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on. diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 19ba47d653..80f7bf09e1 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -192,7 +192,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = 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 CAPTURE ALTITUDE", SETTING_NAV_WP_CAPTURE_ALTITUDE), + OSD_SETTING_ENTRY("WP ENFORCE ALTITUDE", SETTING_NAV_WP_ENFORCE_ALTITUDE), OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE), #ifdef USE_MULTI_MISSION OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 193dc22359..dfbcb9f73f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2392,10 +2392,10 @@ groups: field: general.waypoint_radius min: 10 max: 10000 - - name: nav_wp_capture_altitude + - name: nav_wp_enforce_altitude description: "Forces craft to achieve the set WP altitude as well as position before moving to next WP. Position is held and altitude adjusted as required before moving on." default_value: OFF - field: general.flags.waypoint_capture_altitude + field: general.flags.waypoint_enforce_altitude type: bool - name: nav_wp_safe_distance description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check." diff --git a/src/main/io/osd.c b/src/main/io/osd.c index be3df44608..a5d9b64a2f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4193,7 +4193,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter 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) { - if (navConfig()->general.flags.waypoint_capture_altitude && !posControl.wpAltitudeReached) { + if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); } else { // WP hold time countdown in seconds diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a6812ed684..01bcfe2b06 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -60,7 +60,7 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" -#define WP_ALTITUDE_MARGIN_CM 100 // WP altitude capture tolerance, used when WP altitude setting enforced when WP reached +#define WP_ALTITUDE_MARGIN_CM 100 // WP enforce altitude tolerance, used when WP altitude setting enforced when WP reached // Multirotors: #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) @@ -119,7 +119,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled - .waypoint_capture_altitude = SETTING_NAV_WP_CAPTURE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved + .waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved }, // General navigation parameters @@ -1586,13 +1586,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga { UNUSED(previousState); - if (navConfig()->general.flags.waypoint_capture_altitude) { + if (navConfig()->general.flags.waypoint_enforce_altitude) { posControl.wpAltitudeReached = isWaypointAltitudeReached(); } switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_WAYPOINT: - if (navConfig()->general.flags.waypoint_capture_altitude && !posControl.wpAltitudeReached) { + if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME; } else { return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT @@ -1625,7 +1625,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (navConfig()->general.flags.waypoint_capture_altitude && !posControl.wpAltitudeReached) { + if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { // Adjust altitude to waypoint setting if (STATE(AIRPLANE)) { int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 673601178b..201cd502cf 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -222,7 +222,7 @@ typedef struct navConfig_s { uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled uint8_t mission_planner_reset; // Allow WP Mission Planner reset using mode toggle (resets WPs to 0) uint8_t waypoint_mission_restart; // Waypoint mission restart action - uint8_t waypoint_capture_altitude; // Forces waypoint altitude to be acheived + uint8_t waypoint_enforce_altitude; // Forces waypoint altitude to be achieved } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) From d3d7f345e51cfc4c39ae89f394a0dc1e712618b7 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 14 Jan 2022 14:44:09 +0000 Subject: [PATCH 077/107] Ensure analog OSD operates correctly when HDZero not selected. --- src/main/fc/fc_init.c | 4 - src/main/fc/fc_tasks.c | 4 +- src/main/io/displayport_hdzero_osd.c | 106 +++++++++++++++------------ src/main/io/displayport_hdzero_osd.h | 1 - src/main/io/osd.c | 8 +- 5 files changed, 64 insertions(+), 59 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 77d5a2c346..4bf83d3d05 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -290,10 +290,6 @@ void init(void) djiOsdSerialInit(); #endif -#ifdef USE_HDZERO_OSD - hdzeroOsdSerialInit(); -#endif - #if defined(USE_SMARTPORT_MASTER) smartportMasterInit(); #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 50d3c92e05..262e7fa6e2 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -40,8 +40,6 @@ #include "fc/fc_core.h" #include "fc/fc_msp.h" #include "fc/fc_tasks.h" - -#include "../io/displayport_hdzero_osd.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -69,6 +67,7 @@ #include "io/smartport_master.h" #include "io/vtx.h" #include "io/osd_dji_hd.h" +#include "io/displayport_hdzero_osd.h" #include "io/servo_sbus.h" #include "msp/msp_serial.h" @@ -112,6 +111,7 @@ void taskHandleSerial(timeUs_t currentTimeUs) #endif #ifdef USE_HDZERO_OSD + // Capture HDZero messages to determine if VTX is connected hdzeroOsdSerialProcess(mspFcProcessCommand); #endif } diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index fad3334eb6..a8cb3bbf62 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -52,13 +52,14 @@ FILE_COMPILE_FOR_SPEED #define MSP_WRITE_STRING 3 #define MSP_DRAW_SCREEN 4 #define MSP_SET_OPTIONS 5 -#define MAX_UPDATES 10 +#define DRAW_FREQ_DENOM 4 +#define UPDATES_PER_CALL 10 #define VTX_TIMEOUT 1000 // 1 second timer static mspProcessCommandFnPtr mspProcessCommand; static mspPort_t hdZeroMspPort; static displayPort_t hdZeroOsdDisplayPort; -static bool vtxReady, vtxReset; +static bool vtxSeen, vtxActive, vtxReset; static timeMs_t vtxHeartbeat; // HD screen size @@ -77,7 +78,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int int sent = 0; - if (!cliMode && vtxReady) { + if (!cliMode && vtxActive) { sent = mspSerialPushPort(cmd, subcmd, len, &hdZeroMspPort, MSP_V1); } @@ -86,8 +87,8 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int static void checkVtxPresent(void) { - if (vtxReady && (millis()-vtxHeartbeat) > VTX_TIMEOUT) { - vtxReady = false; + if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) { + vtxActive = false; } } @@ -174,51 +175,54 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con /** * Write only changed characters to the VTX */ -static int drawScreen(displayPort_t *displayPort) // 62.5hz or 16ms +static int drawScreen(displayPort_t *displayPort) // 250Hz { - uint8_t subcmd[COLS + 4]; - uint8_t updateCount = 0; + static uint8_t counter = 0; - subcmd[0] = MSP_WRITE_STRING; + if ((counter++ % DRAW_FREQ_DENOM) == 0) { // 62.5Hz + uint8_t subcmd[COLS + 4]; + uint8_t updateCount = 0; + subcmd[0] = MSP_WRITE_STRING; - int next = BITARRAY_FIND_FIRST_SET(dirty, 0); - while (next >= 0 && updateCount < MAX_UPDATES) { - // Look for sequential dirty characters on the same line for the same font page - int pos = next; - uint8_t row = pos / COLS; - uint8_t col = pos % COLS; - int endOfLine = row * COLS + COLS; - bool page = bitArrayGet(fontPage, pos); + int next = BITARRAY_FIND_FIRST_SET(dirty, 0); + while (next >= 0 && updateCount < UPDATES_PER_CALL) { + // Look for sequential dirty characters on the same line for the same font page + int pos = next; + uint8_t row = pos / COLS; + uint8_t col = pos % COLS; + int endOfLine = row * COLS + COLS; + bool page = bitArrayGet(fontPage, pos); - uint8_t len = 4; - do { - bitArrayClr(dirty, pos); - subcmd[len++] = screen[pos++]; + uint8_t len = 4; + do { + bitArrayClr(dirty, pos); + subcmd[len++] = screen[pos++]; - if (bitArrayGet(dirty, pos)) { - next = pos; - } - } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page); + if (bitArrayGet(dirty, pos)) { + next = pos; + } + } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page); - subcmd[1] = row; - subcmd[2] = col; - subcmd[3] = page; - output(displayPort, MSP_DISPLAYPORT, subcmd, len); - updateCount++; - next = BITARRAY_FIND_FIRST_SET(dirty, pos); - } + subcmd[1] = row; + subcmd[2] = col; + subcmd[3] = page; + output(displayPort, MSP_DISPLAYPORT, subcmd, len); + updateCount++; + next = BITARRAY_FIND_FIRST_SET(dirty, pos); + } - if (updateCount > 0) - { - subcmd[0] = MSP_DRAW_SCREEN; - output(displayPort, MSP_DISPLAYPORT, subcmd, 1); - } + if (updateCount > 0) + { + subcmd[0] = MSP_DRAW_SCREEN; + output(displayPort, MSP_DISPLAYPORT, subcmd, 1); + } - checkVtxPresent(); + checkVtxPresent(); - if (vtxReset) { - clearScreen(displayPort); - vtxReset = false; + if (vtxReset) { + clearScreen(displayPort); + vtxReset = false; + } } return 0; @@ -266,7 +270,7 @@ static bool isTransferInProgress(const displayPort_t *displayPort) static bool isReady(displayPort_t *displayPort) { UNUSED(displayPort); - return vtxReady; + return vtxActive; } static int grab(displayPort_t *displayPort) @@ -305,7 +309,7 @@ static const displayPortVTable_t hdzeroOsdVTable = { .isReady = isReady, }; -void hdzeroOsdSerialInit(void) +bool hdzeroOsdSerialInit(void) { memset(&hdZeroMspPort, 0, sizeof(mspPort_t)); @@ -315,15 +319,21 @@ void hdzeroOsdSerialInit(void) baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); if (port) { resetMspPort(&hdZeroMspPort, port); + return true; } } + + return false; } displayPort_t* hdzeroOsdDisplayPortInit(void) { - hdZeroInit(); - displayInit(&hdZeroOsdDisplayPort, &hdzeroOsdVTable); - return &hdZeroOsdDisplayPort; + if (hdzeroOsdSerialInit()) { + hdZeroInit(); + displayInit(&hdZeroOsdDisplayPort, &hdzeroOsdVTable); + return &hdZeroOsdDisplayPort; + } + return NULL; } /* @@ -333,11 +343,11 @@ displayPort_t* hdzeroOsdDisplayPortInit(void) */ static mspResult_e hdZeroProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { - if (!vtxReady) { + if (vtxSeen && !vtxActive) { vtxReset = true; } - vtxReady = true; + vtxSeen = vtxActive = true; vtxHeartbeat = millis(); // Process MSP command diff --git a/src/main/io/displayport_hdzero_osd.h b/src/main/io/displayport_hdzero_osd.h index 73cf5a71dd..54d9f8480b 100644 --- a/src/main/io/displayport_hdzero_osd.h +++ b/src/main/io/displayport_hdzero_osd.h @@ -29,6 +29,5 @@ typedef struct displayPort_s displayPort_t; -void hdzeroOsdSerialInit(void); displayPort_t *hdzeroOsdDisplayPortInit(void); void hdzeroOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 674ee80d18..2faeb729e2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4102,11 +4102,11 @@ void osdUpdate(timeUs_t currentTimeUs) osdUpdateStats(); } - if (counter % DRAW_FREQ_DENOM == 0) { - // redraw values in buffer one at a time + if ((counter % DRAW_FREQ_DENOM) == 0) { + // redraw values in buffer osdRefresh(currentTimeUs); - } else if (counter % DRAW_FREQ_DENOM == 1) { - // redraw screen + } else { + // rest of time redraw screen displayDrawScreen(osdDisplayPort); } From db83d2a9a1812941924e5b1ce90a25421b6bd4cc Mon Sep 17 00:00:00 2001 From: Andr0x Date: Fri, 14 Jan 2022 21:06:14 +0100 Subject: [PATCH 078/107] Update target.h Definition added for driving led strip --- src/main/target/SPEEDYBEEF7V2/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/SPEEDYBEEF7V2/target.h b/src/main/target/SPEEDYBEEF7V2/target.h index 0e5f5d190a..33ac6a2b9c 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.h +++ b/src/main/target/SPEEDYBEEF7V2/target.h @@ -137,6 +137,9 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PB0 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) #define USE_DSHOT From 7a5eedb2dc0054edbb518d54e963c0e90d8ac5a4 Mon Sep 17 00:00:00 2001 From: yajo10 Date: Sat, 15 Jan 2022 10:47:27 +0100 Subject: [PATCH 079/107] Only fix reversible motors on dshot protocol --- src/main/flight/mixer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9243d4d50d..8d7e55a352 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -541,7 +541,7 @@ void FAST_CODE mixTable() motorValueWhenStopped = getReversibleMotorsThrottleDeadband(); mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax); - if(feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { + if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { /* * We need to start the throttle output from stick input to start in the middle of the stick at the low and. * Without this, it's starting at the high side. From de0b3e2ee82fe8a3832895d850014048421438b3 Mon Sep 17 00:00:00 2001 From: yajo10 Date: Sat, 15 Jan 2022 11:04:23 +0100 Subject: [PATCH 080/107] Fix failing builds: check for DSHOT protocol at compile time and at runtime --- src/main/flight/mixer.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8d7e55a352..e9bc0c56cb 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -541,6 +541,7 @@ void FAST_CODE mixTable() motorValueWhenStopped = getReversibleMotorsThrottleDeadband(); mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax); + #ifdef USE_DSHOT if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { /* * We need to start the throttle output from stick input to start in the middle of the stick at the low and. @@ -549,6 +550,7 @@ void FAST_CODE mixTable() int throttleDistanceToMax = throttleRangeMax - rcCommand[THROTTLE]; mixerThrottleCommand = throttleRangeMin + throttleDistanceToMax; } + #endif } else { mixerThrottleCommand = rcCommand[THROTTLE]; throttleRangeMin = throttleIdleValue; From 725062b36df342c7ac22b5f3847322d92e67e21f Mon Sep 17 00:00:00 2001 From: giacomo892 Date: Tue, 14 Dec 2021 17:34:37 +0100 Subject: [PATCH 081/107] Add FOXEER F745 AIO Target --- src/main/target/FOXEERF745AIO/CMakeLists.txt | 1 + src/main/target/FOXEERF745AIO/target.c | 37 +++++ src/main/target/FOXEERF745AIO/target.h | 146 +++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 src/main/target/FOXEERF745AIO/CMakeLists.txt create mode 100644 src/main/target/FOXEERF745AIO/target.c create mode 100644 src/main/target/FOXEERF745AIO/target.h diff --git a/src/main/target/FOXEERF745AIO/CMakeLists.txt b/src/main/target/FOXEERF745AIO/CMakeLists.txt new file mode 100644 index 0000000000..a0c1257288 --- /dev/null +++ b/src/main/target/FOXEERF745AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f745xg(FOXEERF745AIO) diff --git a/src/main/target/FOXEERF745AIO/target.c b/src/main/target/FOXEERF745AIO/target.c new file mode 100644 index 0000000000..28786c70ba --- /dev/null +++ b/src/main/target/FOXEERF745AIO/target.c @@ -0,0 +1,37 @@ +/* + * 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/bus.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/pwm_mapping.h" + +const timerHardware_t timerHardware[] = { + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0) + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // M1 - D(1, 4, 5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR, 0, 0), // M2 - D(1, 5, 5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // M3 - D(1, 2, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR, 0, 0), // M4 - D(1, 7, 5) + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERF745AIO/target.h b/src/main/target/FOXEERF745AIO/target.h new file mode 100644 index 0000000000..dd1f307a3e --- /dev/null +++ b/src/main/target/FOXEERF745AIO/target.h @@ -0,0 +1,146 @@ +/* + * 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 "FXF5" +#define USBD_PRODUCT_STRING "FOXEERF745AIO" + +/*** Indicators ***/ +#define LED0 PC13 +#define BEEPER PD2 +#define BEEPER_INVERTED + +/*** IMU sensors ***/ +#define USE_EXTI + + +#define USE_MPU_DATA_READY_SIGNAL + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_BUS BUS_SPI3 +#define MPU6000_EXTI_PIN PD0 + + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PE4 +#define M25P16_SPI_BUS BUS_SPI4 + +/*** OSD ***/ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA4 + +/*** Serial ports ***/ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_CURRENT_METER) + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define SERIALRX_UART SERIAL_PORT_USART1 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define CURRENT_METER_SCALE 100 + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_DSHOT +#define USE_ESC_SENSOR + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff From 337dff0a33829afd6593cd916b5a2d8f5f5ab41e Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 18 Jan 2022 18:02:12 +0000 Subject: [PATCH 082/107] Use uint32_t in sortSerialTxBytesFree when calculating the bytes used. --- src/main/drivers/serial_softserial.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index aaed828375..255087f004 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -572,7 +572,7 @@ uint32_t softSerialTxBytesFree(const serialPort_t *instance) softSerial_t *s = (softSerial_t *)instance; - uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1); + uint32_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1); return (s->port.txBufferSize - 1) - bytesUsed; } From 7aa06a5f0022de9984a6f6521d0aeb6bd75372aa Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 18 Jan 2022 18:19:56 +0000 Subject: [PATCH 083/107] Send all changes each time drawScreen is called. Increase the TX buffer size of the selected serial port to accommodate. Add debug code (#defined out) --- src/main/io/displayport_hdzero_osd.c | 77 ++++++++++++++++++++++++---- 1 file changed, 67 insertions(+), 10 deletions(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index a8cb3bbf62..0013d5bfda 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -30,6 +30,8 @@ FILE_COMPILE_FOR_SPEED +//#define HDZERO_STATS + #if defined(USE_OSD) && defined(USE_HDZERO_OSD) #include "common/utils.h" @@ -52,8 +54,8 @@ FILE_COMPILE_FOR_SPEED #define MSP_WRITE_STRING 3 #define MSP_DRAW_SCREEN 4 #define MSP_SET_OPTIONS 5 -#define DRAW_FREQ_DENOM 4 -#define UPDATES_PER_CALL 10 +#define DRAW_FREQ_DENOM 4 // 60Hz +#define TX_BUFFER_SIZE 1024 #define VTX_TIMEOUT 1000 // 1 second timer static mspProcessCommandFnPtr mspProcessCommand; @@ -72,6 +74,11 @@ static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character extern uint8_t cliMode; +#ifdef HDZERO_STATS +static uint32_t dataSent; +static uint8_t resetCount; +#endif + static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len) { UNUSED(displayPort); @@ -82,6 +89,10 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int sent = mspSerialPushPort(cmd, subcmd, len, &hdZeroMspPort, MSP_V1); } +#ifdef HDZERO_STATS + dataSent += sent; +#endif + return sent; } @@ -125,8 +136,7 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1 } *c = screen[pos]; - if (bitArrayGet(fontPage, pos)) - { + if (bitArrayGet(fontPage, pos)) { *c |= 0x100; } @@ -165,13 +175,46 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con UNUSED(attr); uint16_t pos = (row * COLS) + col; - while (*string) - { + while (*string) { setChar(pos++, *string++); } return 0; } +#ifdef HDZERO_STATS +static void printStats(displayPort_t *displayPort, uint32_t updates) +{ + static timeMs_t lastTime; + static uint32_t maxDataSent, maxBufferUsed, maxUpdates; + timeMs_t currentTime = millis(); + char lineBuffer[100]; + + if (updates > maxUpdates) { + maxUpdates = updates; // updates sent per displayWrite + } + + uint32_t bufferUsed = TX_BUFFER_SIZE - serialTxBytesFree(hdZeroMspPort.port); + if (bufferUsed > maxBufferUsed) { + maxBufferUsed = bufferUsed; // serial buffer used after displayWrite + } + + uint32_t diff = (currentTime - lastTime); + if (diff > 1000) { // Data sampled in 1 second + if (dataSent > maxDataSent) { + maxDataSent = dataSent; // bps (max 11520 allowed) + } + + dataSent = 0; + lastTime = currentTime; + } + + + tfp_sprintf(lineBuffer, "R:%2d %4ld %5ld(%5ld) U:%2ld(%2ld) B:%3ld(%4ld,%4ld)", resetCount, (millis()-vtxHeartbeat), + dataSent, maxDataSent, updates, maxUpdates, bufferUsed, maxBufferUsed, hdZeroMspPort.port->txBufferSize); + writeString(displayPort, 0, 17, lineBuffer, 0); +} +#endif + /** * Write only changed characters to the VTX */ @@ -179,13 +222,13 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz { static uint8_t counter = 0; - if ((counter++ % DRAW_FREQ_DENOM) == 0) { // 62.5Hz + if ((counter++ % DRAW_FREQ_DENOM) == 0) { uint8_t subcmd[COLS + 4]; uint8_t updateCount = 0; subcmd[0] = MSP_WRITE_STRING; int next = BITARRAY_FIND_FIRST_SET(dirty, 0); - while (next >= 0 && updateCount < UPDATES_PER_CALL) { + while (next >= 0) { // Look for sequential dirty characters on the same line for the same font page int pos = next; uint8_t row = pos / COLS; @@ -211,15 +254,20 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz next = BITARRAY_FIND_FIRST_SET(dirty, pos); } - if (updateCount > 0) - { + if (updateCount > 0) { subcmd[0] = MSP_DRAW_SCREEN; output(displayPort, MSP_DISPLAYPORT, subcmd, 1); } +#ifdef HDZERO_STATS + printStats(displayPort, updateCount); +#endif checkVtxPresent(); if (vtxReset) { +#ifdef HDZERO_STATS + resetCount++; +#endif clearScreen(displayPort); vtxReset = false; } @@ -311,14 +359,23 @@ static const displayPortVTable_t hdzeroOsdVTable = { bool hdzeroOsdSerialInit(void) { + static volatile uint8_t txBuffer[TX_BUFFER_SIZE]; memset(&hdZeroMspPort, 0, sizeof(mspPort_t)); serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD); if (portConfig) { serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + if (port) { + // Use a bigger TX buffer size to accommodate the configuration menus + port->txBuffer = txBuffer; + port->txBufferSize = TX_BUFFER_SIZE; + port->txBufferTail = 0; + port->txBufferHead = 0; + resetMspPort(&hdZeroMspPort, port); + return true; } } From 87bebf4b97ad25e7fa54abf39529e18f76815c86 Mon Sep 17 00:00:00 2001 From: yajo10 Date: Wed, 19 Jan 2022 15:43:32 +0100 Subject: [PATCH 084/107] Fix code style: align compiler directives to left --- src/main/flight/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e9bc0c56cb..af974a3734 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -541,7 +541,7 @@ void FAST_CODE mixTable() motorValueWhenStopped = getReversibleMotorsThrottleDeadband(); mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax); - #ifdef USE_DSHOT +#ifdef USE_DSHOT if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { /* * We need to start the throttle output from stick input to start in the middle of the stick at the low and. @@ -550,7 +550,7 @@ void FAST_CODE mixTable() int throttleDistanceToMax = throttleRangeMax - rcCommand[THROTTLE]; mixerThrottleCommand = throttleRangeMin + throttleDistanceToMax; } - #endif +#endif } else { mixerThrottleCommand = rcCommand[THROTTLE]; throttleRangeMin = throttleIdleValue; From 9d1d61ee1c9a71abea6400869d32d5eaa2d30d18 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 19 Jan 2022 20:44:36 -0300 Subject: [PATCH 085/107] [maths.h] Add Macro to convert Celsius to Kelvin --- src/main/common/maths.h | 2 ++ src/main/drivers/pitotmeter/pitotmeter_ms4525.c | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 4450723a2b..4348a77fc3 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -66,6 +66,8 @@ #define CMSEC_TO_CENTIKPH(cms) (cms * 3.6) #define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845) +#define C_TO_KELVIN(temp) (temp + 273.15f) + // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ ( __extension__ ({ \ diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c index 436bddc3cf..3b731eaa49 100644 --- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c +++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c @@ -97,7 +97,7 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f; const float dP_psi = -((ctx->ms4525_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); float dP = dP_psi * PSI_to_Pa; - float T = (float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f + 273.15f; + float T = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f); if (pressure) { *pressure = dP; // Pa From b90bebe2fd70c86af24b628ab5f39450b64f4915 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 19 Jan 2022 21:05:45 -0300 Subject: [PATCH 086/107] Add macros to multicopter land detector --- src/main/navigation/navigation_multicopter.c | 10 +++++----- src/main/navigation/navigation_private.h | 5 +++++ 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2247d05835..b17b4432b9 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -696,17 +696,17 @@ bool isMulticopterLandingDetected(void) } // Average climb rate should be low enough - bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > 25.0f; + bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > MC_LAND_CHECK_VEL_Z_MOVING; // check if we are moving horizontally - bool horizontalMovement = posControl.actualState.velXY > 100.0f; + bool horizontalMovement = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING; // We have likely landed if throttle is 40 units below average descend throttle // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core) // Wait for 1 second so throttle has stabilized. bool isAtMinimalThrust = false; - if (currentTimeUs - landingDetectorStartedAt > 1000 * 1000) { + if (currentTimeUs - landingDetectorStartedAt > HZ2US(MC_LAND_THR_SUM_RATE)) { landingThrSamples += 1; landingThrSum += rcCommandAdjustedThrottle; isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40); @@ -719,7 +719,7 @@ bool isMulticopterLandingDetected(void) // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety. // TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy. // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed - possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f)); + possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + MC_LAND_SAFE_SURFACE)); } if (!possibleLandingDetected) { @@ -727,7 +727,7 @@ bool isMulticopterLandingDetected(void) return false; } else { - return ((currentTimeUs - landingTimer) > (navConfig()->mc.auto_disarm_delay * 1000)) ? true : false; + return ((currentTimeUs - landingTimer) > MS2US(navConfig()->mc.auto_disarm_delay)) ? true : false; } } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 67497f5ec2..d618637c9a 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -35,6 +35,11 @@ #define INAV_SURFACE_MAX_DISTANCE 40 +#define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s +#define MC_LAND_CHECK_VEL_Z_MOVING 25.0f // cm/s +#define MC_LAND_THR_SUM_RATE 1 // hz +#define MC_LAND_SAFE_SURFACE 5.0f // cm + #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); From 8ae1060dc26d837f71317d92dd2cda36d2c8676f Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Wed, 19 Jan 2022 21:14:01 -0300 Subject: [PATCH 087/107] add new macro --- src/main/navigation/navigation_multicopter.c | 2 +- src/main/navigation/navigation_private.h | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index b17b4432b9..ee251f60e3 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -709,7 +709,7 @@ bool isMulticopterLandingDetected(void) if (currentTimeUs - landingDetectorStartedAt > HZ2US(MC_LAND_THR_SUM_RATE)) { landingThrSamples += 1; landingThrSum += rcCommandAdjustedThrottle; - isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40); + isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE); } bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index d618637c9a..bd51e3a717 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -38,6 +38,7 @@ #define MC_LAND_CHECK_VEL_XY_MOVING 100.0f // cm/s #define MC_LAND_CHECK_VEL_Z_MOVING 25.0f // cm/s #define MC_LAND_THR_SUM_RATE 1 // hz +#define MC_LAND_DESCEND_THROTTLE 40 // uS #define MC_LAND_SAFE_SURFACE 5.0f // cm #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro From d8ef57de5dc741abccc53d4d140af1013f583e15 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 20 Jan 2022 12:29:44 +0000 Subject: [PATCH 088/107] User the peripheral baud rate and not the msp baud rate --- src/main/io/displayport_hdzero_osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index 0013d5bfda..c5883d9ea4 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -365,7 +365,7 @@ bool hdzeroOsdSerialInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HDZERO_OSD); if (portConfig) { serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_HDZERO_OSD, NULL, NULL, - baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); if (port) { // Use a bigger TX buffer size to accommodate the configuration menus From da8cc2e9d2d5a54664ae92f04911b4dec70ee7d9 Mon Sep 17 00:00:00 2001 From: Martin Luessi Date: Wed, 12 Jan 2022 16:00:55 -0800 Subject: [PATCH 089/107] Fix compilation when USE_SECONDARY_IMU not defined --- src/main/fc/cli.c | 4 ++++ src/main/fc/fc_msp.c | 4 ++++ src/main/flight/secondary_imu.c | 7 ++++++- src/main/sensors/diagnostics.c | 4 ++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5d75c0342a..71533e6d6a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3344,7 +3344,11 @@ static void cliStatus(char *cmdline) hardwareSensorStatusNames[getHwRangefinderStatus()], hardwareSensorStatusNames[getHwOpticalFlowStatus()], hardwareSensorStatusNames[getHwGPSStatus()], +#ifdef USE_SECONDARY_IMU hardwareSensorStatusNames[getHwSecondaryImuStatus()] +#else + hardwareSensorStatusNames[0] +#endif ); #ifdef USE_ESC_SENSOR diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f0194a72c5..36d91b0bbb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -425,7 +425,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, getHwRangefinderStatus()); sbufWriteU8(dst, getHwPitotmeterStatus()); sbufWriteU8(dst, getHwOpticalFlowStatus()); +#ifdef USE_SECONDARY_IMU sbufWriteU8(dst, getHwSecondaryImuStatus()); +#else + sbufWriteU8(dst, 0); +#endif break; case MSP_ACTIVEBOXES: diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 21a0f613af..c8323f9940 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -22,6 +22,9 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include "platform.h" +#ifdef USE_SECONDARY_IMU + #include "stdint.h" #include "build/debug.h" @@ -256,4 +259,6 @@ hardwareSensorStatus_e getHwSecondaryImuStatus(void) #else return HW_SENSOR_NONE; #endif -} \ No newline at end of file +} + +#endif diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 2825440b49..ed36e3e23c 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -227,7 +227,11 @@ bool isHardwareHealthy(void) const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); const hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus(); +#ifdef USE_SECONDARY_IMU const hardwareSensorStatus_e imu2Status = getHwSecondaryImuStatus(); +#else + const hardwareSensorStatus_e imu2Status = HW_SENSOR_NONE; +#endif // Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings) if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY) From a454883a132fabd04b59bd9f8559153f32575270 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 20 Jan 2022 20:18:57 +0000 Subject: [PATCH 090/107] Added max length to name --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8073301585..cfe615d3d6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2818,7 +2818,7 @@ Craft name | Default | Min | Max | | --- | --- | --- | -| _empty_ | | | +| _empty_ | | MAX_NAME_LENGTH | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2e2e65a610..26371764da 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3510,6 +3510,7 @@ groups: - name: name description: "Craft name" default_value: "" + max: MAX_NAME_LENGTH - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG type: modeActivationOperatorConfig_t From 36027c2f2683500bb645fd5ae8fa7fe3cae34465 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sat, 22 Jan 2022 12:09:44 +0000 Subject: [PATCH 091/107] Ensure a drawscreen message is sent to VTX if the screen has been cleared. --- src/main/io/displayport_hdzero_osd.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/io/displayport_hdzero_osd.c b/src/main/io/displayport_hdzero_osd.c index c5883d9ea4..ee2462e5fc 100644 --- a/src/main/io/displayport_hdzero_osd.c +++ b/src/main/io/displayport_hdzero_osd.c @@ -71,6 +71,7 @@ static timeMs_t vtxHeartbeat; static uint8_t screen[SCREENSIZE]; static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen +static bool screenCleared; extern uint8_t cliMode; @@ -123,6 +124,7 @@ static int clearScreen(displayPort_t *displayPort) hdZeroInit(); setHdMode(displayPort); + screenCleared = true; return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd)); } @@ -254,7 +256,10 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz next = BITARRAY_FIND_FIRST_SET(dirty, pos); } - if (updateCount > 0) { + if (updateCount > 0 || screenCleared) { + if (screenCleared) { + screenCleared = false; + } subcmd[0] = MSP_DRAW_SCREEN; output(displayPort, MSP_DISPLAYPORT, subcmd, 1); } From 681a838090077ec905ac5a1d6617be9b37ec91cd Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Mon, 24 Jan 2022 20:04:27 -0300 Subject: [PATCH 092/107] [fc_core.c] Make use of macro US2S --- src/main/fc/fc_core.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 74e90fadc7..0a91630b1c 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -978,12 +978,12 @@ void taskUpdateRxMain(timeUs_t currentTimeUs) // returns seconds float getFlightTime() { - return (float)(flightTime / 1000) / 1000; + return US2S(flightTime); } float getArmTime() { - return (float)(armTime / 1000) / 1000; + return US2S(armTime); } void fcReboot(bool bootLoader) From bb702c1337b5a2e0e3fea562630b65b4d7bfaa2b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 25 Jan 2022 08:56:04 +0100 Subject: [PATCH 093/107] Fix OSD PG version --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2faeb729e2..90cbf84e21 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -196,8 +196,8 @@ 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, 5); -PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6); +PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); static int digitCount(int32_t value) { From ced7c444777bea3ec7808166dbb703629898d011 Mon Sep 17 00:00:00 2001 From: Krasiyan Nedelchev Date: Tue, 1 Feb 2022 01:24:26 +0100 Subject: [PATCH 094/107] add missing bash shebang in the build.sh script --- build.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/build.sh b/build.sh index 59d9221b6e..da4b69dc2b 100755 --- a/build.sh +++ b/build.sh @@ -1,3 +1,4 @@ +#!/usr/bin/env bash set -e if [[ $# == 0 ]]; then From 58806a2bd638960367f0d339f936518533d9655a Mon Sep 17 00:00:00 2001 From: Krasiyan Nedelchev Date: Tue, 1 Feb 2022 01:27:16 +0100 Subject: [PATCH 095/107] extend the build.sh script usage notes; add reference in the dev "Building in Docker.md" --- build.sh | 10 ++++++++-- docs/development/Building in Docker.md | 6 ++++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/build.sh b/build.sh index da4b69dc2b..58db51aa42 100755 --- a/build.sh +++ b/build.sh @@ -7,9 +7,15 @@ Usage syntax: ./build.sh Notes: * You can specify multiple targets. - * If no targets are specified, *all* of them will be built. + ./build.sh + * To get a list of all targets use \"help\". Hint: pipe the output through a pager. + ./build.sh help | less + * To build all targets use \"all\" + ./build.sh all * To clean a target prefix it with \"clean_\". - * To clean all targets just use \"clean\"." + ./build.sh clean_MATEKF405SE + * To clean all targets just use \"clean\". + ./build.sh clean" exit 1 fi diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md index 99a8784e6d..eafb9b5e63 100755 --- a/docs/development/Building in Docker.md +++ b/docs/development/Building in Docker.md @@ -20,6 +20,12 @@ Where `` must be replaced with the name of the target that you want to b ./build.sh MATEKF405SE ``` +Run the script with no arguments to get more details on its usage: + +``` +./build.sh +``` + ## 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` . From 334c48a8c603c5a89bcb5f253f96289c94b553bd Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Wed, 2 Feb 2022 14:01:06 -0500 Subject: [PATCH 096/107] CRSF code clean up Removed extra stat used for OSD that was not part of the protocol. Also beautified the mW power array/stat for future CRSFv3 port. --- src/main/io/osd.c | 2 +- src/main/rx/crsf.c | 10 +++++----- src/main/rx/rx.h | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 90cbf84e21..6603d6010e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2015,7 +2015,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_CRSF_RSSI_DBM: { int16_t rssi = rxLinkStatistics.uplinkRSSI; - buff[0] = (rxLinkStatistics.activeAnt == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna + buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna if (rssi <= -100) { tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); } else { diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 01b9a72abd..9aace4660a 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) +#define CRSF_POWER_COUNT 9 STATIC_UNIT_TESTED bool crsfFrameDone = false; STATIC_UNIT_TESTED crsfFrame_t crsfFrame; @@ -59,8 +60,7 @@ static timeUs_t crsfFrameStartAt = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; -// The power levels represented by uplinkTXPower above in mW (250mW added to full TX in v4.00 firmware, 50mW added for ExpressLRS) -const uint16_t crsfPowerStates[] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50}; +const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50}; /* * CRSF protocol @@ -122,7 +122,6 @@ typedef struct crsfPayloadLinkStatistics_s { uint8_t downlinkRSSI; uint8_t downlinkLQ; int8_t downlinkSNR; - uint8_t activeAnt; } __attribute__ ((__packed__)) crsfPayloadLinkStatistics_t; typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t; @@ -234,13 +233,14 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload; + const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0; rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1); rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ; rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR; rxLinkStatistics.rfMode = linkStats->rfMode; - rxLinkStatistics.uplinkTXPower = crsfPowerStates[linkStats->uplinkTXPower]; - rxLinkStatistics.activeAnt = linkStats->activeAntenna; + rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex]; + rxLinkStatistics.activeAntenna = linkStats->activeAntenna; if (rxLinkStatistics.uplinkLQ > 0) { int16_t uplinkStrength; // RSSI dBm converted to % diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5b00281905..665f6d6e60 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -191,7 +191,7 @@ typedef struct rxLinkStatistics_s { int8_t uplinkSNR; // The SNR of the uplink in dB uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] uint16_t uplinkTXPower; // power in mW - uint8_t activeAnt; + uint8_t activeAntenna; } rxLinkStatistics_t; extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount @@ -227,5 +227,5 @@ uint16_t rxGetRefreshRate(void); // Processed RC channel value. These values might include // filtering and some extra processing like value holding -// during failsafe. +// during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); From db046d9b08e38f7cf452629334459c6a84284efd Mon Sep 17 00:00:00 2001 From: Andr0x Date: Wed, 2 Feb 2022 22:42:28 +0100 Subject: [PATCH 097/107] Optiical Flow adn Lidar This enable the Otical FLow and the lidar in SPEEDYBEEF7V2 adapt to the right UART port in the following command in CLI for activating (UART 5 in the example) serial 4 1 115200 115200 0 115200 set rangefinder_hardware = MSP set opflow_hardware = MSP --- src/main/target/SPEEDYBEEF7V2/target.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/target/SPEEDYBEEF7V2/target.h b/src/main/target/SPEEDYBEEF7V2/target.h index 33ac6a2b9c..82b9655ca0 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.h +++ b/src/main/target/SPEEDYBEEF7V2/target.h @@ -141,6 +141,14 @@ #define USE_LED_STRIP #define WS2811_PIN PB0 +// ********** Optiical Flow adn Lidar ************** + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + + #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) #define USE_DSHOT #define USE_ESC_SENSOR From 1043a63222e38b2b9e944fe34615120d4f72ab34 Mon Sep 17 00:00:00 2001 From: Andr0x Date: Wed, 2 Feb 2022 22:54:43 +0100 Subject: [PATCH 098/107] Optical Flow and Lidar This enable the Otical Flow and the lidar in SPEEDYBEEF7V2 adapt to the right UART port in the following command in CLI for activating (UART 5 in the example) Tested with Matek 3901-L0X serial 4 1 115200 115200 0 115200 set rangefinder_hardware = MSP set opflow_hardware = MSP --- src/main/target/SPEEDYBEEF7V2/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/SPEEDYBEEF7V2/target.h b/src/main/target/SPEEDYBEEF7V2/target.h index 82b9655ca0..dbd4263db0 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.h +++ b/src/main/target/SPEEDYBEEF7V2/target.h @@ -142,13 +142,13 @@ #define WS2811_PIN PB0 // ********** Optiical Flow adn Lidar ************** - #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP #define USE_OPFLOW #define USE_OPFLOW_MSP + #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) #define USE_DSHOT #define USE_ESC_SENSOR From 0e4e6cf6b83699f2c3511b45074427a1c6780a3c Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 5 Feb 2022 11:38:44 +0000 Subject: [PATCH 099/107] Update Battery.md Added missing setings to the battery profile description --- docs/Battery.md | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/docs/Battery.md b/docs/Battery.md index f7277b264a..6fc3a12570 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -195,7 +195,25 @@ Note that in this example even though your warning capacity (`battery_capacity_w ## Battery profiles -Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): `bat_cells`, `vbat_cell_detect_voltage`, `vbat_max_cell_voltage`, `vbat_warning_cell_voltage`, `vbat_min_cell_voltage`, `battery_capacity_unit`, `battery_capacity`, `battery_capacity_warning`, `battery_capacity_critical` +Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): +- `bat_cells` +- `vbat_cell_detect_voltage` +- `vbat_max_cell_voltage` +- `vbat_warning_cell_voltage` +- `vbat_min_cell_voltage` +- `battery_capacity_unit` +- `battery_capacity` +- `battery_capacity_warning` +- `battery_capacity_critical` +- `throttle_idle` +- `fw_min_throttle_down_pitch` +- `nav_fw_cruise_thr` +- `nav_fw_min_thr` +- `nav_fw_pitch2thr` +- `nav_fw_launch_thr` +- `nav_fw_launch_idle_thr` +- `failsafe_throttle` +- `nav_mc_hover_thr` To enable the automatic battery profile switching based on battery voltage enable the `BAT_PROF_AUTOSWITCH` feature. For a profile to be automatically selected the number of cells of the battery needs to be specified (>0). From bdc83e4e0e615287279b76fbc43436699a4d4d46 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 6 Feb 2022 18:09:12 +0000 Subject: [PATCH 100/107] Added some comparative operations Added comparative operations for constraints, min, and max. --- docs/Programming Framework.md | 4 ++++ src/main/programming/logic_condition.c | 16 ++++++++++++++++ src/main/programming/logic_condition.h | 6 +++++- 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 91a1003693..bd2b3b2fcc 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -76,6 +76,10 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | | 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | | 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | +| 43 | CONSTRAIN_MIN | Ensures that `Operand A` cannot be less than `Operand B` | +| 44 | CONSTRAIN_MAX | Ensures that `Operand A` cannot be greater than `Operand B` | +| 45 | MIN | Finds the lowest value of `Operand A` and `Operand B` | +| 46 | MAX | Finds the highest value of `Operand A` and `Operand B` | ### Operands diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 9ad94591e8..7e3b0e1229 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -303,6 +303,22 @@ static int logicConditionCompute( temporaryValue = (operandB == 0) ? 500 : operandB; return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; + + case LOGIC_CONDITION_CONSTRAIN_MIN: + return (operandA < operandB) ? operandB : operandA; + break; + + case LOGIC_CONDITION_CONSTRAIN_MAX: + return (operandA > operandB) ? operandB : operandA; + break; + + case LOGIC_CONDITION_MIN: + return (operandA < operandB) ? operandA : operandB; + break; + + case LOGIC_CONDITION_MAX: + return (operandA > operandB) ? operandA : operandB; + break; case LOGIC_CONDITION_MAP_INPUT: return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000); diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 4e32bd0ac9..55251dd1bd 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -72,7 +72,11 @@ typedef enum { LOGIC_CONDITION_MODULUS = 40, LOGIC_CONDITION_LOITER_OVERRIDE = 41, LOGIC_CONDITION_SET_PROFILE = 42, - LOGIC_CONDITION_LAST = 43, + LOGIC_CONDITION_CONSTRAIN_MIN = 43, + LOGIC_CONDITION_CONSTRAIN_MAX = 44, + LOGIC_CONDITION_MIN = 45, + LOGIC_CONDITION_MAX = 46, + LOGIC_CONDITION_LAST = 47, } logicOperation_e; typedef enum logicOperandType_s { From 78f2bb6f073d7a0329f87bd526350cbf20d2d03b Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Sun, 6 Feb 2022 16:29:59 -0300 Subject: [PATCH 101/107] Remove duplicate semicolon --- src/main/flight/pid_autotune.c | 2 +- src/main/io/osd_dji_hd.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 98ca4b9ba6..d9688f1e82 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -198,7 +198,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa tuneCurrent[axis].updateCount++; tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); - tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);; + tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) { if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 3ebb3200e1..99205cdddb 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1051,7 +1051,7 @@ static bool djiFormatMessages(char *buff) // 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)]);; + strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]); haveMessage = true; } } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { From 3d6f4f94fdaa468856d28a7c0ea2121d2e07f635 Mon Sep 17 00:00:00 2001 From: JuliooCesarMDM Date: Sun, 6 Feb 2022 16:32:02 -0300 Subject: [PATCH 102/107] +1 --- src/main/drivers/io_port_expander.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/io_port_expander.c b/src/main/drivers/io_port_expander.c index 5125fe2260..40717633aa 100644 --- a/src/main/drivers/io_port_expander.c +++ b/src/main/drivers/io_port_expander.c @@ -60,7 +60,7 @@ void ioPortExpanderSync(void) { if (ioPortExpanderState.active && ioPortExpanderState.shouldSync) { pcf8574Write(ioPortExpanderState.state); - ioPortExpanderState.shouldSync = false;; + ioPortExpanderState.shouldSync = false; } } From 2f00540c604fb85044acc1e3becf12fc9e17294b Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 7 Feb 2022 08:30:56 +0000 Subject: [PATCH 103/107] Removed constrain min and max as they are unnecessary --- docs/Programming Framework.md | 6 ++---- src/main/programming/logic_condition.c | 8 -------- src/main/programming/logic_condition.h | 8 +++----- 3 files changed, 5 insertions(+), 17 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index bd2b3b2fcc..3052b4f8ef 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -76,10 +76,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder | | 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | | 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | -| 43 | CONSTRAIN_MIN | Ensures that `Operand A` cannot be less than `Operand B` | -| 44 | CONSTRAIN_MAX | Ensures that `Operand A` cannot be greater than `Operand B` | -| 45 | MIN | Finds the lowest value of `Operand A` and `Operand B` | -| 46 | MAX | Finds the highest value of `Operand A` and `Operand B` | +| 43 | MIN | Finds the lowest value of `Operand A` and `Operand B` | +| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` | ### Operands diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 7e3b0e1229..19b0cb6e2d 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -304,14 +304,6 @@ static int logicConditionCompute( return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; - case LOGIC_CONDITION_CONSTRAIN_MIN: - return (operandA < operandB) ? operandB : operandA; - break; - - case LOGIC_CONDITION_CONSTRAIN_MAX: - return (operandA > operandB) ? operandB : operandA; - break; - case LOGIC_CONDITION_MIN: return (operandA < operandB) ? operandA : operandB; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 55251dd1bd..68e69d68c8 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -72,11 +72,9 @@ typedef enum { LOGIC_CONDITION_MODULUS = 40, LOGIC_CONDITION_LOITER_OVERRIDE = 41, LOGIC_CONDITION_SET_PROFILE = 42, - LOGIC_CONDITION_CONSTRAIN_MIN = 43, - LOGIC_CONDITION_CONSTRAIN_MAX = 44, - LOGIC_CONDITION_MIN = 45, - LOGIC_CONDITION_MAX = 46, - LOGIC_CONDITION_LAST = 47, + LOGIC_CONDITION_MIN = 43, + LOGIC_CONDITION_MAX = 44, + LOGIC_CONDITION_LAST = 45, } logicOperation_e; typedef enum logicOperandType_s { From 18da0f2c620f8b732d7b5a749c3bc9e77b064da3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 7 Feb 2022 11:50:45 +0100 Subject: [PATCH 104/107] Mamba F722_X8 target --- src/main/target/MAMBAF722_X8/CMakeLists.txt | 1 + src/main/target/MAMBAF722_X8/config.c | 61 +++++++ src/main/target/MAMBAF722_X8/target.c | 43 +++++ src/main/target/MAMBAF722_X8/target.h | 172 ++++++++++++++++++++ 4 files changed, 277 insertions(+) create mode 100644 src/main/target/MAMBAF722_X8/CMakeLists.txt create mode 100644 src/main/target/MAMBAF722_X8/config.c create mode 100644 src/main/target/MAMBAF722_X8/target.c create mode 100644 src/main/target/MAMBAF722_X8/target.h diff --git a/src/main/target/MAMBAF722_X8/CMakeLists.txt b/src/main/target/MAMBAF722_X8/CMakeLists.txt new file mode 100644 index 0000000000..dcec183c1c --- /dev/null +++ b/src/main/target/MAMBAF722_X8/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(MAMBAF722_X8) \ No newline at end of file diff --git a/src/main/target/MAMBAF722_X8/config.c b/src/main/target/MAMBAF722_X8/config.c new file mode 100644 index 0000000000..3165a2efb4 --- /dev/null +++ b/src/main/target/MAMBAF722_X8/config.c @@ -0,0 +1,61 @@ +/* + * 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 "common/axis.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" + +void targetConfiguration(void) +{ + + /* + * UART1 is SerialRX + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + + /* + * Enable MSP at 115200 at UART4 + */ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/MAMBAF722_X8/target.c b/src/main/target/MAMBAF722_X8/target.c new file mode 100644 index 0000000000..f630fdfa26 --- /dev/null +++ b/src/main/target/MAMBAF722_X8/target.c @@ -0,0 +1,43 @@ +/* + * 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 "platform.h" +#include "drivers/io.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" +#include "drivers/bus.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MAMBAF722_X8/target.h b/src/main/target/MAMBAF722_X8/target.h new file mode 100644 index 0000000000..c9d86ec22e --- /dev/null +++ b/src/main/target/MAMBAF722_X8/target.h @@ -0,0 +1,172 @@ +/* + * 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 USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "M7X8" +#define USBD_PRODUCT_STRING "MAMBAF722_X8" + +// ******** Board LEDs ********************** +#define LED0 PC15 +#define LED1 PC14 + +// ******* Beeper *********** +#define BEEPER PB2 +#define BEEPER_INVERTED + +// ******* GYRO and ACC ******** +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG + +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + + +// *************** Baro ************************** +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 + +//*********** Magnetometer / Compass ************* +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS + +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +// ******* SERIAL ******** +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +// ******* SPI ******** +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +// ******* ADC ******** +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define VBAT_SCALE_DEFAULT 1100 + +// ******* OSD ******** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +//******* FLASH ******** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +//************ LEDSTRIP ***************** +#define USE_LED_STRIP +#define WS2811_PIN PB3 + +// ******* FEATURES ******** +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY) + +#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 8 +#define TARGET_MOTOR_COUNT 8 + +// ESC-related features +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define BNO055_I2C_BUS DEFAULT_I2C_BUS From b8cf0272e927dacecb6f91cf4f6713a900c34d7f Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 9 Feb 2022 17:51:56 +0100 Subject: [PATCH 105/107] battery cells in logic condition --- src/main/programming/logic_condition.c | 5 +++++ src/main/programming/logic_condition.h | 1 + 2 files changed, 6 insertions(+) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 19b0cb6e2d..0fc6d0d6fd 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -431,6 +431,11 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10 return getBatteryAverageCellVoltage(); break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS: + return getBatteryCellCount(); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 return getAmperage(); break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 68e69d68c8..69f3d9c7d9 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -126,6 +126,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 35 LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36 + LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS, // 37 } logicFlightOperands_e; From c73cdc9e5759e6ca208e71b507edcc0e9aa4467e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 10 Feb 2022 09:33:12 +0100 Subject: [PATCH 106/107] Adjust MatekH743 LED DMA channel --- src/main/target/MATEKH743/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 08a7dae478..723c084e40 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -47,7 +47,7 @@ const timerHardware_t timerHardware[] = { 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, 8), // LED_2812 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM From c4bcd2ea2adde17915effcd32419e7a721a51f1c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 11 Feb 2022 15:40:51 +0100 Subject: [PATCH 107/107] Versio bump --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 316531a8b2..f126830cfd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 4.1.0) +project(INAV VERSION 5.0.0) enable_language(ASM)