Prevents possibility of changing paramaters after initializing affecting runtime operation of RC smoothing. Parameter values are loaded during initialization instead of relying on the current PG values.
The blackbox "P" frame is incorrectly offsetting the rcCommand[THROTTLE] value by `min_throttle`. The rcCommand value already ranges from 1000-2000 and is an input so it shouldn't be offset by a motor output value. Also this is clearly incorrect in the case of DSHOT which doesn't use `min_throttle`.
Currently only rcCommand values are included in the log data and the configurator calculates the actual setpoint values based on rates values added to the blackbox header. The problem with this is that the rates information is only written at arming so if the rates change during the log (rateprofile change, in-flight adjustments, etc.) then the calculated setpoints will be incorrect. There's no way to tell from the log that this happened. This often causes confusion because it will suddenly make it appear in the log that the PID controller is not acheiving the requested rates when it's just a presentation error. Also the rates will be incorrectly calculated when the user selects Raceflight style rates as the rates type is not supplied in the log header (and the viewer doesn't have the forumla for them anyway).
This change adds the actual setpoint values for each axis as used by the PID controller, removing the necessity for the viewer to perform any calculations. In addition to showing any rate changes, it will also show any cases where other flight features have modified the setpoints from the user's input. These were invisible previously (examples include level modes, Acro Trainer, GPS Rescue, yaw spin recovery, etc.).
Also the throttle value used in the mixer is included in the throttle axis. This allow visualization of things that affect the commanded throttle like throttle boost, throttle limit, GPS Rescue, angle level strength, etc.
Adds `rc_smoothing_auto_smoothness` parameter that allows the user to tune the filter auto cutoff calculation. Increasing the value makes the resulting rcCommand traces smoother.
Allows the user to retain the "auto" calculation which is important for protocols that can change rate (like CRSF), but still tune the resulting "smoothness" to their preference.
Updated to conform to style guide.
Fixed issue with statement ordering. Minimum rates of 200deg/s.
Added rate limits to MSP for use in Configurator.
Refactoring to improve efficiency.
Revert 'msp.c' changes.
Added BUILD_BUG_ON(CONTROL_RATE_CONFIG_RATE_LIMIT_MAX > SETPOINT_RATE_LIMIT).
Added rate_limits to log header to faciliate support for blackbox-log-viewer to correctly handle the rate calculation.
Use STATIC_ASSERT() for build protection.
Harmonized (and partly corrected) all occurancies of gpsSol.llh.alt and getEstimatedAltitude() to handle altiude sourced in cm resolution.
This was introduced by GSP_RESCUE/RTH.
Introduced a naming convention that include the unit into the variable/function names:
gpsSol.llh.alt -> gpsSol.llh.alt_cm
getEstimatedAltitude() -> getEstimatedAltitude_cm()
Restructures the PID controller to decouple feedforward from D.
Cleaned up the structure of the PID controller; moved some feature-based enhancements out of the main structure.
Feedforward becomes a separate component of the PID controller and there is now:
f_pitch
f_roll
f_yaw
The default values of 60 for pitch and roll matches the default setpoint weight used in BF3.4. Yaw previously had no setpoint weight capability so the default here needs to be discussed. Currently it's also set to 60 and flight testing seems positive. Feedforward on yaw adds a lot of value so I don't think we want to default to 0. Instead we need decide on the default.
All occurences of setpoint weight have been replaced by feedforward. "setpoint_relax_ratio" has been renamed to "feedforward_transition".
The pidSum now consists of P + I + D + F.
D has been added back for yaw (disabled by default with d_yaw = 0). We've found little need for D for normal quads but it may have value for other configurations - particularly tricopters.
Updated CMS menus to support adjusting the feedforward for each axis.
Changed the default for "rc_interp_ch" to be "RPYT". Need yaw to be smoothed to support feedforward.
Open issues:
Needs BFC support
- Need to add support for the axis "F" gains.
- Remove "setpoint weight" slider.
- Rename "D Setpoint transition" to "Feedforward transition"
Needs BBE support
- Header "setpoint_relaxation_ratio" has been renamed "feedforward_transition"
- Header "dterm_setpoint_weight" has been replaced with an array named "feed_forward_weight".
example: H feed_forward_weight:65,60,60 (R,P,Y)
- PID component "AXISF" has been added for all axes. Should be handled like P, I and D values.
- PidSum calculation needs to include F.
Needs LUA script support
- Support the renamed "setpoint_relax_ratio".
- Support for feedforward weight on all 3 axes.
Open code issues:
- rc_adjustments.c - support for adjusting feedforward weight for all axes. Currently only supporting roll - needs coordination with BFC.
Adds in flight monitoring of the rx frame rate and adapts the filters if the frame rate changes. Primarily to add support for Crossfire with its ability to switch from 150hz to 50hz (and back) under some circumstances. Will work with any protocol - not CRSF specific. So if future receivers add the ability to switch frame rates dynamically the logic should support them.
If the current rx frame rate is more than +-20% from the previously detected rate, then the process will retrain for the next 50 samples as long as the rate continues to be outside the 20% tolerance. Once 50 samples are collected the new frame rate is updated and the filter cutoffs are adjusted. Only filters set with their cutoffs = 0 (auto) will be adjusted. There is a 2 second guard time after a successful update before retraining can start again to prevent rapid switching back and forth.
The logic is optimized to not perform any training if the filters are set to manual cutoffs. So there is an opportunity for advanced users to choose specific cutoffs and reduce the PID loop load slightly. However this is not recommended for Crossfire or other protocols that might change their rx frame rate.
Updated the output of the `rc_smoothing_info` cli command to match the revised logic.
Renamed variables to be more representative of their content.
Simplified the auto-cutoff calculation to be 90% of the nyquist frequency of the measured rx frame rate.
Simplified the PT1 from BIQUAD calculation.
Added active cutoffs to the blackbox log header. Reduce the number of headers by combining like entries into a single line.
* * Put PID variables into the structure
* Precalculate DTerm gyro filter outside the axis loop
* Removed unused variables PIDweight[3], airmodeWasActivated
* If zero throttle or gyro overflow, we can just set values and exit, this saves checks and jumps in axis loop
* Compute PIDSUM after the axis loop, this saves branching inside the loop because of Yaw has no D term
* * Incorporated review changes from DieHertz and fujin
* * Incorporated another review requests from DieHertz
- PidSum renamed to Sum
- pidData[3] redone to pidData[XYZ_AXIS_COUNT]
Added a `USE_32K_CAPABLE_GYRO` define set in `common_fc_post.h` that replaces the repeated testing for each gyro define wherever 32KHz compatibility is checked.
The old gyro_lpf setting was based on the DLPF_CFG values for the MPU6050 gyro and the enumeration was inaccurate and misleading. For example, the default "OFF" setting did not disable the DLPF, but actually set it to around 250hz. The actual cutoff frequency for each setting varies by gyro hardware so the literal frequencies in the enumeration were also incorrect.
Removed gyro_lpf and replaced it with gyro_hardware_lpf (8KHz) and gyro_32khz_hardware_lpf (32KHz). The parameters were renamed to indicate that they are hardware filtering options to differentiate from the many software lowpass filtering options.
gyro_hardware_lpf - This parameter sets the filtering and sample rate options for 8KHz gyros (or 32KHz capable gyros running in 8KHz mode).
- NORMAL - default setting that is equivalent to the previous "OFF" setting. Configures 8KHz sampling with ~250Hz filter cutoff.
- EXPERIMENTAL - 8KHz sampling with a higher frequency filter cutoff (around 3000hz). Considerably more noisy and requires additional software filtering. Note that for the MPU6000 Invensense doesn't officially document the filter cutoff frequency for this selection and simply lists it as "reserved". In testing it's clear that a higher frequency filter cutoff is being selected due to the increased noise, but the actual cutoff frequency is unknown.
- 1KHZ_SAMPLING - 1KHz sample rate with and approximate 188Hz filter cutoff.
Note that the following additional 1KHz sample rate options with lower filter cutoffs have been eliminated - "98HZ", "42HZ", "20HZ", "10HZ", "5HZ". It seems unlikely that these are still needed are probably no longer viable and flight performance would be very poor.
gyro_32khz_hardware_lpf - This parameter sets the filtering options while running in 32KHz mode on capable gyros. It also exposes a new high frequency filter cutoff mode.
- NORMAL - The default and matches the current settings used for 32KHz mode. Provides a filter cutoff around 3000Hz.
- EXPERIMENTAL - Selects a filter cutoff around 8000Hz. This is a very noisy setting and will require substantial software filtering.
The default values for both 8KHz and 32KHz sample rates were chosen to match the previous defaults and users should not experience any performance differences.
Normalized the gyro initialization. Previously there was little consistency on how the initialization was performed and the settings interpreted. For example, MPU9250 used a completely different logic tree when configuring the registers.
Disconnected the literal parameter value from the gyro initialization. The gyro_lpf parameter contained a number from 0-7 that was literally applied to the configuration register during the gyro initialization. This caused some older gyro initializations to be incorrect as they used a different register layout (MPU3050 and L3G4200D). By transitioning to a logical selection the actual value applied to the hardware register is abstracted. This will better future-proof the design as new gyros may have a different register structure that may be incompatible with the old method.
Added a gyroregisters command to the CLI that is used to read the current register settings from the gyro and dump them to the CLI. This is used to verify the configuration in comparison to the datasheets for the various gyros. Testing empirically by looking at the relative noise from the gyros can give a rough estimate whether the different options are selecting correctly, but it's not very precise. The code for the gyroregisters CLI command is wrapped inside #ifdef USE_GYRO_REGISTER_DUMP blocks to allow easy disabling. It's currently enabled for all targets but we may decide to disable before release or only limit to targets with more available space (>=F4).
* Dual-stage Gyro Filtering: PT1, FKF, and Biquad RC+FIR2
* Builds on the previous work of apocolipse.
* Fixes 'stage2'/'stage1' mis-naming to reflect where it is applied in the loop.
That is, the older Biquad, PT1, Denoise (FIR) filters are 'stage2' - applied
after dynamic and static notches (if enabled), and the controversial PT1,
'fast Kalman' filter, and Biquad RC+FIR2 filters, are 'stage1'. e.g. before
dynamic notch.
* FKF bruteforce Kalman gain removed. Calculate from half of PT1 RC constant,
automatically taking loop-time into account.
* New union type definition for stage1 filtering.
* New gyro sensor members for stage1 filter application function and states for
all three supported filter types
* New enum types for stage1 v. stage2. dterm lowpass type references 'stage2'.
* updates to CMS/MSP/FC to allow compilation (untested, probably breaks
MSP, Lua, and ~comms with BFC~).
* Refactors FKF initialization, update and associated structures to be faster by
not continuously calculating 'k'. Filter gain is calculated once during
initialization from RC constant as per PT1 and Biquad RC+FIR2. It was
discovered this converges to static value within 100 samples at 32kHz, so can
be removed. Remove related interface (CLI) settings.
* update dterm_lowpass_type to use new 'TABLE_LOWPASS2_TYPE' (biquad/pt1/FIR)
* Stage 1 defaults to PT1, 763Hz (equivalent to Q400 / R88 from quasi-kalman
filter) - suitable for 32kHz sampling modes. Can be switched to Biquad
RC+FIR2, and FKF.
* Update `#if defined(USE_GYRO_SLEW_LIMITER) to `#ifdef`.
* Includes optional Lagged Moving Average 'smoothing' pipeline step, applied (in
code) after the output of stage1.
* (diehertz): Removed redundant pointers from gyro filtering
* blackbox: fix indentation
* cms IMU menu: fix indentation
* filters: remove USE_GYRO_FIR_FILTER_DENOISE in filter type enum
* gyro sensors: go back to `if defined()` form. for slew limiter
* gyro sensors: increment parameter group version
* due to non-appending changes, the version must be bumped.
* Second PT1 on DTerm
This PR replaces the default biquad filter with a second PT1 set to
200Hz.
Basically allows the user to enable a second, set point configurable,
PT1 type first order low-pass filter on DTerm.
This is useful because most noise in most logs arises from D, not P.
The default is set to on, at twice the normal Dterm setpoint. This
provides greater Dterm cut than a single PT1, and twice the steepness
of cut above the second setpoint. Modelling shows significant
reductions in higher frequency Dterm noise with only minor additional
delay.
The improvement in noise performance will be less than for biquad, but
the delay is considerably less.
If with the default settings the overall noise improves a lot, it may
be possible bring D both filtering set points to higher numbers (e.g.
140/280), or alternatively remove other filters such as the notch
filters, while maintaining an adequate level of control over noise.
* Update names, old defaults, fix whitespace
Defaults restored to biquad with second PT1 off. ‘lpf’ retained as
abbreviation for values, otherwise generally remove ‘Filter’ where
redundant, replace ‘FilterLpf’ with ‘Lowpass’, etc, thanks Fujin and
DieHertz
* Remove underscore in lowpass_2, add hz to setpoint for lowpass
Thanks DieHertz
* completed replacing lpf with lowpass, added _hz to all lowpass set points in profile
Thanks DieHertz
* fix whitespace
fixed whitespace in settings.c
* whitespace attempt #57
* change lpf to lowpass where appropriate elsewhere
Note did not change OSD abbreviations, they are still LPF, and did not
change gyro_lpf anywhere.
* second attempt at a simple PT1 implementation
Basically copied from the DtermNotch implementation
* Second PT1 on DTerm
This PR replaces the default biquad filter with a second PT1 set to
200Hz.
Basically allows the user to enable a second, set point configurable,
PT1 type first order low-pass filter on DTerm.
This is useful because most noise in most logs arises from D, not P.
The default is set to on, at twice the normal Dterm setpoint. This
provides greater Dterm cut than a single PT1, and twice the steepness
of cut above the second setpoint. Modelling shows significant
reductions in higher frequency Dterm noise with only minor additional
delay.
The improvement in noise performance will be less than for biquad, but
the delay is considerably less.
If with the default settings the overall noise improves a lot, it may
be possible bring D both filtering set points to higher numbers (e.g.
140/280), or alternatively remove other filters such as the notch
filters, while maintaining an adequate level of control over noise.
* Rebase
* Remove underscore in lowpass_2, add hz to setpoint for lowpass
Thanks DieHertz
* completed replacing lpf with lowpass, added _hz to all lowpass set points in profile
Thanks DieHertz
* fix whitespace
fixed whitespace in settings.c
* whitespace attempt #57
* change lpf to lowpass where appropriate elsewhere
Note did not change OSD abbreviations, they are still LPF, and did not
change gyro_lpf anywhere.
* second attempt at a simple PT1 implementation
Basically copied from the DtermNotch implementation
* Whitespace fix - thanks, Ledvinap
* Fix PG issue
by moving added dterm_lowpass2_hz to bottom of struct
* Got rid of redundant indirection
* Fixed indentantion shifts
