Kp for side stabilisation changed
speed changes for heading in auto_pilot()