У меня стоит похожая задача, решения пока не нашел. Вообще FreeRtos
очень расстроила, для следующих проектов буду искать что то другое.
Не хватает сервиса: очереди, потоки, сообщения в том числе
широковещательные с возможностью подписки. То что Queues и Message Buffers во FreeRtos все действия выполняют при запрещенных прерываниях - просто убило наповал. Давно изобретены lock-free буферы причем в 146 тысячах реализаций.