From 15abb159a8b80ca7b56a9534c6317cd0f17bd45f Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Thu, 15 Nov 2018 10:51:33 +0100 Subject: [PATCH] camera_capture: change topic namings to make logging, mavlink streaming and geotagging easier --- msg/camera_trigger.msg | 2 +- src/drivers/camera_capture/camera_capture.cpp | 4 ++-- src/drivers/camera_trigger/camera_trigger.cpp | 20 +++++++++++++++++-- .../camera_feedback/camera_feedback.cpp | 16 +++------------ src/modules/logger/logger.cpp | 1 + 5 files changed, 25 insertions(+), 18 deletions(-) diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg index b61d331c31..6562874c36 100644 --- a/msg/camera_trigger.msg +++ b/msg/camera_trigger.msg @@ -4,4 +4,4 @@ uint64 timestamp_utc # UTC timestamp uint32 seq # Image sequence number bool feedback # Trigger feedback from camera -# TOPICS camera_trigger camera_trigger_feedback \ No newline at end of file +# TOPICS camera_trigger camera_trigger_secondary \ No newline at end of file diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 8b15367787..1d4d460df6 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -158,11 +158,11 @@ CameraCapture::publish_trigger() if (_trigger_pub == nullptr) { - _trigger_pub = orb_advertise(ORB_ID(camera_trigger_feedback), &trigger); + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); } else { - orb_publish(ORB_ID(camera_trigger_feedback), _trigger_pub, &trigger); + orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); } } diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index f8a9da7d72..a4b61eb703 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -183,8 +183,10 @@ private: param_t _p_interval; param_t _p_distance; param_t _p_interface; + param_t _p_cam_cap_fback; trigger_mode_t _trigger_mode; + int32_t _cam_cap_fback; camera_interface_mode_t _camera_interface_mode; CameraInterface *_camera_interface; ///< instance of camera interface @@ -251,6 +253,7 @@ CameraTrigger::CameraTrigger() : _trigger_pub(nullptr), _cmd_ack_pub(nullptr), _trigger_mode(TRIGGER_MODE_NONE), + _cam_cap_fback(0), _camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO), _camera_interface(nullptr) { @@ -269,12 +272,14 @@ CameraTrigger::CameraTrigger() : _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); _p_interface = param_find("TRIG_INTERFACE"); + _p_cam_cap_fback = param_find("CAM_CAP_FBACK"); param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); param_get(_p_distance, &_distance); param_get(_p_mode, (int32_t *)&_trigger_mode); param_get(_p_interface, (int32_t *)&_camera_interface_mode); + param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback); switch (_camera_interface_mode) { #ifdef __PX4_NUTTX @@ -315,7 +320,13 @@ CameraTrigger::CameraTrigger() : // Advertise critical publishers here, because we cannot advertise in interrupt context struct camera_trigger_s trigger = {}; - _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); + + if (!_cam_cap_fback) { + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); + + } else { + _trigger_pub = orb_advertise(ORB_ID(camera_trigger_secondary), &trigger); + } } @@ -763,7 +774,12 @@ CameraTrigger::engage(void *arg) trigger.seq = trig->_trigger_seq; trigger.feedback = false; - orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); + if (!trig->_cam_cap_fback) { + orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); + + } else { + orb_publish(ORB_ID(camera_trigger_secondary), trig->_trigger_pub, &trigger); + } // increment frame count trig->_trigger_seq++; diff --git a/src/modules/camera_feedback/camera_feedback.cpp b/src/modules/camera_feedback/camera_feedback.cpp index 224d7af728..4e3fb0ce9f 100644 --- a/src/modules/camera_feedback/camera_feedback.cpp +++ b/src/modules/camera_feedback/camera_feedback.cpp @@ -122,12 +122,7 @@ CameraFeedback::stop() void CameraFeedback::task_main() { - if (!_camera_capture_feedback) { - _trigger_sub = orb_subscribe(ORB_ID(camera_trigger)); - - } else { - _trigger_sub = orb_subscribe(ORB_ID(camera_trigger_feedback)); - } + _trigger_sub = orb_subscribe(ORB_ID(camera_trigger)); // Polling sources struct camera_trigger_s trig = {}; @@ -157,12 +152,7 @@ CameraFeedback::task_main() /* trigger subscription updated */ if (fds[0].revents & POLLIN) { - if (!_camera_capture_feedback) { - orb_copy(ORB_ID(camera_trigger), _trigger_sub, &trig); - - } else { - orb_copy(ORB_ID(camera_trigger_feedback), _trigger_sub, &trig); - } + orb_copy(ORB_ID(camera_trigger), _trigger_sub, &trig); /* update geotagging subscriptions */ orb_check(_gpos_sub, &updated); @@ -213,7 +203,7 @@ CameraFeedback::task_main() capture.q[3] = att.q[3]; - // Indicate that whether capture feedback from camera is available + // Indicate whether capture feedback from camera is available // What is case 0 for capture.result? if (!_camera_capture_feedback) { capture.result = -1; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ae7a2b0040..68b60042ae 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -621,6 +621,7 @@ void Logger::add_default_topics() add_topic("battery_status", 500); add_topic("camera_capture"); add_topic("camera_trigger"); + add_topic("camera_trigger_secondary"); add_topic("cpuload"); add_topic("distance_sensor", 100); add_topic("ekf2_innovations", 200);