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

ROMFS/px4fmu_common/init.d/rcS

@@ -215,11 +215,6 @@ else
 	#
 	tone_alarm start
 
-	if param compare SYS_FMU_TASK 1
-	then
-		set FMU_ARGS "-t"
-	fi
-
 	#
 	# Set parameters and env variables for selected AUTOSTART.
 	#

Tools/uorb_graph/create.py

@@ -229,7 +229,6 @@ class Graph(object):
     ('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
     ('pwm_out_sim', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
     ('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
-    ('fmu', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
     ('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
     ]
         special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))

boards/auav/x21/init/rc.board_defaults

@@ -6,5 +6,5 @@
 
 if [ $AUTOCNF = yes ]
 then
-	param set SYS_FMU_TASK 0
+
 fi

boards/px4/fmu-v2/init/rc.board_defaults

@@ -24,5 +24,5 @@ unset BL_FILE
 
 if [ $AUTOCNF = yes ]
 then
-	param set SYS_FMU_TASK 0
+
 fi

boards/px4/fmu-v3/init/rc.board_defaults

@@ -6,5 +6,5 @@
 
 if [ $AUTOCNF = yes ]
 then
-	param set SYS_FMU_TASK 0
+
 fi

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();