This code cuts D by a specified percentage durning normal flight.
It lets D smoothly rise up to normal during rapid gyro moves like flips and rolls, and increase during prop wash events.
D should now be tuned to values the 'normal' 30-45 range.
If D is 40, a dterm_cut_percentage of 65 will cut D to 14 in normal flight, but the quad will still get full 40 of D to control bounce-back after flips and about 25 of D during strong prop wash.
The dterm_cut_percentage can be adjusted via the OSD, from the D filtering page.
Adding d_cut results in cooler motors, lower amounts of noise in motor traces and faster reactions to quick stick inputs.
Too high a dterm_cut_percentage may bring out P oscillation from lack of D. Values of 70% are generally OK.
Input is gyro differential (delta). Frequencies above 40hz (above propwash) are attenuated with a configurable (dterm_cut_range_hz) biquad filter. Lower values for range can be used if the quad is very noisy or gets low frequency D resonant oscillation. Up to 50 or 60hz may suit clean quads where prop wash control is the main priority. Too high a range value results in D being boosted from noise in normal flight.
The boost signal is 'integrated, smoothed and delayed' with a 7hz PT1 'dterm_cut_lowpass_hz' filter. The default of 7Hz gives about the right amount of smoothing and delay. Higher numbers cause the boost to come on faster, with less delay. Lower values delay the boost effect and cause it to last longer.
The dterm_cut_gain amount controls the strength of the boost effect by amplifying the input to the boosting effect. If the quad is flow gently, a higher gain value may be needed to gain full boost.
Logging with set debug_mode = D_CUT allows recording of realtime D values on roll and pitch into debug 2 and 3. The reatime D value should reach its set maximum during rapid turns, ideally at about the time D itself peaks. If it fails to reach the maximum, gain should be increased.
The D_Cut feature is not enabled on LUXV2RACE, OMNIBUS, SPRACINGF3NEO because there isn't enough flash space.
Adds a race start assistance system that allows the pilot to pitch forward and then release the sticks with the quad holding position for the race start.
Refactor iterm_relax to iterm_relax_axis
Isolate pidLevel test
Isolate pidHorizon tests
remove if UNIT_TEST
revert to iterm_relax from iterm_relax_axis
Refactor PID code Iterm Relax and Absolute control
Move defines for unittest to Makefile
Fix unittest isAirmodeActivated()
Absolute control unittests + optimizations
Fix dead code absolute control // unittests
Further optimizations to absolute control switch logic
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.
Adds options to select the filter type for both input and derivative.
rc_smoothing_input_type = PT1 | BIQUAD (default is BIQUAD)
rc_smoothing_derivative_type = OFF | PT1 | BIQUAD (default is OFF)
Adds an additional rc channel smoothing algorithm that can be used in place of the default rc interpolation. Utilizing a filter-based approach the smoothing has lower latency and is immune to loop time jitter that can introduce artifacts. Additionally a smoothing filter is added to the setpoint derivative used to produce D-term setpoint weight resulting in a smoother effect on D.
The default setting is to use the previous interpolation logic and there are no changes unless the optional method is selected.
Configuration:
rc_smoothing_type: (INTERPOLATION | FILTER) - defaults to INTERPOLATION
rc_smoothing_input_hz: (0-255) - sets the rc channel input filter cutoff in Hz. Default value of 0 will enable auto calculation based on received RX frame rate.
rc_smoothing_derivative_hz: (0-255) - sets the setpoint weight derivative filter cutoff in Hz. Default value of 0 will enable auto calculation based on received RX frame rate.
rc_smoothing_debug_axis: (ROLL | PITCH | YAW | THROTTLE) - determines which axis is logged in the debug fields
Debug logging:
set debug_mode = RC_SMOOTHING
debug(0) = raw un-smoothed rc channel data
debug(1) = raw un-smoothed setpoint derivative
debug(2) = filtered setpoint derivative before applied to setpoint weight
debug(3) = auto-calculated filter cutoff frequency base after sampling the rx frame rate
Notes:
Currently only enabled for F4/F7 due to flash size limitations
Uses the rc_inter_ch parameter to determine which channels are smoothed (same as default interpolation logic)
The auto filter cutoff calculation will set a cutoff frequency of 30Hz for typical SBUS frames (9ms). 11ms Spektrum will calculate to approximately 25Hz. The user can manually enter the filter cutoffs to be used instead of the auto calculation. The current default calculation was chosen as a good starting point but may be adjusted in the future.
Setting a lower cutoff frequency will result in more smoothing, but also more delay.
There currently isn't any support for receivers that change their rx frame rate dynamically. So for CRSF users wishing to use this alternate smoothing method should change their settings to lock the rx frame rate for now. Support for auto-adjusting to new frame rates in flight will likely be added.
Adds a new angle limiting mode for pilots who are learning to fly in acro mode. Primarily targeted at new LOS acro pilots, but can be used with FPV as well.
The feature is activated with a new mode named "ACRO TRAINER". When the feature is active, the craft will fly in normal acro mode but will limit roll/pitch axes so that they don't exceed the configured angle limit. New pilots can start with a small angle limit and progressively increase as their skills improve.
The accelerometer must be enabled for the feature to be configured and function.
Also the feature will only be active while in acro flight and will disable if ANGLE or HORIZON modes are selected.
For most users all they need to do is simply configure the new mode to be active as desired on the "Modes" tab in the configurator and configure the desired angle limit in the cli.
Configuration parameters:
acro_trainer_angle_limit: (range 10-80) Angle limit in degrees.
acro_trainer_lookahead_ms: (range 10-200) Time in milliseconds that the logic will "look ahead" to help minimize overshoot and bounce-back if the limit is approached at high gyro rates. The default value of 50 should be good for most users. For low powered or unresponsive craft (micros or brushed) it may be helpful to increase this setting if you're seeing substantial overshoot.
acro_trainer_debug_axis: (ROLL, PITCH) The axis that will log information if debugging is active.
To enable debugging:
set debug_mode = ACRO_TRAINER
debug(0) - Current angle
debug(1) - The internal logic state
debug(2) - Modified setpoint
debug(3) - Projected angle based gyro rate and lookahead period
* PID controller unittest
* Clean code for yaw spin recovery
* Yaw spin recovery optimizations
* Flash size optimizations, use 50% throttle when airmode is off, and override pidsum_limit_yaw
Also rebasing from betaflight/master
* * 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]
* ITerm rotation
* address requested changes
* now counting up
* scale errors according to Ki while rotating
* iterm_rotation profile setting
* revert to non scaled version, style related fixes
* Triggering a CI build.
* 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
Removed parameters runaway_takeoff_threshold (hardcode to 60) and runaway_takeoff_activate_delay (hardcode to 75). The previous default values worked well and required no tuning.
Enhance the deactivate logic to remove the R/P/Y stick activity condition once throttle reaches 2X runaway_takeoff_deactivate_throttle_percent. Additionally reduce the runaway_takeoff_deactivate_delay by 50% when throttle exceeds 75%.
