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 d38a03de373f33def020ead063c359f80f68b560..7c6a82047b75b20e2d36f43aa20ad89345fef818 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 e2e9d5f7a391595706d0dbcb28049a9904764708..99822e764ae667cc1670d3b2e021e58fb7dc1c4a 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