С++ в объеме "Cи с классами" отлично ложится на железо. Есть какой-то датчик/девайс - выносишь работу с ним в отдельный класс.
Есть функции инициализации, настройки параметров, опроса...
А наследование и полиморфизм хорошо работает, когда нужно
работать с большим количеством разных датчиков и исполнительных устройств.
Тогда main loop сводится "взять из таблицы очередной экземпляр класса датчика, вызвать ->doStep. Повторить то же для управляющих автоматов".
Причем все это не требует накладных расходов _вообще_. На plain C код получится такой же, но его нужно будет выписывать вручную.