Feedback to AutoPilot

Need to fix foward kinematics model and perhaps conversion between AX12
value and radians. Fuckin, wtf.