diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg index 3718e457cb..26de92e833 100644 --- a/msg/gps_dump.msg +++ b/msg/gps_dump.msg @@ -6,3 +6,5 @@ uint64 timestamp # time since system start (microseconds) uint8 len # length of data, MSB bit set = message to the gps device, # clear = message from the device uint8[79] data # data to write to the log + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg index 00b1aa1b8a..5daae0623e 100644 --- a/msg/gps_inject_data.msg +++ b/msg/gps_inject_data.msg @@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds) uint8 len # length of data uint8 flags # LSB: 1=fragmented uint8[182] data # data to write to GPS device (RTCM message) + +uint8 ORB_QUEUE_LENGTH = 6 diff --git a/msg/led_control.msg b/msg/led_control.msg index 582c9dfb73..d602158e5d 100644 --- a/msg/led_control.msg +++ b/msg/led_control.msg @@ -33,3 +33,5 @@ uint8 mode # see MODE_* uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) + +uint8 ORB_QUEUE_LENGTH = 4 # needs to match BOARD_MAX_LEDS diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg index e7387b08dd..6a8ab72bc6 100644 --- a/msg/mavlink_log.msg +++ b/msg/mavlink_log.msg @@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds) char[50] text uint8 severity # log level (same as in the linux kernel, starting with 0) + +uint8 ORB_QUEUE_LENGTH = 5 diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index 37eaca5e94..389d5d6183 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -34,4 +34,4 @@ bool enabled bool ok uint64 subsystem_type -uint32 ORB_QUEUE_LENGTH = 5 +uint8 ORB_QUEUE_LENGTH = 5 diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 9d897c6444..3f6f3248c8 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -55,3 +55,5 @@ float32 rate_rx float32 rate_tx float32 rate_txerr + +uint8 ORB_QUEUE_LENGTH = 3 diff --git a/msg/test_motor.msg b/msg/test_motor.msg index eaad0d74e7..55bfd3f8f2 100644 --- a/msg/test_motor.msg +++ b/msg/test_motor.msg @@ -3,3 +3,5 @@ uint8 NUM_MOTOR_OUTPUTS = 8 uint32 motor_number # number of motor to spin float32 value # output power, range [0..1] + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index 603f034fa4..99d2a332b3 100644 --- a/msg/transponder_report.msg +++ b/msg/transponder_report.msg @@ -22,4 +22,4 @@ uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16 uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32 uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256 -uint32 ORB_QUEUE_LENGTH = 10 +uint8 ORB_QUEUE_LENGTH = 10 diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg index e6e480ebd4..cb188ce297 100644 --- a/msg/uavcan_parameter_request.msg +++ b/msg/uavcan_parameter_request.msg @@ -7,3 +7,5 @@ int16 param_index # -1 if the param_id field should be used as identifier uint8 param_type # MAVLink parameter type int64 int_value # current value if param_type is int-like float32 real_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 3 diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg index 0f7d291774..4dedf1879d 100644 --- a/msg/vehicle_command_ack.msg +++ b/msg/vehicle_command_ack.msg @@ -13,7 +13,7 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 -uint32 ORB_QUEUE_LENGTH = 3 +uint8 ORB_QUEUE_LENGTH = 3 uint16 command uint8 result diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 074673cd68..f731110bff 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -45,25 +45,12 @@ namespace camera_capture { -CameraCapture *g_camera_capture; +CameraCapture *g_camera_capture{nullptr}; } CameraCapture::CameraCapture() : - _capture_enabled(false), - _gpio_capture(false), - _trigger_pub(nullptr), - _command_ack_pub(nullptr), - _command_sub(-1), - _trig_buffer(nullptr), - _camera_capture_mode(0), - _camera_capture_edge(0), - _capture_seq(0), - _last_trig_begin_time(0), - _last_exposure_time(0), - _capture_overflows(0) + ScheduledWorkItem(px4::wq_configurations::lp_default) { - - memset(&_work, 0, sizeof(_work)); memset(&_work_publisher, 0, sizeof(_work_publisher)); // Capture Parameters @@ -75,7 +62,6 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - } CameraCapture::~CameraCapture() @@ -89,10 +75,8 @@ CameraCapture::~CameraCapture() } void -CameraCapture::capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { - _trigger.chan_index = chan_index; _trigger.edge_time = edge_time; _trigger.edge_state = edge_state; @@ -114,7 +98,6 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0); return PX4_OK; - } void @@ -130,7 +113,7 @@ CameraCapture::publish_trigger() { bool publish = false; - struct camera_trigger_s trigger {}; + camera_trigger_s trigger{}; // MODES 1 and 2 are not fully tested if (_camera_capture_mode == 0 || _gpio_capture) { @@ -173,48 +156,23 @@ CameraCapture::publish_trigger() return; } - if (_trigger_pub == nullptr) { - - _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); - - } else { - - orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); - } - + _trigger_pub.publish(trigger); } void -CameraCapture::capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, + uint32_t overflow) { camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); } void -CameraCapture::cycle_trampoline(void *arg) +CameraCapture::Run() { - CameraCapture *dev = reinterpret_cast(arg); - - dev->cycle(); -} - -void -CameraCapture::cycle() -{ - - if (_command_sub < 0) { - _command_sub = orb_subscribe(ORB_ID(vehicle_command)); - } - - bool updated = false; - orb_check(_command_sub, &updated); - // Command handling - if (updated) { + vehicle_command_s cmd{}; - vehicle_command_s cmd; - orb_copy(ORB_ID(vehicle_command), _command_sub, &cmd); + if (_command_sub.update(&cmd)) { // TODO : this should eventuallly be a capture control command if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { @@ -235,31 +193,17 @@ CameraCapture::cycle() } // Acknowledge the command - vehicle_command_ack_s command_ack = { - .timestamp = 0, - .result_param2 = 0, - .command = cmd.command, - .result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, - .from_external = false, - .result_param1 = 0, - .target_system = cmd.source_system, - .target_component = cmd.source_component - }; + vehicle_command_ack_s command_ack{}; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = cmd.command; + command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - - } + _command_ack_pub.publish(command_ack); } - } - - work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture, - USEC2TICK(100000)); // 100ms } void @@ -267,9 +211,7 @@ CameraCapture::set_capture_control(bool enabled) { #if !defined CONFIG_ARCH_BOARD_AV_X_V1 - int fd = -1; - - fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); + int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) { PX4_ERR("open fail"); @@ -343,7 +285,9 @@ err_out: void CameraCapture::reset_statistics(bool reset_seq) { - if (reset_seq) { _capture_seq = 0; } + if (reset_seq) { + _capture_seq = 0; + } _last_trig_begin_time = 0; _last_exposure_time = 0; @@ -361,9 +305,8 @@ CameraCapture::start() return PX4_ERROR; } - // start to monitor at low rates for capture control commands - work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this, - USEC2TICK(1)); + // run every 100 ms (10 Hz) + ScheduleOnInterval(100000, 10000); return PX4_OK; } @@ -371,8 +314,8 @@ CameraCapture::start() void CameraCapture::stop() { + ScheduleClear(); - work_cancel(LPWORK, &_work); work_cancel(HPWORK, &_work_publisher); if (camera_capture::g_camera_capture != nullptr) { diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 95fc1a41c0..c865407116 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -38,22 +38,23 @@ #pragma once -#include -#include - -#include -#include -#include - #include #include -#include #include - +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include #define PX4FMU_DEVICE_PATH "/dev/px4fmu" @@ -61,7 +62,7 @@ #define GPIO_TRIG_AVX /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14) -class CameraCapture +class CameraCapture : public px4::ScheduledWorkItem { public: /** @@ -86,10 +87,10 @@ public: void status(); - void cycle(); + // Low-rate command handling loop + void Run() override; - static void capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, + static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); void set_capture_control(bool enabled); @@ -99,20 +100,16 @@ public: void publish_trigger(); - static struct work_s _work; static struct work_s _work_publisher; private: - bool _capture_enabled; - bool _gpio_capture; - // Publishers - orb_advert_t _trigger_pub; - orb_advert_t _command_ack_pub; + uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _trigger_pub{ORB_ID(camera_trigger)}; // Subscribers - int _command_sub; + uORB::Subscription _command_sub{ORB_ID(vehicle_command)}; // Trigger Buffer struct _trig_s { @@ -120,30 +117,30 @@ private: hrt_abstime edge_time; uint32_t edge_state; uint32_t overflow; - }; + } _trigger{}; - struct _trig_s _trigger; + ringbuffer::RingBuffer *_trig_buffer{nullptr}; - ringbuffer::RingBuffer *_trig_buffer; + bool _capture_enabled{false}; + bool _gpio_capture{false}; // Parameters - param_t _p_strobe_delay; - float _strobe_delay; - param_t _p_camera_capture_mode; - int32_t _camera_capture_mode; - param_t _p_camera_capture_edge; - int32_t _camera_capture_edge; + param_t _p_strobe_delay{PARAM_INVALID}; + float _strobe_delay{0.0f}; + param_t _p_camera_capture_mode{PARAM_INVALID}; + int32_t _camera_capture_mode{0}; + param_t _p_camera_capture_edge{PARAM_INVALID}; + int32_t _camera_capture_edge{0}; // Signal capture statistics - uint32_t _capture_seq; - hrt_abstime _last_trig_begin_time; - hrt_abstime _last_exposure_time; - hrt_abstime _last_trig_time; - uint32_t _capture_overflows; + uint32_t _capture_seq{0}; + hrt_abstime _last_trig_begin_time{0}; + hrt_abstime _last_exposure_time{0}; + hrt_abstime _last_trig_time{0}; + uint32_t _capture_overflows{0}; // Signal capture callback - void capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); // GPIO interrupt routine (for AV_X board) static int gpio_interrupt_routine(int irq, void *context, void *arg); @@ -151,9 +148,6 @@ private: // Signal capture publish static void publish_trigger_trampoline(void *arg); - // Low-rate command handling loop - static void cycle_trampoline(void *arg); - }; -struct work_s CameraCapture::_work; + struct work_s CameraCapture::_work_publisher; diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index a5b1186069..44062d07f0 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -55,6 +55,7 @@ #include #include +#include #include #include #include @@ -174,7 +175,8 @@ private: uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; orb_advert_t _trigger_pub; - orb_advert_t _cmd_ack_pub; + + uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; param_t _p_mode; param_t _p_activation_time; @@ -192,7 +194,7 @@ private: /** * Vehicle command handler */ - void Run(); + void Run() override; /** * Fires trigger @@ -250,7 +252,6 @@ CameraTrigger::CameraTrigger() : _last_shoot_position(0.0f, 0.0f), _valid_position(false), _trigger_pub(nullptr), - _cmd_ack_pub(nullptr), _trigger_mode(TRIGGER_MODE_NONE), _cam_cap_fback(0), _camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO), @@ -477,12 +478,13 @@ CameraTrigger::stop() void CameraTrigger::test() { - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); vcmd.param5 = 1.0; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; - orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); } void @@ -682,20 +684,15 @@ CameraTrigger::Run() // Command ACK handling if (updated && need_ack) { - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.result = (uint8_t)cmd_result; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; - if (_cmd_ack_pub == nullptr) { - _cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _cmd_ack_pub, &command_ack); - } + _cmd_ack_pub.publish(command_ack); } ScheduleDelayed(poll_interval_usec); diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index e02bb4fa24..53dc01c2d2 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -49,14 +49,12 @@ #define BOARD_MAX_LEDS 4 #endif +static_assert(led_control_s::ORB_QUEUE_LENGTH >= BOARD_MAX_LEDS, "led_control_s::ORB_QUEUE_LENGTH too small"); + #if BOARD_MAX_LEDS > 8 // because led_mask is uint8_t #error "BOARD_MAX_LEDS too large. You need to change the led_mask type in the led_control uorb topic (and where it's used)" #endif - -// set the queue size to the number of LED's, so that each led can be controlled individually -static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS; - // Legacy paths - 2 are need to allow both pwm and i2c drviers to co-exist #define RGBLED0_DEVICE_PATH "/dev/rgbled0" // Primary RGB LED on i2c #define RGBLED1_DEVICE_PATH "/dev/rgbled1" // Primary RGB LED(NCP5623C) on i2c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 3979a267ce..75dd3bad50 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -172,9 +173,10 @@ private: const Instance _instance; uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; - orb_advert_t _dump_communication_pub{nullptr}; ///< if non-null, dump communication + uORB::PublicationQueued _dump_communication_pub{ORB_ID(gps_dump)}; gps_dump_s *_dump_to_device{nullptr}; gps_dump_s *_dump_from_device{nullptr}; + bool _should_dump_communication{false}; ///< if true, dump communication static volatile bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 /// and thus we wait until the first one publishes at least one message. @@ -549,16 +551,16 @@ void GPS::initializeCommunicationDump() memset(_dump_to_device, 0, sizeof(gps_dump_s)); memset(_dump_from_device, 0, sizeof(gps_dump_s)); - int instance; //make sure to use a large enough queue size, so that we don't lose messages. You may also want //to increase the logger rate for that. - _dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance, - ORB_PRIO_DEFAULT, 8); + _dump_communication_pub.publish(*_dump_from_device); + + _should_dump_communication = true; } void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) { - if (!_dump_communication_pub) { + if (!_should_dump_communication) { return; } @@ -582,7 +584,7 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) } dump_data->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data); + _dump_communication_pub.publish(*dump_data); dump_data->len = 0; } } @@ -820,10 +822,6 @@ GPS::run() PX4_INFO("exiting"); - if (_dump_communication_pub) { - orb_unadvertise(_dump_communication_pub); - } - if (_serial_fd >= 0) { ::close(_serial_fd); _serial_fd = -1; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 21c5abab43..045735d900 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -76,6 +76,7 @@ #include #include +#include #include #include #include @@ -738,7 +739,8 @@ PX4IO::init() vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; /* send command once */ - orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); /* spin here until IO's state has propagated into the system */ do { @@ -755,7 +757,7 @@ PX4IO::init() /* re-send if necessary */ if (!actuator_armed.force_failsafe) { - orb_publish(ORB_ID(vehicle_command), pub, &vcmd); + vcmd_pub.publish(vcmd); PX4_WARN("re-sending flight termination cmd"); } @@ -769,7 +771,8 @@ PX4IO::init() vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; /* send command once */ - orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); /* spin here until IO's state has propagated into the system */ do { @@ -786,7 +789,7 @@ PX4IO::init() /* re-send if necessary */ if (!actuator_armed.armed) { - orb_publish(ORB_ID(vehicle_command), pub, &vcmd); + vcmd_pub.publish(vcmd); PX4_WARN("re-sending arm cmd"); } diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 9c118fc5c5..ade79e314c 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -1111,14 +1111,8 @@ void IridiumSBD::publish_iridium_status() // publish the status if it changed if (need_to_publish) { - if (_iridiumsbd_status_pub == nullptr) { - _iridiumsbd_status_pub = orb_advertise(ORB_ID(iridiumsbd_status), &_status); - - } else { - orb_publish(ORB_ID(iridiumsbd_status), _iridiumsbd_status_pub, &_status); - } + _iridiumsbd_status_pub.publish(_status); } - } void IridiumSBD::publish_subsystem_status() @@ -1134,12 +1128,7 @@ void IridiumSBD::publish_subsystem_status() _info.enabled = enabled; _info.ok = ok; - if (_subsystem_pub == nullptr) { - _subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info); - } + _subsystem_pub.publish(_info); } } diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 18085182a7..85fae0ab42 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -39,7 +39,8 @@ #include #include -#include +#include +#include #include #include @@ -304,8 +305,8 @@ private: bool _writing_mavlink_packet = false; uint16_t _packet_length = 0; - orb_advert_t _iridiumsbd_status_pub = nullptr; - orb_advert_t _subsystem_pub = nullptr; + uORB::Publication _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)}; + uORB::PublicationQueued _subsystem_pub{ORB_ID(subsystem_info)}; bool _test_pending = false; char _test_command[32]; diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index ed96586bc5..c47c739fc1 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -564,20 +564,14 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } // Acknowledge the received command - vehicle_command_ack_s ack = {}; + vehicle_command_ack_s ack{}; ack.timestamp = hrt_absolute_time(); ack.command = cmd.command; ack.result = cmd_ack_result; ack.target_system = cmd.source_system; ack.target_component = cmd.source_component; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &ack); - } - + _command_ack_pub.publish(ack); } // Shut down once armed diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index ec01ea5010..7465e546c3 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -36,7 +36,9 @@ #include #include "uavcan_driver.hpp" #include -#include +#include +#include +#include #include #include @@ -160,7 +162,7 @@ private: // uORB topic handle for MAVLink parameter responses orb_advert_t _param_response_pub = nullptr; - orb_advert_t _command_ack_pub = nullptr; + uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; typedef uavcan::MethodBinder &)> GetSetCallback; diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index cdc4aa16d2..8596b5745e 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -137,22 +137,16 @@ void FlightTasks::reActivate() void FlightTasks::_updateCommand() { - // lazy subscription to command topic - if (_sub_vehicle_command < 0) { - _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - } - // check if there's any new command - bool updated = false; - orb_check(_sub_vehicle_command, &updated); + bool updated = _sub_vehicle_command.updated(); if (!updated) { return; } // get command - struct vehicle_command_s command; - orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); + vehicle_command_s command{}; + _sub_vehicle_command.copy(&command); // check what command it is FlightTaskIndex desired_task = switchVehicleCommand(command.command); @@ -183,19 +177,13 @@ void FlightTasks::_updateCommand() } // send back acknowledgment - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = command.command; command_ack.result = cmd_result; command_ack.result_param1 = switch_result; command_ack.target_system = command.source_system; command_ack.target_component = command.source_component; - if (_pub_vehicle_command_ack == nullptr) { - _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); - - } + _pub_vehicle_command_ack.publish(command_ack); } diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index dc5020c2b2..cba7706d3a 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,6 +45,10 @@ #include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include +#include +#include +#include +#include #include @@ -180,8 +184,10 @@ private: */ void _updateCommand(); FlightTaskIndex switchVehicleCommand(const int command); - int _sub_vehicle_command = -1; /**< topic handle on which commands are received */ - orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */ + + uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */ + + uORB::PublicationQueued _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)}; int _initTask(FlightTaskIndex task_index); }; diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp index 2fac77484c..fbdcb8f982 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp @@ -63,19 +63,6 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : } -ObstacleAvoidance::~ObstacleAvoidance() -{ - //unadvertise publishers - if (_pub_traj_wp_avoidance_desired != nullptr) { - orb_unadvertise(_pub_traj_wp_avoidance_desired); - } - - if (_pub_pos_control_status != nullptr) { - orb_unadvertise(_pub_pos_control_status); - } - -} - bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) { @@ -170,7 +157,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - _publishAvoidanceDesiredWaypoint(); + _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); _desired_waypoint = empty_trajectory_waypoint; } @@ -208,27 +195,7 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector // do not check for waypoints yaw acceptance in navigator pos_control_status.yaw_acceptance = NAN; - if (_pub_pos_control_status == nullptr) { - _pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status); - - } else { - orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status); - - } - -} - -void -ObstacleAvoidance::_publishAvoidanceDesiredWaypoint() -{ - // publish desired waypoint - if (_pub_traj_wp_avoidance_desired != nullptr) { - orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _pub_traj_wp_avoidance_desired, &_desired_waypoint); - - } else { - _pub_traj_wp_avoidance_desired = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired), - &_desired_waypoint); - } + _pub_pos_control_status.publish(pos_control_status); } void ObstacleAvoidance::_publishVehicleCmdDoLoiter() @@ -248,11 +215,5 @@ void ObstacleAvoidance::_publishVehicleCmdDoLoiter() command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; // publish the vehicle command - if (_pub_vehicle_command == nullptr) { - _pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command, - vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command); - } + _pub_vehicle_command.publish(command); } diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index f873e1702b..f5e2803429 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -46,6 +46,8 @@ #include #include +#include +#include #include #include #include @@ -62,7 +64,7 @@ class ObstacleAvoidance : public ModuleParams { public: ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance(); + ~ObstacleAvoidance() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array); @@ -113,10 +115,11 @@ private: (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ ); - vehicle_trajectory_waypoint_s _desired_waypoint = {}; /**< desired vehicle trajectory waypoint to be sent to OA */ - orb_advert_t _pub_traj_wp_avoidance_desired{nullptr}; /**< trajectory waypoint desired publication */ - orb_advert_t _pub_pos_control_status{nullptr}; /**< position controller status publication */ - orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */ + vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ + + uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ + uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ + uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ matrix::Vector3f _curr_wp = {}; /**< current position triplet */ matrix::Vector3f _position = {}; /**< current vehicle position */ @@ -126,11 +129,6 @@ private: bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ - /** - * Publishes vehicle trajectory waypoint desired. - */ - void _publishAvoidanceDesiredWaypoint(); - /** * Publishes vehicle command. */ diff --git a/src/lib/systemlib/CMakeLists.txt b/src/lib/systemlib/CMakeLists.txt index 62a3b11619..4975dfcb12 100644 --- a/src/lib/systemlib/CMakeLists.txt +++ b/src/lib/systemlib/CMakeLists.txt @@ -35,7 +35,7 @@ set(SRCS conversions.c cpuload.c crc.c - mavlink_log.c + mavlink_log.cpp otp.c ) diff --git a/src/lib/systemlib/mavlink_log.c b/src/lib/systemlib/mavlink_log.cpp similarity index 90% rename from src/lib/systemlib/mavlink_log.c rename to src/lib/systemlib/mavlink_log.cpp index a83a6d680a..142be78ced 100644 --- a/src/lib/systemlib/mavlink_log.c +++ b/src/lib/systemlib/mavlink_log.cpp @@ -48,9 +48,6 @@ #include #include "mavlink_log.h" -#define MAVLINK_LOG_QUEUE_SIZE 5 - - __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...) { // TODO: add compile check for maxlen @@ -59,31 +56,23 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con return; } - if (mavlink_log_pub == NULL) { + if (mavlink_log_pub == nullptr) { return; } - struct mavlink_log_s log_msg; - + mavlink_log_s log_msg; log_msg.severity = severity; - log_msg.timestamp = hrt_absolute_time(); va_list ap; - va_start(ap, fmt); - vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap); - va_end(ap); - if (*mavlink_log_pub != NULL) { + if (*mavlink_log_pub != nullptr) { orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); } else { - *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), - &log_msg, - MAVLINK_LOG_QUEUE_SIZE); + *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH); } } - diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0349882b1a..87d008d584 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -93,7 +93,6 @@ #include #include #include -#include #include #include @@ -205,7 +204,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch */ void *commander_low_prio_loop(void *arg); -static void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub); +static void answer_command(const vehicle_command_s &cmd, unsigned result, + uORB::PublicationQueued &command_ack_pub); static int power_button_state_notification_cb(board_power_button_state_notification_e request) { @@ -249,7 +249,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN) { - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = param1; vcmd.param2 = param2; @@ -262,9 +263,9 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 vcmd.target_system = status.system_id; vcmd.target_component = status.component_id; - orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; - return (h != nullptr); + return vcmd_pub.publish(vcmd); } int commander_main(int argc, char *argv[]) @@ -577,7 +578,7 @@ Commander::~Commander() bool Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local, - orb_advert_t *command_ack_pub, bool *changed) + uORB::PublicationQueued &command_ack_pub, bool *changed) { /* only handle commands that are meant to be handled by this system and component */ if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) @@ -1078,13 +1079,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); break; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(cmd, cmd_result, *command_ack_pub); + answer_command(cmd, cmd_result, command_ack_pub); } return true; @@ -1239,7 +1240,7 @@ Commander::run() hrt_abstime last_disarmed_timestamp = 0; /* command ack */ - orb_advert_t command_ack_pub = nullptr; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ mission_init(); @@ -2152,7 +2153,7 @@ Commander::run() cmd_sub.copy(&cmd); /* handle it */ - if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) { + if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) { status_changed = true; } } @@ -3260,7 +3261,8 @@ print_reject_arm(const char *msg) } } -void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub) +void answer_command(const vehicle_command_s &cmd, unsigned result, + uORB::PublicationQueued &command_ack_pub) { switch (result) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -3288,20 +3290,16 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t } /* publish ACK */ - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.result = (uint8_t)result; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; - if (command_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); - } else { - command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + command_ack_pub.publish(command_ack); } void *commander_low_prio_loop(void *arg) @@ -3313,7 +3311,7 @@ void *commander_low_prio_loop(void *arg) int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); /* command ack */ - orb_advert_t command_ack_pub = nullptr; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; /* wakeup source(s) */ px4_pollfd_struct_t fds[1]; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e29f54e44a..47c9328c56 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -45,8 +45,10 @@ // publications #include +#include #include #include +#include #include #include #include @@ -180,7 +182,7 @@ private: bool _flight_termination_triggered{false}; bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, - orb_advert_t *command_ack_pub, bool *changed); + uORB::PublicationQueued &command_ack_pub, bool *changed); bool set_home_position(); bool set_home_position_alt_only(); @@ -258,12 +260,13 @@ private: // Publications uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; - uORB::PublicationData _home_pub{ORB_ID(home_position)}; uORB::Publication _status_pub{ORB_ID(vehicle_status)}; uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; + uORB::PublicationData _home_pub{ORB_ID(home_position)}; + }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/arm_auth.cpp index 891f9c7038..4dbd79b799 100644 --- a/src/modules/commander/arm_auth.cpp +++ b/src/modules/commander/arm_auth.cpp @@ -39,10 +39,10 @@ #include #include +#include #include #include -static orb_advert_t handle_vehicle_command_pub; static orb_advert_t *mavlink_log_pub; static int command_ack_sub = -1; @@ -78,17 +78,13 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = { static void arm_auth_request_msg_send() { - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST; vcmd.target_system = arm_parameters.authorizer_system_id; - if (handle_vehicle_command_pub == nullptr) { - handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd); - } + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); } static uint8_t _auth_method_arm_req_check() diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a0107297e7..d8e1554c76 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -290,7 +290,7 @@ int led_init() led_control.mode = led_control_s::MODE_OFF; led_control.priority = 0; led_control.timestamp = hrt_absolute_time(); - led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); + led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH); /* first open normal LEDs */ DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 752c7652a4..1ef9c6589f 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -195,20 +195,15 @@ void SendEvent::process_commands() void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result) { /* publish ACK */ - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.result = (uint8_t)result; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; - if (_command_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - - } else { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); } int SendEvent::print_usage(const char *reason) @@ -275,7 +270,7 @@ int SendEvent::custom_command(int argc, char *argv[]) } } - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); vcmd.param2 = NAN; @@ -286,7 +281,8 @@ int SendEvent::custom_command(int argc, char *argv[]) vcmd.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; - orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); } else { print_usage("unrecognized command"); diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index be3b9a49bd..805084cabd 100644 --- a/src/modules/events/send_event.h +++ b/src/modules/events/send_event.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -132,9 +133,6 @@ private: /** @var _rc_loss_alarm Pointer to the RC loss alarm object. */ rc_loss::RC_Loss_Alarm *_rc_loss_alarm = nullptr; - /** @var _command_ack_pub The command ackowledgement topic. */ - orb_advert_t _command_ack_pub = nullptr; - /** @note Declare local parameters using defined parameters. */ DEFINE_PARAMETERS( /** @var _param_status_display Parameter to enable/disable the LED status display. */ diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp index b2c4fe5a65..118e342efc 100644 --- a/src/modules/events/status_display.cpp +++ b/src/modules/events/status_display.cpp @@ -102,12 +102,7 @@ void StatusDisplay::publish() { _led_control.timestamp = hrt_absolute_time(); - if (_led_control_pub != nullptr) { - orb_publish(ORB_ID(led_control), _led_control_pub, &_led_control); - - } else { - _led_control_pub = orb_advertise_queue(ORB_ID(led_control), &_led_control, LED_UORB_QUEUE_LENGTH); - } + _led_control_pub.publish(_led_control); } } /* namespace status */ diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index 696956416c..8a6d35eb2e 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -44,7 +44,7 @@ #include -#include +#include #include #include #include @@ -96,7 +96,9 @@ private: bool _critical_battery = false; int _old_nav_state = -1; int _old_battery_status_warning = -1; - orb_advert_t _led_control_pub = nullptr; + + uORB::PublicationQueued _led_control_pub{ORB_ID(led_control)}; + const events::SubscriberHandler &_subscriber_handler; }; diff --git a/src/modules/events/temperature_calibration/task.cpp b/src/modules/events/temperature_calibration/task.cpp index d8c3b97065..29283014b4 100644 --- a/src/modules/events/temperature_calibration/task.cpp +++ b/src/modules/events/temperature_calibration/task.cpp @@ -39,6 +39,7 @@ * @author Beat Küng */ +#include #include #include #include @@ -92,7 +93,7 @@ public: private: void publish_led_control(led_control_s &led_control); - orb_advert_t _led_control_pub = nullptr; + uORB::PublicationQueued _led_control_pub{ORB_ID(led_control)}; bool _force_task_exit = false; int _control_task = -1; // task handle for task @@ -349,13 +350,7 @@ int TemperatureCalibration::start() void TemperatureCalibration::publish_led_control(led_control_s &led_control) { led_control.timestamp = hrt_absolute_time(); - - if (_led_control_pub == nullptr) { - _led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(led_control), _led_control_pub, &led_control); - } + _led_control_pub.publish(led_control); } int run_temperature_calibration(bool accel, bool baro, bool gyro) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index f3f986dfeb..26d0a65db8 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -44,7 +44,7 @@ #include #include -#include +#include #include #include #include @@ -935,7 +935,6 @@ void Logger::run() } int vehicle_command_sub = -1; - orb_advert_t vehicle_command_ack_pub = nullptr; if (_writer.backend() & LogWriter::BackendMavlink) { vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1042,7 +1041,7 @@ void Logger::run() /* check for logging command from MAVLink (start/stop streaming) */ if (vehicle_command_sub >= 0) { - handle_vehicle_command_update(vehicle_command_sub, vehicle_command_ack_pub); + handle_vehicle_command_update(vehicle_command_sub); } @@ -1293,10 +1292,6 @@ void Logger::run() _mavlink_log_pub = nullptr; } - if (vehicle_command_ack_pub) { - orb_unadvertise(vehicle_command_ack_pub); - } - if (vehicle_command_sub != -1) { orb_unsubscribe(vehicle_command_sub); } @@ -1402,7 +1397,7 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su return bret; } -void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub) +void Logger::handle_vehicle_command_update(int vehicle_command_sub) { bool command_updated = false; int ret = orb_check(vehicle_command_sub, &command_updated); @@ -1413,23 +1408,19 @@ void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { if ((int)(command.param1 + 0.5f) != 0) { - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } else if (can_start_mavlink_log()) { - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); start_log_mavlink(); } else { - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } } else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) { stop_log_mavlink(); - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } } } @@ -2339,7 +2330,7 @@ void Logger::write_changed_parameters(LogType type) _writer.notify(); } -void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result) +void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result) { vehicle_command_ack_s vehicle_command_ack = {}; vehicle_command_ack.timestamp = hrt_absolute_time(); @@ -2348,13 +2339,8 @@ void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_ vehicle_command_ack.target_system = cmd->source_system; vehicle_command_ack.target_component = cmd->source_component; - if (vehicle_command_ack_pub == nullptr) { - vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack); - } + uORB::PublicationQueued cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); } int Logger::print_usage(const char *reason) diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index e76146bc1d..4816e589dc 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -357,8 +357,8 @@ private: */ bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type); - void handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub); - void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result); + void handle_vehicle_command_update(int vehicle_command_sub); + void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result); /** * initialize the output for the process load, so that ~1 second later it will be written to the log diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 946aa8cce4..a018c52511 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -49,8 +49,9 @@ #endif #include -#include -#include +#include +#include +#include #include "mavlink_receiver.h" #include "mavlink_main.h" @@ -2126,7 +2127,7 @@ Mavlink::task_main(int argc, char *argv[]) cmd_sub->subscribe_from_beginning(true); /* command ack */ - orb_advert_t command_ack_pub = nullptr; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); @@ -2263,7 +2264,7 @@ Mavlink::task_main(int argc, char *argv[]) } // send positive command ack - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = vehicle_cmd.timestamp; command_ack.command = vehicle_cmd.command; command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; @@ -2271,13 +2272,7 @@ Mavlink::task_main(int argc, char *argv[]) command_ack.target_system = vehicle_cmd.source_system; command_ack.target_component = vehicle_cmd.source_component; - if (command_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); - - } else { - command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + command_ack_pub.publish(command_ack); } } @@ -2542,12 +2537,7 @@ void Mavlink::publish_telemetry_status() _tstatus.timestamp = hrt_absolute_time(); - if (_telem_status_pub == nullptr) { - _telem_status_pub = orb_advertise_queue(ORB_ID(telemetry_status), &_tstatus, 3); - - } else { - orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &_tstatus); - } + _telem_status_pub.publish(_tstatus); } void Mavlink::check_radio_config() diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5ab8d0fb9b..2aab99e98e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -71,11 +71,11 @@ #include #include #include +#include #include #include #include #include -#include #include "mavlink_command_sender.h" #include "mavlink_messages.h" @@ -540,7 +540,8 @@ private: bool _first_heartbeat_sent{false}; orb_advert_t _mavlink_log_pub{nullptr}; - orb_advert_t _telem_status_pub{nullptr}; + + uORB::PublicationQueued _telem_status_pub{ORB_ID(telemetry_status)}; bool _task_running{false}; static bool _boot_complete; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b76d098137..32faf3acd4 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -270,7 +270,6 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } - void MavlinkMissionManager::send_mission_current(uint16_t seq) { diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 635683408c..2a868e20bf 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -50,13 +50,6 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : { } -MavlinkParametersManager::~MavlinkParametersManager() -{ - if (_uavcan_parameter_request_pub) { - orb_unadvertise(_uavcan_parameter_request_pub); - } -} - unsigned MavlinkParametersManager::get_size() { @@ -91,12 +84,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) req.node_id = req_list.target_component; req.param_index = 0; - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + _uavcan_parameter_request_pub.publish(req); } break; @@ -163,12 +151,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) req.int_value = val; } - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + _uavcan_parameter_request_pub.publish(req); } break; @@ -577,12 +560,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter() if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) { uavcan_parameter_request_s req = _uavcan_open_request_list->req; - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise_queue(ORB_ID(uavcan_parameter_request), &req, 5); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + _uavcan_parameter_request_pub.publish(req); _uavcan_waiting_for_request_response = true; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 6c82a42cd8..4219a85332 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -45,7 +45,7 @@ #include #include "mavlink_bridge_header.h" -#include +#include #include #include #include @@ -59,7 +59,7 @@ class MavlinkParametersManager { public: explicit MavlinkParametersManager(Mavlink *mavlink); - ~MavlinkParametersManager(); + ~MavlinkParametersManager() = default; /** * Handle sending of messages. Call this regularly at a fixed frequency. @@ -128,7 +128,8 @@ protected: orb_advert_t _rc_param_map_pub{nullptr}; rc_parameter_map_s _rc_param_map{}; - orb_advert_t _uavcan_parameter_request_pub{nullptr}; + uORB::PublicationQueued _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; + uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)}; uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a723b51f7b..818d0c0a45 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,20 +98,14 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result) { - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = command; command_ack.result = result; command_ack.target_system = sysid; command_ack.target_component = compid; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - } + _cmd_ack_pub.publish(command_ack); } void @@ -515,12 +509,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } if (!send_ack) { - if (_cmd_pub == nullptr) { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command); - } + _cmd_pub.publish(vehicle_command); } } @@ -547,13 +536,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) command_ack.target_system = ack.target_system; command_ack.target_component = ack.target_component; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - } + _cmd_ack_pub.publish(command_ack); // TODO: move it to the same place that sent the command if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) { @@ -687,7 +670,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) union px4_custom_mode custom_mode; custom_mode.data = new_mode.custom_mode; - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ @@ -703,12 +686,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.confirmation = true; vcmd.from_external = true; - if (_cmd_pub == nullptr) { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } + _cmd_pub.publish(vcmd); } void @@ -2274,12 +2252,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) //PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc); - if (_transponder_report_pub == nullptr) { - _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); - } + _transponder_report_pub.publish(t); } void @@ -2343,13 +2316,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) // Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not // provide these. - - if (_transponder_report_pub == nullptr) { - _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); - } + _transponder_report_pub.publish(t); _last_utm_global_pos_com = t.timestamp; } @@ -2392,16 +2359,7 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data, math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len)); - orb_advert_t &pub = _gps_inject_data_pub; - - if (pub == nullptr) { - pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic, - _gps_inject_data_queue_size); - - } else { - orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic); - } - + _gps_inject_data_pub.publish(gps_inject_data_topic); } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 23982cd590..08144cb427 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -43,6 +43,7 @@ #include +#include #include #include #include @@ -71,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -212,6 +214,11 @@ private: vehicle_land_detected_s _hil_land_detector {}; vehicle_control_mode_s _control_mode {}; + uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::PublicationQueued _transponder_report_pub{ORB_ID(transponder_report)}; + uORB::PublicationQueued _cmd_pub{ORB_ID(vehicle_command)}; + uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + orb_advert_t _accel_pub{nullptr}; orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr}; orb_advert_t _airspeed_pub{nullptr}; @@ -219,9 +226,7 @@ private: orb_advert_t _attitude_pub{nullptr}; orb_advert_t _baro_pub{nullptr}; orb_advert_t _battery_pub{nullptr}; - orb_advert_t _cmd_pub{nullptr}; orb_advert_t _collision_report_pub{nullptr}; - orb_advert_t _command_ack_pub{nullptr}; orb_advert_t _debug_array_pub{nullptr}; orb_advert_t _debug_key_value_pub{nullptr}; orb_advert_t _debug_value_pub{nullptr}; @@ -231,7 +236,6 @@ private: orb_advert_t _flow_pub{nullptr}; orb_advert_t _follow_target_pub{nullptr}; orb_advert_t _global_pos_pub{nullptr}; - orb_advert_t _gps_inject_data_pub{nullptr}; orb_advert_t _gps_pub{nullptr}; orb_advert_t _gyro_pub{nullptr}; orb_advert_t _hil_distance_sensor_pub{nullptr}; @@ -249,15 +253,12 @@ private: orb_advert_t _rates_sp_pub{nullptr}; orb_advert_t _rc_pub{nullptr}; orb_advert_t _trajectory_waypoint_pub{nullptr}; - orb_advert_t _transponder_report_pub{nullptr}; orb_advert_t _visual_odometry_pub{nullptr}; uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - static constexpr int _gps_inject_data_queue_size{6}; - static constexpr unsigned int MOM_SWITCH_COUNT{8}; int _orb_class_instance{-1}; 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 d2c7cfb300..a4caeca89b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -108,7 +109,7 @@ private: Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ - orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */ + uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */ orb_advert_t _mavlink_log_pub{nullptr}; orb_id_t _attitude_setpoint_id{nullptr}; @@ -1038,13 +1039,7 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state) } // publish the vehicle command - if (_pub_vehicle_command == nullptr) { - _pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command, - vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command); - } + _pub_vehicle_command.publish(command); } int MulticopterPositionControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/muorb/test/muorb_test_example.cpp b/src/modules/muorb/test/muorb_test_example.cpp index d8af5f81e5..364b24981f 100644 --- a/src/modules/muorb/test/muorb_test_example.cpp +++ b/src/modules/muorb/test/muorb_test_example.cpp @@ -43,6 +43,9 @@ #include #include #include +#include +#include +#include px4::AppState MuorbTestExample::appState; @@ -50,7 +53,7 @@ int MuorbTestExample::main() { int rc; appState.setRunning(true); - rc = PingPongTest(); + rc = PingPongTest(); appState.setRunning(false); return rc; } @@ -58,63 +61,37 @@ int MuorbTestExample::main() int MuorbTestExample::DefaultTest() { int i = 0; - orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status); - if (pub_id == nullptr) { - PX4_ERR("error publishing esc_status"); - return -1; - } - - orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); - - if (pub_id_vc == nullptr) { - PX4_ERR("error publishing vehicle_command"); - return -1; - } - - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - return -1; - } - - int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); - - if (sub_vc == PX4_ERROR) { - PX4_ERR("Error subscribing to vehicle_command topic"); - return -1; - } + uORB::Subscription sub_vc{ORB_ID(vehicle_command)}; + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + uORB::Publication pub_id{ORB_ID(esc_status)}; + pub_id.publish(m_esc_status); while (!appState.exitRequested() && i < 100) { PX4_DEBUG("[%d] Doing work...", i); - if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) { + if (!pub_id.publish(m_esc_status)) { PX4_ERR("[%d]Error publishing the esc status message for iter", i); break; } - bool updated = false; + if (sub_vc.updated()) { + PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); - if (orb_check(sub_vc, &updated) == 0) { - if (updated) { - PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); + if (!sub_vc.copy(&m_vc)) { + PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); + break; + } - if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { - PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); - break; - } - - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - break; - } - - } else { - PX4_DEBUG("[%d] VC topic is not updated", i); + if (!vcmd_pub.publish(m_vc)) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; } } else { PX4_ERR("[%d]Error checking the updated status for vechile command... ", i); + PX4_DEBUG("[%d] VC topic is not updated", i); break; } @@ -127,51 +104,28 @@ int MuorbTestExample::DefaultTest() int MuorbTestExample::PingPongTest() { int i = 0; - orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); - - if (pub_id_vc == nullptr) { - PX4_ERR("error publishing vehicle_command"); - return -1; - } - - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - return -1; - } - - int sub_esc_status = orb_subscribe(ORB_ID(esc_status)); - - if (sub_esc_status == PX4_ERROR) { - PX4_ERR("Error subscribing to esc_status topic"); - return -1; - } + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + uORB::Subscription sub_esc_status{ORB_ID(esc_status)}; while (!appState.exitRequested()) { PX4_INFO("[%d] Doing work...", i); - bool updated = false; - if (orb_check(sub_esc_status, &updated) == 0) { - if (updated) { - PX4_INFO("[%d]ESC status is updated... reading new value", i); + if (sub_esc_status.updated()) { + PX4_INFO("[%d]ESC status is updated... reading new value", i); - if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) { - PX4_ERR("[%d]Error calling orb copy for esc status... ", i); - break; - } + if (!sub_esc_status.copy(&m_esc_status)) { + PX4_ERR("[%d]Error calling orb copy for esc status... ", i); + break; + } - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - break; - } - - } else { - PX4_INFO("[%d] esc status topic is not updated", i); + if (!vcmd_pub.publish(m_vc)) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; } } else { - PX4_ERR("[%d]Error checking the updated status for esc status... ", i); - break; + PX4_INFO("[%d] esc status topic is not updated", i); } // sleep for 1 sec. diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7643b2cad2..b0549c8a28 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -330,8 +331,9 @@ private: uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ - orb_advert_t _vehicle_cmd_ack_pub{nullptr}; - orb_advert_t _vehicle_cmd_pub{nullptr}; + + uORB::PublicationQueued _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationQueued _vehicle_cmd_pub{ORB_ID(vehicle_command)}; // Subscriptions home_position_s _home_pos{}; /**< home position for RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8aae5510ae..3f9b6336eb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -903,7 +903,7 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; - transponder_report_s tr = {}; + transponder_report_s tr{}; tr.timestamp = hrt_absolute_time(); tr.icao_address = 1234; tr.lat = lat; // Latitude, expressed as degrees @@ -923,8 +923,8 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields tr.squawk = 6667; - orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(h); + uORB::PublicationQueued tr_pub{ORB_ID(transponder_report)}; + tr_pub.publish(tr); } void Navigator::check_traffic() @@ -1142,12 +1142,7 @@ Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) break; } - if (_vehicle_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd); - - } else { - _vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - } + _vehicle_cmd_pub.publish(*vcmd); } void @@ -1165,13 +1160,7 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res command_ack.result_param1 = 0; command_ack.result_param2 = 0; - if (_vehicle_cmd_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack); - - } else { - _vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + _vehicle_cmd_ack_pub.publish(command_ack); } int Navigator::print_usage(const char *reason) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index dd874fe9e3..bfa67ba020 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -951,12 +951,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n _info.enabled = true; _info.ok = false; - if (_info_pub == nullptr) { - _info_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(subsystem_info), _info_pub, &_info); - } + _info_pub.publish(_info); } } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index f905690506..d008430fed 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -52,6 +52,7 @@ #include #include +#include #include #include #include @@ -274,8 +275,8 @@ private: bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */ /* subsystem info publication */ - struct subsystem_info_s _info; - orb_advert_t _info_pub = nullptr; + subsystem_info_s _info{}; + uORB::PublicationQueued _info_pub{ORB_ID(subsystem_info)}; uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */ uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {}; diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index c146c5e853..debdce3f44 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -45,8 +45,7 @@ namespace uORB { /** - * Base publication wrapper class, used in list traversal - * of various publications. + * Base publication wrapper class */ template class Publication @@ -61,7 +60,7 @@ public: * @param priority The priority for multi pub/sub, 0-based, -1 means * don't publish as multi */ - Publication(const orb_metadata *meta, int priority = -1) : _meta(meta), _priority(priority) {} + Publication(const orb_metadata *meta, uint8_t priority = 0) : _meta(meta), _priority(priority) {} ~Publication() { orb_unadvertise(_handle); } @@ -74,12 +73,7 @@ public: bool updated = false; if (_handle != nullptr) { - if (orb_publish(_meta, _handle, &data) != PX4_OK) { - PX4_ERR("%s publish fail", _meta->o_name); - - } else { - updated = true; - } + updated = (orb_publish(_meta, _handle, &data) == PX4_OK); } else { orb_advert_t handle = nullptr; @@ -106,13 +100,14 @@ public: protected: const orb_metadata *_meta; - const int _priority; orb_advert_t _handle{nullptr}; + + const uint8_t _priority; }; /** - * The publication base class as a list node. + * The publication class with data. */ template class PublicationData : public Publication diff --git a/src/modules/uORB/PublicationQueued.hpp b/src/modules/uORB/PublicationQueued.hpp new file mode 100644 index 0000000000..d3db333106 --- /dev/null +++ b/src/modules/uORB/PublicationQueued.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PublicationQueued.hpp + * + */ + +#pragma once + +#include +#include + +namespace uORB +{ + +/** + * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) + */ +template +class PublicationQueued +{ +public: + + /** + * Constructor + * + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + */ + PublicationQueued(const orb_metadata *meta) : _meta(meta) {} + + ~PublicationQueued() { orb_unadvertise(_handle); } + + /** + * Publish the struct + * @param data The uORB message struct we are updating. + */ + bool publish(const T &data) + { + bool updated = false; + + if (_handle != nullptr) { + updated = (orb_publish(_meta, _handle, &data) == PX4_OK); + + } else { + orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH); + + if (handle != nullptr) { + _handle = handle; + updated = true; + + } else { + PX4_ERR("%s advertise failed", _meta->o_name); + } + } + + return updated; + } + +protected: + const orb_metadata *_meta; + + orb_advert_t _handle{nullptr}; +}; + +} // namespace uORB diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 91b4f2dc7c..dbceb7d15f 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -39,7 +39,7 @@ */ #include "input_mavlink.h" -#include +#include #include #include #include @@ -333,7 +333,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) { - vehicle_command_ack_s vehicle_command_ack = {}; + vehicle_command_ack_s vehicle_command_ack{}; vehicle_command_ack.timestamp = hrt_absolute_time(); vehicle_command_ack.command = cmd->command; @@ -341,13 +341,8 @@ void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) vehicle_command_ack.target_system = cmd->source_system; vehicle_command_ack.target_component = cmd->source_component; - if (_vehicle_command_ack_pub == nullptr) { - _vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack); - } + uORB::PublicationQueued cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); } void InputMavlinkCmdMount::print_status() diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index 424e1a60eb..b3661466ec 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -43,13 +43,11 @@ #include "input_rc.h" #include -#include #include +#include namespace vmount { - - /** ** class InputMavlinkROI ** Input based on the vehicle_roi topic @@ -95,7 +93,6 @@ private: void _ack_vehicle_command(vehicle_command_s *cmd); int _vehicle_command_sub = -1; - orb_advert_t _vehicle_command_ack_pub = nullptr; bool _stabilize[3] = { false, false, false }; int32_t _mav_sys_id{1}; ///< our mavlink system id diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 4e10867de8..1d508be2b1 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -54,16 +54,9 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config) { } -OutputMavlink::~OutputMavlink() -{ - if (_vehicle_command_pub) { - orb_unadvertise(_vehicle_command_pub); - } -} - int OutputMavlink::update(const ControlData *control_data) { - vehicle_command_s vehicle_command = {}; + vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id; vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id; @@ -82,18 +75,7 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; } - if (_vehicle_command_pub) { - orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command); - - } else { - _vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, - vehicle_command_s::ORB_QUEUE_LENGTH); - } - - } - - if (!_vehicle_command_pub) { - return 0; + _vehicle_command_pub.publish(vehicle_command); } _handle_position_update(); @@ -110,7 +92,7 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F; vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F; - orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command); + _vehicle_command_pub.publish(vehicle_command); _last_update = t; diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 24c2272afa..b4a6ce3f0d 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -41,13 +41,11 @@ #include "output.h" -#include - +#include +#include namespace vmount { - - /** ** class OutputMavlink * Output via vehicle_command topic @@ -56,14 +54,15 @@ class OutputMavlink : public OutputBase { public: OutputMavlink(const OutputConfig &output_config); - virtual ~OutputMavlink(); + virtual ~OutputMavlink() = default; virtual int update(const ControlData *control_data); virtual void print_status(); private: - orb_advert_t _vehicle_command_pub = nullptr; + + uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; }; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e535e78f50..de638dde14 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -49,6 +49,7 @@ #include "vtol_att_control_main.h" #include #include +#include using namespace matrix; @@ -193,20 +194,15 @@ VtolAttitudeControl::handle_command() // This might not be optimal but is better than no response at all. if (_vehicle_cmd.from_external) { - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = _vehicle_cmd.command; command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; command_ack.target_system = _vehicle_cmd.source_system; command_ack.target_component = _vehicle_cmd.source_component; - if (_v_cmd_ack_pub == nullptr) { - _v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack); - } + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); } } } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 4a381b3ab6..8d4ddcb2bc 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -144,7 +144,6 @@ private: orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle orb_advert_t _v_att_sp_pub{nullptr}; - orb_advert_t _v_cmd_ack_pub{nullptr}; orb_advert_t _vtol_vehicle_status_pub{nullptr}; orb_advert_t _actuators_1_pub{nullptr}; diff --git a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp b/src/platforms/common/px4_work_queue/WorkQueueManager.hpp index d53813f8ee..e9259e2428 100644 --- a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp +++ b/src/platforms/common/px4_work_queue/WorkQueueManager.hpp @@ -50,12 +50,12 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI1{"wq:SPI1", 1250, -1}; -static constexpr wq_config_t SPI2{"wq:SPI2", 1250, -2}; -static constexpr wq_config_t SPI3{"wq:SPI3", 1250, -3}; -static constexpr wq_config_t SPI4{"wq:SPI4", 1250, -4}; -static constexpr wq_config_t SPI5{"wq:SPI5", 1250, -5}; -static constexpr wq_config_t SPI6{"wq:SPI6", 1250, -6}; +static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1}; +static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2}; +static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3}; +static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4}; +static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -5}; +static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -6}; static constexpr wq_config_t I2C1{"wq:I2C1", 1250, -7}; static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8}; diff --git a/src/systemcmds/led_control/led_control.cpp b/src/systemcmds/led_control/led_control.cpp index 60e987c88b..bd07fbb2cf 100644 --- a/src/systemcmds/led_control/led_control.cpp +++ b/src/systemcmds/led_control/led_control.cpp @@ -45,9 +45,10 @@ #include #include -static void usage(); +#include +#include -static orb_advert_t led_control_pub = nullptr; +static void usage(); extern "C" { __EXPORT int led_control_main(int argc, char *argv[]); @@ -57,12 +58,8 @@ static void publish_led_control(led_control_s &led_control) { led_control.timestamp = hrt_absolute_time(); - if (led_control_pub == nullptr) { - led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(led_control), led_control_pub, &led_control); - } + uORB::PublicationQueued led_control_pub{ORB_ID(led_control)}; + led_control_pub.publish(led_control); } static void run_led_test1() diff --git a/src/systemcmds/motor_test/CMakeLists.txt b/src/systemcmds/motor_test/CMakeLists.txt index 27605cef54..d2937e6a01 100644 --- a/src/systemcmds/motor_test/CMakeLists.txt +++ b/src/systemcmds/motor_test/CMakeLists.txt @@ -36,7 +36,7 @@ px4_add_module( STACK_MAIN 4096 COMPILE_FLAGS SRCS - motor_test.c + motor_test.cpp DEPENDS ) diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.cpp similarity index 86% rename from src/systemcmds/motor_test/motor_test.c rename to src/systemcmds/motor_test/motor_test.cpp index 76b2e1e150..0426316212 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.cpp @@ -38,48 +38,30 @@ * Tool for drive testing */ +#include #include #include #include #include - -#include -#include -#include -#include - -#include -#include +#include #include - - - -__EXPORT int motor_test_main(int argc, char *argv[]); +extern "C" __EXPORT int motor_test_main(int argc, char *argv[]); static void motor_test(unsigned channel, float value); static void usage(const char *reason); -static orb_advert_t _test_motor_pub = NULL; - void motor_test(unsigned channel, float value) { - struct test_motor_s _test_motor; + test_motor_s test_motor{}; + test_motor.timestamp = hrt_absolute_time(); + test_motor.motor_number = channel; + test_motor.value = value; - _test_motor.motor_number = channel; - _test_motor.timestamp = hrt_absolute_time(); - _test_motor.value = value; + uORB::PublicationQueued test_motor_pub{ORB_ID(test_motor)}; + test_motor_pub.publish(test_motor); - if (_test_motor_pub != NULL) { - /* publish test state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise_queue(ORB_ID(test_motor), &_test_motor, 4); - } - - printf("motor %d set to %.2f\n", channel, (double)value); + PX4_INFO("motor %d set to %.2f", channel, (double)value); } static void usage(const char *reason)