uORB2: many fixes and optimizations