Se vc estiver aberto a sugestões, eu dei uma olhada no código e te recomendaria algumas coisas.

Cria uma HAL porquê provavelmente no futuro você vai ter alterações no hardware, usará outros dispositivos e caso isso aconteça você vai perder muito tempo reescrevendo código.

Como eu faria isso:
1 - criaria abstrações pros dispositivos, como gps (get position), e2prom (write data, read data), usb (setup, read, write), por ai vai.

2 - implementaria essas abstrações com SFINAE se vc tiver usando até C++17 ou concepts caso C++20. Lembrando que essas duas técnicas usam polimorfismo estático e não dinamico, ou seja, não oneram performance ou memória.

3 - faria inversão de dependências pra injetar o módulo de hardware/dispositivo que você quer usar no main.cpp