PID functions added & sensor tranforms in place