PID and 3D orientation on PIC32