Fixed and calibrated input capture and output compare.
GPS confirmed to be working.
Datalink is causing opcode and TRAP resets. This needs to be fixed.