Adjust test code to new hardware
Test firmware can test all parts
Better comments
Add readme to test firmware