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 98a21e77bc..048a036f6c 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 @@ -66,9 +66,20 @@ static constexpr wq_config_t I2C4{"wq:I2C4", 1400, -12}; static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors +static constexpr wq_config_t hp_default{"wq:hp_default", 1800, -12}; + static constexpr wq_config_t uavcan{"uavcan", 2400, -13}; -static constexpr wq_config_t hp_default{"wq:hp_default", 1800, -12}; +static constexpr wq_config_t UART0{"wq:UART0", 1400, -14}; +static constexpr wq_config_t UART1{"wq:UART1", 1400, -15}; +static constexpr wq_config_t UART2{"wq:UART2", 1400, -16}; +static constexpr wq_config_t UART3{"wq:UART3", 1400, -17}; +static constexpr wq_config_t UART4{"wq:UART4", 1400, -18}; +static constexpr wq_config_t UART5{"wq:UART5", 1400, -19}; +static constexpr wq_config_t UART6{"wq:UART6", 1400, -20}; +static constexpr wq_config_t UART7{"wq:UART7", 1400, -21}; +static constexpr wq_config_t UART8{"wq:UART8", 1400, -22}; + static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; static constexpr wq_config_t test1{"wq:test1", 800, 0}; @@ -107,5 +118,13 @@ WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq); */ const wq_config_t &device_bus_to_wq(uint32_t device_id); +/** + * Map a serial device path (eg /dev/ttyS1) to a work queue. + * + * @param device_id The device path. + * @return A work queue configuration. + */ +const wq_config_t &serial_port_to_wq(const char *serial); + } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 5f7c733aea..f0fb3a093c 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -162,6 +162,45 @@ device_bus_to_wq(uint32_t device_id_int) return wq_configurations::hp_default; }; +const wq_config_t & +serial_port_to_wq(const char *serial) +{ + if (serial == nullptr) { + return wq_configurations::hp_default; + + } else if (strstr(serial, "ttyS0")) { + return wq_configurations::UART0; + + } else if (strstr(serial, "ttyS1")) { + return wq_configurations::UART1; + + } else if (strstr(serial, "ttyS2")) { + return wq_configurations::UART2; + + } else if (strstr(serial, "ttyS3")) { + return wq_configurations::UART3; + + } else if (strstr(serial, "ttyS4")) { + return wq_configurations::UART4; + + } else if (strstr(serial, "ttyS5")) { + return wq_configurations::UART5; + + } else if (strstr(serial, "ttyS6")) { + return wq_configurations::UART6; + + } else if (strstr(serial, "ttyS7")) { + return wq_configurations::UART7; + + } else if (strstr(serial, "ttyS8")) { + return wq_configurations::UART8; + } + + PX4_ERR("unknown serial port: %s", serial); + + return wq_configurations::hp_default; +} + static void * WorkQueueRunner(void *context) {