Replace Mahoney and Magiwick code with include of Fusion library

[Eiren Rain]

https://github.com/xioTechnologies/Fusion

We should include (a for of) this library into firmware code instead of copying Magiwick/Mahoney fusion code. This will improve license compatibility and overal code support.