Added low pass filter to compass to improve calibration and performance.
Added capabilities for config program
Fixed motor output in config program