mc_att_control: run rate controller first and increase fmu prio by one

The rate controller is now run directly after a gyro publication, and
as soon as it publishes the actuator controls, the output driver (fmu/...)
runs.

Test on a Pixracer:
Reduces fmu control latency from 219us to 134us.
If we run the rate controller last (same order as before, just increase
the prio), the latency is 201us.

CPU load is unchanged.

The drawback is that the attitude to rate setpoint generation is delayed
by one cycle (4ms), but it will be reduced to 1ms as soon as we run at
1kHz.

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -720,6 +720,14 @@ MulticopterAttitudeControl::run()
 			/* copy gyro data */
 			orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
 
+			/* run the rate controller immediately after a gyro update */
+			if (_v_control_mode.flag_control_rates_enabled) {
+				control_attitude_rates(dt);
+
+				publish_actuator_controls();
+				publish_rate_controller_status();
+			}
+
 			/* check for updates in other topics */
 			vehicle_control_mode_poll();
 			vehicle_status_poll();
@@ -772,13 +780,6 @@ MulticopterAttitudeControl::run()
 				}
 			}
 
-			if (_v_control_mode.flag_control_rates_enabled) {
-				control_attitude_rates(dt);
-
-				publish_actuator_controls();
-				publish_rate_controller_status();
-			}
-
 			if (_v_control_mode.flag_control_termination_enabled) {
 				if (!_vehicle_status.is_vtol) {
 					_rates_sp.zero();

src/platforms/px4_tasks.h

@@ -114,9 +114,9 @@ typedef struct {
 // which typically runs at a slower rate
 #define SCHED_PRIORITY_ATTITUDE_CONTROL		(SCHED_PRIORITY_MAX - 4)
 
-// Actuator outputs should run before right after the attitude controller
-// updated
-#define SCHED_PRIORITY_ACTUATOR_OUTPUTS		(SCHED_PRIORITY_MAX - 4)
+// Actuator outputs should run as soon as the rate controller publishes
+// the actuator controls topic
+#define SCHED_PRIORITY_ACTUATOR_OUTPUTS		(SCHED_PRIORITY_MAX - 3)
 
 // Position controllers typically are in a blocking wait on estimator data
 // so when new sensor data is available they will run last. Keeping them