We've added some features to the Marlin Reprap firmware, such as writing Z axis position in Gcode file.