Added initial code for Mendieta's firmware. It's a real mess, but it works...