From 8ddb611ea2fef7e8bc6bcf2d11e2d27db5ce538a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 23 Apr 2020 12:00:01 -0400 Subject: [PATCH] WIP: move mc_att_control to new wq:attitude_ctrl --- .../px4_work_queue/WorkQueueManager.hpp | 33 ++++++++++--------- .../include/px4_platform_common/tasks.h | 2 +- .../airspeed_selector_main.cpp | 2 +- .../attitude_estimator_q_main.cpp | 2 +- src/modules/ekf2/ekf2_main.cpp | 2 +- .../FixedwingAttitudeControl.cpp | 2 +- .../FixedwingPositionControl.cpp | 2 +- src/modules/land_detector/LandDetector.cpp | 2 +- .../BlockLocalPositionEstimator.cpp | 2 +- .../mc_att_control/mc_att_control_main.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/sensors/sensors.cpp | 2 +- .../VehicleAcceleration.cpp | 2 +- .../vehicle_air_data/VehicleAirData.cpp | 2 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 2 +- 15 files changed, 31 insertions(+), 30 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 98b3dd4deb48..bc00c972750f 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -65,22 +65,23 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11}; static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12}; // PX4 att/pos controllers, highest priority after sensors. -static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7200, -13}; - -static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -14}; - -static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -15}; - -static constexpr wq_config_t UART0{"wq:UART0", 1400, -16}; -static constexpr wq_config_t UART1{"wq:UART1", 1400, -17}; -static constexpr wq_config_t UART2{"wq:UART2", 1400, -18}; -static constexpr wq_config_t UART3{"wq:UART3", 1400, -19}; -static constexpr wq_config_t UART4{"wq:UART4", 1400, -20}; -static constexpr wq_config_t UART5{"wq:UART5", 1400, -21}; -static constexpr wq_config_t UART6{"wq:UART6", 1400, -22}; -static constexpr wq_config_t UART7{"wq:UART7", 1400, -23}; -static constexpr wq_config_t UART8{"wq:UART8", 1400, -24}; -static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -25}; +static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1500, -13}; +static constexpr wq_config_t navigation_and_position_ctrl{"wq:INS", 7200, -14}; + +static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -15}; + +static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -16}; + +static constexpr wq_config_t UART0{"wq:UART0", 1400, -17}; +static constexpr wq_config_t UART1{"wq:UART1", 1400, -18}; +static constexpr wq_config_t UART2{"wq:UART2", 1400, -19}; +static constexpr wq_config_t UART3{"wq:UART3", 1400, -20}; +static constexpr wq_config_t UART4{"wq:UART4", 1400, -21}; +static constexpr wq_config_t UART5{"wq:UART5", 1400, -22}; +static constexpr wq_config_t UART6{"wq:UART6", 1400, -23}; +static constexpr wq_config_t UART7{"wq:UART7", 1400, -24}; +static constexpr wq_config_t UART8{"wq:UART8", 1400, -25}; +static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26}; static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; diff --git a/platforms/common/include/px4_platform_common/tasks.h b/platforms/common/include/px4_platform_common/tasks.h index 4436a091e863..70d5c4c6cfc8 100644 --- a/platforms/common/include/px4_platform_common/tasks.h +++ b/platforms/common/include/px4_platform_common/tasks.h @@ -101,7 +101,7 @@ typedef struct { #endif // PX4 work queue starting high priority -#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 12) +#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 15) // Fast drivers - they need to run as quickly as possible to minimize control // latency. diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index e6a38224981e..c3cb55c49f08 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -176,7 +176,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, AirspeedModule::AirspeedModule(): ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl) { // initialise parameters update_params(); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index aea02511fff0..901bfecce3c4 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -166,7 +166,7 @@ class AttitudeEstimatorQ : public ModuleBase, public ModuleP AttitudeEstimatorQ::AttitudeEstimatorQ() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl) { _vel_prev.zero(); _pos_acc.zero(); diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 83c073c5ebae..bf8702dc7860 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -542,7 +542,7 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Sch Ekf2::Ekf2(bool replay_mode): ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _replay_mode(replay_mode), _ekf_update_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": update")), _params(_ekf.getParamHandle()), diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 38846902c724..67991353db67 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -42,7 +42,7 @@ using math::radians; FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 197e3561cb43..90006d992959 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -37,7 +37,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _launchDetector(this), diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 662ec226d3c0..58d75525a625 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -47,7 +47,7 @@ namespace land_detector LandDetector::LandDetector() : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl) {} LandDetector::~LandDetector() diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index b16d35f12609..757f92a1d82a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -19,7 +19,7 @@ static const char *msg_label = "[lpe] "; // rate of land detector correction BlockLocalPositionEstimator::BlockLocalPositionEstimator() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), // this block has no parent, and has name LPE SuperBlock(nullptr, "LPE"), 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 aeeaeabd3030..591cdef1ef05 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,7 +53,7 @@ using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::attitude_ctrl), _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 50de09701931..03e30da5bc23 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -284,7 +284,7 @@ class MulticopterPositionControl : public ModuleBase MulticopterPositionControl::MulticopterPositionControl(bool vtol) : SuperBlock(nullptr, "MPC"), ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 96fee8ff1f89..7093b3885c37 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -199,7 +199,7 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch Sensors::Sensors(bool hil_enabled) : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _hil_enabled(hil_enabled), _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), _voted_sensors_update(_parameters, hil_enabled) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index f14e53792b78..1a101416467e 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -43,7 +43,7 @@ namespace sensors VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _corrections(this, SensorCorrections::SensorType::Accelerometer) { _lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get()); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index bc32a92545b1..e0a61a6f081a 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -43,7 +43,7 @@ static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl) { _voter.set_timeout(SENSOR_TIMEOUT); } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 174e71b18628..1bfe8fc89526 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -43,7 +43,7 @@ namespace sensors VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_position_ctrl), _sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index), _sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index), _accel_corrections(this, SensorCorrections::SensorType::Accelerometer),