Work around to better handle RX protocols that have frame intervals >= 20ms. Otherwise the task would run at the end of the task interval causing slower protocols to run again after the remaining delay. This interferes with rc interpolation and also the frame rate detection in rc smoothing.
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.
