Change of init order + time avg

Changed the order of initialization as the device was locking up.
Replaced the time command with time averaging.
Reorg into structs

Move hanging data to structs.
Tested with 1.5.8 Beta IDE

Tested with 1.5.8 Beta IDE and updated code