Refactor code structure

[jkaflik]

All logic lives in a single source file. It should be reorganised per responsibility and eventually run on both cores:

• handling IO
  • IMU driver
• handling Micro ROS communication

for a memory allocation savings, all should operate on ROS msgs structs