Rework into c++
Rework to use the new library class; remove dependency on arm_math.h