This is best compared against the original code here https://github.com/TauLabs/TauLabs/blob/master/flight/Modules/Stabilization/stabilization.c#L212