Refactored UAVCAN integration, added test