I had added this to the work list for RepRapFirmware 3.