Properly include 'config.h' in all firmware files
Added assertions
Reorganized includes