send_event: move to new WQ and uORB::Subscription

Author: dagar

src/modules/events/CMakeLists.txt

@@ -41,7 +41,6 @@ px4_add_module(
 		send_event.cpp
 		set_leds.cpp
 		status_display.cpp
-		subscriber_handler.cpp
 		temperature_calibration/accel.cpp
 		temperature_calibration/baro.cpp
 		temperature_calibration/gyro.cpp

src/modules/events/rc_loss_alarm.cpp

@@ -52,40 +52,27 @@ namespace events
 namespace rc_loss
 {
 
-RC_Loss_Alarm::RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler)
-	: _subscriber_handler(subscriber_handler)
-{
-}
-
-bool RC_Loss_Alarm::check_for_updates()
-{
-	if (_subscriber_handler.vehicle_status_updated()) {
-		orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status);
-		return true;
-	}
-
-	return false;
-}
-
 void RC_Loss_Alarm::process()
 {
-	if (!check_for_updates()) {
+	vehicle_status_s status{};
+
+	if (!_vehicle_status_sub.update(&status)) {
 		return;
 	}
 
 	if (!_was_armed &&
-	    _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+	    status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
 		_was_armed = true;	// Once true, impossible to go back to false
 	}
 
-	if (!_had_rc && !_vehicle_status.rc_signal_lost) {
+	if (!_had_rc && !status.rc_signal_lost) {
 
 		_had_rc = true;
 	}
 
-	if (_was_armed && _had_rc && _vehicle_status.rc_signal_lost &&
-	    _vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+	if (_was_armed && _had_rc && status.rc_signal_lost &&
+	    status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 		play_tune();
 		_alarm_playing = true;
 

src/modules/events/rc_loss_alarm.h

@@ -39,9 +39,7 @@
 
 #pragma once
 
-#include "subscriber_handler.h"
-
-#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
 
 namespace events
@@ -53,7 +51,7 @@ class RC_Loss_Alarm
 {
 public:
 
-	RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler);
+	RC_Loss_Alarm() = default;
 
 	/** regularily called to handle state updates */
 	void process();
@@ -71,12 +69,12 @@ class RC_Loss_Alarm
 	/** Publish tune control to interrupt any sound */
 	void stop_tune();
 
-	struct vehicle_status_s	_vehicle_status = {};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
 	bool 		_was_armed = false;
 	bool 		_had_rc = false;  // Don't trigger alarm for systems without RC
 	bool		_alarm_playing = false;
 	orb_advert_t 	_tune_control_pub = nullptr;
-	const events::SubscriberHandler &_subscriber_handler;
 };
 
 } /* namespace rc_loss */

src/modules/events/send_event.cpp

@@ -40,47 +40,48 @@
 #include <px4_log.h>
 #include <drivers/drv_hrt.h>
 
+using namespace time_literals;
+
 namespace events
 {
 
-struct work_s SendEvent::_work = {};
-
 // Run it at 30 Hz.
-const unsigned SEND_EVENT_INTERVAL_US = 33000;
-
+static constexpr uint32_t SEND_EVENT_INTERVAL_US{1_s / 30};
 
 int SendEvent::task_spawn(int argc, char *argv[])
 {
-	int ret = work_queue(LPWORK, &_work, (worker_t)&SendEvent::initialize_trampoline, nullptr, 0);
-
-	if (ret < 0) {
-		return ret;
-	}
-
-	ret = wait_until_running();
+	SendEvent *send_event = new SendEvent();
 
-	if (ret < 0) {
-		return ret;
+	if (!send_event) {
+		PX4_ERR("alloc failed");
+		return PX4_ERROR;
 	}
 
+	_object.store(send_event);
 	_task_id = task_id_is_work_queue;
 
+	send_event->start();
+
 	return 0;
 }
 
-SendEvent::SendEvent() : ModuleParams(nullptr)
+SendEvent::SendEvent() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(px4::wq_configurations::lp_default)
 {
 	if (_param_ev_tsk_stat_dis.get()) {
-		_status_display = new status::StatusDisplay(_subscriber_handler);
+		_status_display = new status::StatusDisplay();
 	}
 
 	if (_param_ev_tsk_rc_loss.get()) {
-		_rc_loss_alarm = new rc_loss::RC_Loss_Alarm(_subscriber_handler);
+		_rc_loss_alarm = new rc_loss::RC_Loss_Alarm();
 	}
 }
 
 SendEvent::~SendEvent()
 {
+	ScheduleClear();
+
 	if (_status_display != nullptr) {
 		delete _status_display;
 	}
@@ -92,49 +93,18 @@ SendEvent::~SendEvent()
 
 int SendEvent::start()
 {
-	if (is_running()) {
-		return 0;
-	}
-
-	// Subscribe to the topics.
-	_subscriber_handler.subscribe();
-
-	// Kick off the cycling. We can call it directly because we're already in the work queue context.
-	cycle();
-
-	return 0;
-}
-
-void SendEvent::initialize_trampoline(void *arg)
-{
-	SendEvent *send_event = new SendEvent();
-
-	if (!send_event) {
-		PX4_ERR("alloc failed");
-		return;
-	}
+	ScheduleOnInterval(SEND_EVENT_INTERVAL_US, 10000);
 
-	send_event->start();
-	_object.store(send_event);
+	return PX4_OK;
 }
 
-void SendEvent::cycle_trampoline(void *arg)
-{
-	SendEvent *obj = reinterpret_cast<SendEvent *>(arg);
-
-	obj->cycle();
-}
-
-void SendEvent::cycle()
+void SendEvent::Run()
 {
 	if (should_exit()) {
-		_subscriber_handler.unsubscribe();
 		exit_and_cleanup();
 		return;
 	}
 
-	_subscriber_handler.check_for_updates();
-
 	process_commands();
 
 	if (_status_display != nullptr) {
@@ -144,21 +114,16 @@ void SendEvent::cycle()
 	if (_rc_loss_alarm != nullptr) {
 		_rc_loss_alarm->process();
 	}
-
-	work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this,
-		   USEC2TICK(SEND_EVENT_INTERVAL_US));
 }
 
 void SendEvent::process_commands()
 {
-	if (!_subscriber_handler.vehicle_command_updated()) {
+	vehicle_command_s cmd{};
+
+	if (!_vehicle_command_sub.update(&cmd)) {
 		return;
 	}
 
-	struct vehicle_command_s cmd;
-
-	orb_copy(ORB_ID(vehicle_command), _subscriber_handler.get_vehicle_command_sub(), &cmd);
-
 	bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false;
 
 	switch (cmd.command) {

src/modules/events/send_event.h

@@ -33,11 +33,10 @@
 
 #pragma once
 
-#include "subscriber_handler.h"
 #include "status_display.h"
 #include "rc_loss_alarm.h"
 
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_module.h>
 #include <px4_module_params.h>
 #include <uORB/topics/vehicle_command.h>
@@ -49,7 +48,7 @@ namespace events
 extern "C" __EXPORT int send_event_main(int argc, char *argv[]);
 
 /** @class SendEvent The SendEvent class manages the RC loss audible alarm, LED status display, and thermal calibration. */
-class SendEvent : public ModuleBase<SendEvent>, public ModuleParams
+class SendEvent : public ModuleBase<SendEvent>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 
@@ -92,22 +91,10 @@ class SendEvent : public ModuleBase<SendEvent>, public ModuleParams
 	 */
 	void answer_command(const vehicle_command_s &cmd, unsigned result);
 
-	/**
-	 * @brief Process cycle trampoline for the work queue.
-	 * @param arg Pointer to the task startup arguments.
-	 */
-	static void cycle_trampoline(void *arg);
-
 	/**
 	 * @brief Calls process_commands() and schedules the next cycle.
 	 */
-	void cycle();
-
-	/**
-	 * @brief Trampoline for initialisation.
-	 * @param arg Pointer to the task startup arguments.
-	 */
-	static void initialize_trampoline(void *arg);
+	void Run() override;
 
 	/**
 	 * @brief Checks for new commands and processes them.
@@ -120,11 +107,7 @@ class SendEvent : public ModuleBase<SendEvent>, public ModuleParams
 	 */
 	int start();
 
-	/** @struct _work The work queue struct. */
-	static struct work_s _work;
-
-	/** @var _subscriber_handler The uORB subscriber handler. */
-	SubscriberHandler _subscriber_handler;
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	/** @var _status_display Pointer to the status display object. */
 	status::StatusDisplay *_status_display = nullptr;

src/modules/events/status_display.cpp

@@ -48,8 +48,7 @@ namespace events
 namespace status
 {
 
-StatusDisplay::StatusDisplay(const events::SubscriberHandler &subscriber_handler)
-	: _subscriber_handler(subscriber_handler)
+StatusDisplay::StatusDisplay()
 {
 	// set the base color
 	_led_control.priority = 0;
@@ -66,23 +65,19 @@ bool StatusDisplay::check_for_updates()
 {
 	bool got_updates = false;
 
-	if (_subscriber_handler.battery_status_updated()) {
-		orb_copy(ORB_ID(battery_status), _subscriber_handler.get_battery_status_sub(), &_battery_status);
+	if (_battery_status_sub.update()) {
 		got_updates = true;
 	}
 
-	if (_subscriber_handler.cpuload_updated()) {
-		orb_copy(ORB_ID(cpuload), _subscriber_handler.get_cpuload_sub(), &_cpu_load);
+	if (_cpu_load_sub.update()) {
 		got_updates = true;
 	}
 
-	if (_subscriber_handler.vehicle_status_flags_updated()) {
-		orb_copy(ORB_ID(vehicle_status_flags), _subscriber_handler.get_vehicle_status_flags_sub(), &_vehicle_status_flags);
+	if (_vehicle_status_flags_sub.update()) {
 		got_updates = true;
 	}
 
-	if (_subscriber_handler.vehicle_status_updated()) {
-		orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status);
+	if (_vehicle_status_sub.update()) {
 		got_updates = true;
 	}
 

src/modules/events/status_display.h

@@ -40,14 +40,13 @@
 
 #pragma once
 
-#include "subscriber_handler.h"
-
 #include <drivers/drv_hrt.h>
 
-#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/led_control.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
 
@@ -60,7 +59,7 @@ class StatusDisplay
 {
 public:
 
-	StatusDisplay(const events::SubscriberHandler &subscriber_handler);
+	StatusDisplay();
 
 	/** regularily called to handle state updates */
 	void process();
@@ -81,11 +80,11 @@ class StatusDisplay
 	void publish();
 
 	// TODO: review if there is a better variant that allocates this in the memory
-	struct battery_status_s _battery_status = {};
-	struct cpuload_s _cpu_load = {};
-	struct vehicle_status_s _vehicle_status = {};
-	struct vehicle_status_flags_s _vehicle_status_flags = {};
-	struct vehicle_attitude_s _vehicle_attitude = {};
+	uORB::SubscriptionData<battery_status_s> _battery_status_sub{ORB_ID(battery_status)};
+	uORB::SubscriptionData<cpuload_s> _cpu_load_sub{ORB_ID(cpuload)};
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<vehicle_status_flags_s> _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
+	uORB::SubscriptionData<vehicle_attitude_s> _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 
 	struct led_control_s _led_control = {};
 
@@ -97,7 +96,6 @@ class StatusDisplay
 	int _old_nav_state = -1;
 	int _old_battery_status_warning = -1;
 	orb_advert_t _led_control_pub = nullptr;
-	const events::SubscriberHandler &_subscriber_handler;
 };
 
 } /* namespace status */