I think that C++ is the answer. The today MCU are powerful enough, in term of memory, to profitable the Object Oriented approach for firmware development.
I'm working at a set of class that expose the FreeRTOS features. This is the first step to develop more design pattern useful to build embedded application.
Look at my demo page Multitasking meets C++ in the embedded world for more details.
...It is only the beginning! :-)
