Fix M666 Z endstop not working issue