Improved code

-> support the new PID control
-> improved warnings
-> misc minor improvements