Add proper reset of reference values when changing control mode, fixes #129