now compiles and works, tested with my quad