Climb speed fixes

ALT_HOLD was limited to the default climb/descend speeds of +2.5 and -1.5 m/s, regardless of PILOT_VELZ_MAX setting. These patches add a proper initialization of the z leash for ALT_HOLD and fix a bug in vertical leash length calculation. Flight tests were successful.