camera_trigger: move to new WQ and uORB::Subscription

src/drivers/camera_trigger/camera_trigger.cpp

@@ -50,12 +50,12 @@
 #include <poll.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <systemlib/err.h>
 #include <parameters/param.h>
 #include <systemlib/mavlink_log.h>
 
-#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/camera_capture.h>
 #include <uORB/topics/sensor_combined.h>
@@ -91,7 +91,7 @@ typedef enum : int32_t {
 
 #define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
 
-class CameraTrigger
+class CameraTrigger : public px4::ScheduledWorkItem
 {
 public:
 	/**
@@ -127,7 +127,7 @@ public:
 	/**
 	 * Toggle camera power (on/off)
 	 */
-	void        toggle_power();
+	void        	toggle_power();
 
 	/**
 	 * Start the task.
@@ -158,8 +158,6 @@ private:
 	struct hrt_call		_keepalivecall_up;
 	struct hrt_call		_keepalivecall_down;
 
-	static struct work_s	_work;
-
 	float			_activation_time;
 	float			_interval;
 	float 			_distance;
@@ -172,8 +170,8 @@ private:
 	matrix::Vector2f	_last_shoot_position;
 	bool			_valid_position;
 
-	int			_command_sub;
-	int			_lpos_sub;
+	uORB::Subscription	_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription	_lpos_sub{ORB_ID(vehicle_local_position)};
 
 	orb_advert_t		_trigger_pub;
 	orb_advert_t		_cmd_ack_pub;
@@ -194,23 +192,28 @@ private:
 	/**
 	 * Vehicle command handler
 	 */
-	static void	cycle_trampoline(void *arg);
+	void		Run();
+
 	/**
 	 * Fires trigger
 	 */
 	static void	engage(void *arg);
+
 	/**
 	 * Resets trigger
 	 */
 	static void	disengage(void *arg);
+
 	/**
 	 * Fires on/off
 	 */
 	static void engange_turn_on_off(void *arg);
+
 	/**
 	 * Resets  on/off
 	 */
 	static void disengage_turn_on_off(void *arg);
+
 	/**
 	 * Enables keep alive signal
 	 */
@@ -222,15 +225,13 @@ private:
 
 };
 
-struct work_s CameraTrigger::_work;
-
 namespace camera_trigger
 {
-
 CameraTrigger	*g_camera_trigger;
 }
 
 CameraTrigger::CameraTrigger() :
+	ScheduledWorkItem(px4::wq_configurations::lp_default),
 	_engagecall {},
 	_disengagecall {},
 	_engage_turn_on_off_call {},
@@ -248,8 +249,6 @@ CameraTrigger::CameraTrigger() :
 	_turning_on(false),
 	_last_shoot_position(0.0f, 0.0f),
 	_valid_position(false),
-	_command_sub(-1),
-	_lpos_sub(-1),
 	_trigger_pub(nullptr),
 	_cmd_ack_pub(nullptr),
 	_trigger_mode(TRIGGER_MODE_NONE),
@@ -264,8 +263,6 @@ CameraTrigger::CameraTrigger() :
 		_camera_interface = nullptr;
 	}
 
-	memset(&_work, 0, sizeof(_work));
-
 	// Parameters
 	_p_interval = param_find("TRIG_INTERVAL");
 	_p_distance = param_find("TRIG_DISTANCE");
@@ -327,7 +324,6 @@ CameraTrigger::CameraTrigger() :
 	} else {
 		_trigger_pub = orb_advertise(ORB_ID(camera_trigger_secondary), &trigger);
 	}
-
 }
 
 CameraTrigger::~CameraTrigger()
@@ -340,7 +336,6 @@ CameraTrigger::~CameraTrigger()
 void
 CameraTrigger::update_intervalometer()
 {
-
 	// the actual intervalometer runs in interrupt context, so we only need to call
 	// control_intervalometer once on enabling/disabling trigger to schedule the calls.
 
@@ -354,25 +349,18 @@ CameraTrigger::update_intervalometer()
 			       (hrt_callout)&CameraTrigger::disengage, this);
 
 	}
-
 }
 
 void
 CameraTrigger::update_distance()
 {
-
-	if (_lpos_sub < 0) {
-		_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	}
-
 	if (_turning_on) {
 		return;
 	}
 
 	if (_trigger_enabled) {
-
-		struct vehicle_local_position_s local = {};
-		orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &local);
+		vehicle_local_position_s local{};
+		_lpos_sub.copy(&local);
 
 		if (local.xy_valid) {
 
@@ -412,13 +400,11 @@ CameraTrigger::enable_keep_alive(bool on)
 		hrt_cancel(&_keepalivecall_up);
 		hrt_cancel(&_keepalivecall_down);
 	}
-
 }
 
 void
 CameraTrigger::toggle_power()
 {
-
 	// schedule power toggle calls
 	hrt_call_after(&_engage_turn_on_off_call, 0,
 		       (hrt_callout)&CameraTrigger::engange_turn_on_off, this);
@@ -431,7 +417,6 @@ void
 CameraTrigger::shoot_once()
 {
 	if (!_trigger_paused) {
-
 		// schedule trigger on and off calls
 		hrt_call_after(&_engagecall, 0,
 			       (hrt_callout)&CameraTrigger::engage, this);
@@ -439,13 +424,11 @@ CameraTrigger::shoot_once()
 		hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
 			       (hrt_callout)&CameraTrigger::disengage, this);
 	}
-
 }
 
 void
 CameraTrigger::start()
 {
-
 	if ((_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON ||
 	     _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) &&
 	    _camera_interface->has_power_control() &&
@@ -471,14 +454,14 @@ CameraTrigger::start()
 	}
 
 	// start to monitor at high rate for trigger enable command
-	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
-
+	ScheduleNow();
 }
 
 void
 CameraTrigger::stop()
 {
-	work_cancel(LPWORK, &_work);
+	ScheduleClear();
+
 	hrt_cancel(&_engagecall);
 	hrt_cancel(&_disengagecall);
 	hrt_cancel(&_engage_turn_on_off_call);
@@ -503,51 +486,38 @@ CameraTrigger::test()
 }
 
 void
-CameraTrigger::cycle_trampoline(void *arg)
+CameraTrigger::Run()
 {
-
-	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
-
 	// default loop polling interval
 	int poll_interval_usec = 5000;
 
-	if (trig->_command_sub < 0) {
-		trig->_command_sub = orb_subscribe(ORB_ID(vehicle_command));
-	}
-
-	bool updated = false;
-	orb_check(trig->_command_sub, &updated);
-
-	struct vehicle_command_s cmd;
+	vehicle_command_s cmd{};
 	unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 	bool need_ack = false;
 
 	// this flag is set when the polling loop is slowed down to allow the camera to power on
-	trig->_turning_on = false;
+	_turning_on = false;
 
 	// these flags are used to detect state changes in the command loop
-	bool main_state = trig->_trigger_enabled;
-	bool pause_state = trig->_trigger_paused;
+	bool main_state = _trigger_enabled;
+	bool pause_state = _trigger_paused;
+
+	bool updated = _command_sub.update(&cmd);
 
 	// Command handling
 	if (updated) {
-
-		orb_copy(ORB_ID(vehicle_command), trig->_command_sub, &cmd);
-
 		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
 
 			need_ack = true;
 
 			if (commandParamToInt(cmd.param7) == 1) {
 				// test shots are not logged or forwarded to GCS for geotagging
-				trig->_test_shot = true;
-
+				_test_shot = true;
 			}
 
 			if (commandParamToInt((float)cmd.param5) == 1) {
 				// Schedule shot
-				trig->_one_shot = true;
-
+				_one_shot = true;
 			}
 
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -558,25 +528,22 @@ CameraTrigger::cycle_trampoline(void *arg)
 
 			if (commandParamToInt(cmd.param3) == 1) {
 				// pause triggger
-				trig->_trigger_paused = true;
+				_trigger_paused = true;
 
 			} else if (commandParamToInt(cmd.param3) == 0) {
-				trig->_trigger_paused = false;
-
+				_trigger_paused = false;
 			}
 
 			if (commandParamToInt(cmd.param2) == 1) {
 				// reset trigger sequence
-				trig->_trigger_seq = 0;
-
+				_trigger_seq = 0;
 			}
 
 			if (commandParamToInt(cmd.param1) == 1) {
-				trig->_trigger_enabled = true;
+				_trigger_enabled = true;
 
 			} else if (commandParamToInt(cmd.param1) == 0) {
-				trig->_trigger_enabled = false;
-
+				_trigger_enabled = false;
 			}
 
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -590,31 +557,31 @@ CameraTrigger::cycle_trampoline(void *arg)
 			*/
 
 			if (cmd.param1 > 0.0f) {
-				trig->_distance = cmd.param1;
-				param_set_no_notification(trig->_p_distance, &(trig->_distance));
+				_distance = cmd.param1;
+				param_set_no_notification(_p_distance, &_distance);
 
-				trig->_trigger_enabled = true;
-				trig->_trigger_paused = false;
+				_trigger_enabled = true;
+				_trigger_paused = false;
 
 			} else if (commandParamToInt(cmd.param1) == 0) {
-				trig->_trigger_paused = true;
+				_trigger_paused = true;
 
 			} else if (commandParamToInt(cmd.param1) == -1) {
-				trig->_trigger_enabled = false;
+				_trigger_enabled = false;
 			}
 
 			// We can only control the shutter integration time of the camera in GPIO mode (for now)
 			if (cmd.param2 > 0.0f) {
-				if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
-					trig->_activation_time = cmd.param2;
-					param_set_no_notification(trig->_p_activation_time, &(trig->_activation_time));
+				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
+					_activation_time = cmd.param2;
+					param_set_no_notification(_p_activation_time, &(_activation_time));
 				}
 			}
 
 			// Trigger once immediately if param is set
 			if (cmd.param3 > 0.0f) {
 				// Schedule shot
-				trig->_one_shot = true;
+				_one_shot = true;
 			}
 
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -624,103 +591,93 @@ CameraTrigger::cycle_trampoline(void *arg)
 			need_ack = true;
 
 			if (cmd.param1 > 0.0f) {
-				trig->_interval = cmd.param1;
-				param_set_no_notification(trig->_p_interval, &(trig->_interval));
+				_interval = cmd.param1;
+				param_set_no_notification(_p_interval, &(_interval));
 			}
 
 			// We can only control the shutter integration time of the camera in GPIO mode
 			if (cmd.param2 > 0.0f) {
-				if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
-					trig->_activation_time = cmd.param2;
-					param_set_no_notification(trig->_p_activation_time, &(trig->_activation_time));
+				if (_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
+					_activation_time = cmd.param2;
+					param_set_no_notification(_p_activation_time, &_activation_time);
 				}
 			}
 
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
 		}
-
 	}
 
 	// State change handling
-	if ((main_state != trig->_trigger_enabled) ||
-	    (pause_state != trig->_trigger_paused) ||
-	    trig->_one_shot) {
+	if ((main_state != _trigger_enabled) ||
+	    (pause_state != _trigger_paused) ||
+	    _one_shot) {
 
-		if (trig->_trigger_enabled || trig->_one_shot) { // Just got enabled via a command
+		if (_trigger_enabled || _one_shot) { // Just got enabled via a command
 
 			// If camera isn't already powered on, we enable power to it
-			if (!trig->_camera_interface->is_powered_on() &&
-			    trig->_camera_interface->has_power_control()) {
+			if (!_camera_interface->is_powered_on() &&
+			    _camera_interface->has_power_control()) {
 
-				trig->toggle_power();
-				trig->enable_keep_alive(true);
+				toggle_power();
+				enable_keep_alive(true);
 
 				// Give the camera time to turn on before starting to send trigger signals
 				poll_interval_usec = 3000000;
-				trig->_turning_on = true;
+				_turning_on = true;
 			}
 
-		} else if (!trig->_trigger_enabled || trig->_trigger_paused) { // Just got disabled/paused via a command
+		} else if (!_trigger_enabled || _trigger_paused) { // Just got disabled/paused via a command
 
 			// Power off the camera if we are disabled
-			if (trig->_camera_interface->is_powered_on() &&
-			    trig->_camera_interface->has_power_control() &&
-			    !trig->_trigger_enabled) {
+			if (_camera_interface->is_powered_on() &&
+			    _camera_interface->has_power_control() &&
+			    !_trigger_enabled) {
 
-				trig->enable_keep_alive(false);
-				trig->toggle_power();
+				enable_keep_alive(false);
+				toggle_power();
 			}
 
 			// cancel all calls for both disabled and paused
-			hrt_cancel(&trig->_engagecall);
-			hrt_cancel(&trig->_disengagecall);
+			hrt_cancel(&_engagecall);
+			hrt_cancel(&_disengagecall);
 
 			// ensure that the pin is off
-			hrt_call_after(&trig->_disengagecall, 0,
-				       (hrt_callout)&CameraTrigger::disengage, trig);
+			hrt_call_after(&_disengagecall, 0, (hrt_callout)&CameraTrigger::disengage, this);
 
 			// reset distance counter if needed
-			if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
-			    trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
+			if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
+			    _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
 
 				// this will force distance counter reinit on getting enabled/unpaused
-				trig->_valid_position = false;
-
+				_valid_position = false;
 			}
-
 		}
 
 		// only run on state changes, not every loop iteration
-		if (trig->_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
-
+		if (_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
 			// update intervalometer state and reset calls
-			trig->update_intervalometer();
-
+			update_intervalometer();
 		}
-
 	}
 
 	// run every loop iteration and trigger if needed
-	if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
-	    trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
+	if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
+	    _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
 
 		// update distance counter and trigger
-		trig->update_distance();
-
+		update_distance();
 	}
 
 	// One shot command-based capture handling
-	if (trig->_one_shot && !trig->_turning_on) {
+	if (_one_shot && !_turning_on) {
 
 		// One-shot trigger
-		trig->shoot_once();
-		trig->_one_shot = false;
+		shoot_once();
+		_one_shot = false;
 
-		if (trig->_test_shot) {
-			trig->_test_shot = false;
+		if (_test_shot) {
+			_test_shot = false;
 		}
-
 	}
 
 	// Command ACK handling
@@ -732,23 +689,21 @@ CameraTrigger::cycle_trampoline(void *arg)
 		command_ack.target_system = cmd.source_system;
 		command_ack.target_component = cmd.source_component;
 
-		if (trig->_cmd_ack_pub == nullptr) {
-			trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-					     vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+		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), trig->_cmd_ack_pub, &command_ack);
+			orb_publish(ORB_ID(vehicle_command_ack), _cmd_ack_pub, &command_ack);
 		}
 	}
 
-	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
-		   camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
+	ScheduleDelayed(poll_interval_usec);
 }
 
 void
 CameraTrigger::engage(void *arg)
 {
-
 	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
 
 	// Trigger the camera
@@ -783,7 +738,6 @@ CameraTrigger::engage(void *arg)
 
 	// increment frame count
 	trig->_trigger_seq++;
-
 }
 
 void