px4fmu: move to WQ with uORB callback scheduling (#12224)

src/drivers/px4fmu/px4fmu_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -50,19 +50,3 @@
  * @group PWM Outputs
  */
 PARAM_DEFINE_INT32(MOT_ORDERING, 0);
-
-/**
- * Run the FMU as a task to reduce latency
- *
- * If true, the FMU will run in a separate task instead of on the work queue.
- * Set this if low latency is required, for example for racing.
- *
- * This is a trade-off between RAM usage and latency: running as a task, it
- * requires a separate stack and directly polls on the control topics, whereas
- * running on the work queue, it runs at a fixed update rate.
- *
- * @boolean
- * @reboot_required true
- * @group System
- */
-PARAM_DEFINE_INT32(SYS_FMU_TASK, 1);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -242,7 +242,7 @@ VehicleAngularVelocity::Run()
 			rates -= _bias;
 
 			vehicle_angular_velocity_s angular_velocity;
-			angular_velocity.timestamp_sample = sensor_data.timestamp;
+			angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
 			rates.copyTo(angular_velocity.xyz);
 			angular_velocity.timestamp = hrt_absolute_time();
 

src/modules/uORB/SubscriptionCallback.hpp

@@ -124,7 +124,7 @@ public:
 	void call() override
 	{
 		// schedule immediately if no interval, otherwise check time elapsed
-		if ((_interval_us == 0) || (hrt_elapsed_time(&_last_update) >= _interval_us)) {
+		if ((_interval_us == 0) || (hrt_elapsed_time_atomic(&_last_update) >= _interval_us)) {
 			_work_item->ScheduleNow();
 		}
 	}

src/platforms/common/px4_work_queue/WorkItem.hpp

@@ -59,6 +59,14 @@ public:
 
 protected:
 
+	/**
+	 * Initialize WorkItem given a WorkQueue config. This call
+	 * can also be used to switch to a different WorkQueue.
+	 * NOTE: Caller is responsible for synchronization.
+	 *
+	 * @param config The WorkQueue configuration (see WorkQueueManager.hpp).
+	 * @return true if initialization was successful
+	 */
 	bool Init(const wq_config_t &config);
 	void Deinit();