From 4bf9344913cb830eca261cd2efc6bd299ec2f9d6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 1 Aug 2019 12:24:12 -0400 Subject: [PATCH] camera_trigger: move to new WQ and uORB::Subscription --- src/drivers/camera_trigger/camera_trigger.cpp | 212 +++++++----------- 1 file changed, 83 insertions(+), 129 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index a4b61eb703..a5b1186069 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -50,12 +50,12 @@ #include #include #include -#include +#include #include #include #include -#include +#include #include #include #include @@ -91,7 +91,7 @@ typedef enum : int32_t { #define commandParamToInt(n) static_cast(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(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(arg); // Trigger the camera @@ -783,7 +738,6 @@ CameraTrigger::engage(void *arg) // increment frame count trig->_trigger_seq++; - } void