Change the logic to not modify rcCommand directly and instead apply the additional throttle directly in the mixer.
Also move the logic to the attitude task instead of having it calculate in the PID loop. The logic relies on an angle that's only updated in the attitude task so there was no point in running the calculation every PID loop.
Injecting new values directly into rcCommand to override pilot inputs does not work correctly when rc interpolation or smoothing is enabled. This is because the smoothing functions maintain an internal state that is used to produce the final rcCommand values. So in effect it re-overrides the values set by GPS Rescue.
Additionally, modifying rcCommand directly is undesirable because:
- It happens before rates are applied. So the pilot's rates will effect the control commanded by GPS Rescue.
- rcCommand values are used for more than input in the flight control. They also are used for stick commands, etc.
- In the case of throttle, various modifications can be additionally applied like throttle boost and throttle limit that may negatively effect GPS Rescue.
These changes revise the logic to only modify the commanded values used in the PID controller (yaw) and mixer (throttle) rather than attempting to override rcCommand.
Changed main storage structure to use union for the filters.
Renamed storage variable passed to the sub functions to avoid confusion with the global static.
Added whitespace to separate logical blocks and added additional comments to make the code more readable.
Restructured derivative filter initialization/update logic.
Cleans up the many static variables into a single structure. Saves ~60 bytes on F3 but far more importantly will allow the support functions to be easily moved to a size optimized file at some later date.
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.
