CAN now tested with FC
Fix startup bug
Formatting