Improved a lot of the math function
Now more correct and robust
Also power outputs are now in Q1.30 for safety