@DrTon I can't quite follow the changes here just from the code - could you please describe the intent of these changes and what the new vs. old behavior is? I guess I will have to try this in HIL, right? Did you already test it thoroughly in HIL for multicopters?