Nuttx merge 5591

This is running fine on bench tests. Fixed a couple of smaller compile errors due to a change in the low-level NuttX print function. Good to merge to master after a few in-vehicle tests.