From bec43b0b28e6817fb23e0ef323ed929ce65360ed Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 25 Oct 2018 07:56:55 +0200
Subject: [PATCH] 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.
---
.../mc_att_control/mc_att_control_main.cpp | 15 ++++++++-------
src/platforms/px4_tasks.h | 6 +++---
2 files changed, 11 insertions(+), 10 deletions(-)
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index d38a03de37..7c6a82047b 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/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();
diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h
index e2e9d5f7a3..99822e764a 100644
--- a/src/platforms/px4_tasks.h
+++ b/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
--
GitLab