Fix a bug in callback_thread

Signed-off-by: Jukka Laitinen <[email protected]>

platforms/common/uORB/SubscriptionCallback.hpp

@@ -128,14 +128,16 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNode<Subscr
 		return DeviceNode::get_max_writer_priority(_subscription.get_node());
 	}
 
-	void do_call()
+	bool do_call()
 	{
 		bool dequeued = uORB::Manager::dequeueCallback(_subscription.get_node(), _cb_handle);
 
 		if (dequeued) {
 
 			call();
 		}
+
+		return dequeued;
 	}
 #endif
 

platforms/common/uORB/uORBManager.cpp

@@ -530,14 +530,24 @@ int
 uORB::Manager::callback_thread(int argc, char *argv[])
 {
 	while (true) {
+		/* Sleep here waiting for callbacks */
 		lockThread(per_process_lock);
 
+		/* Each publish has unlocked once, and each do_call() will
+		 * lock it back. Free the lock once here, so that after the
+		 * last call we won't lock in the last do_call()
+		*/
+		unlockThread(per_process_lock);
+
 		lock_cb_list();
 
 		for (auto sub : per_process_cb_list) {
 			adjust_callback_thread_priority(sub->priority());
-			sub->do_call();
 
+			/* Just in cast the callback thread has been starved,
+			 * run all the queued callbacks now
+			 */
+			while (sub->do_call()) {}
 		}
 
 		unlock_cb_list();