Firmware: Fixed many bugs and added message transmission support.
The addition of a test framework helped find and fix these bugs, which is now also committed.