WPNav: reduce max acceleration to 2.5m/s

Also includes bug fix to ramp up of speed of intermediate point