Fix pitch and roll angle and add heading
Fix pitch and roll angle and add heading

zegervdv/STM-Quadcopter,zegervdv/STM-Quadcopter,zegervdv/Quadcopter,zegervdv/Quadcopter,zegervdv/STM-Quadcopter,zegervdv/Quadcopter,zegervdv/STM-Quadcopter,zegervdv/Quadcopter