From 96961c6f9c07123ec1dba3b0fcda084fa753fe63 Mon Sep 17 00:00:00 2001
From: DanielePettenuzzo <daniele@px4.io>
Date: Tue, 18 Sep 2018 09:51:43 +0200
Subject: [PATCH] camera trigger and camera capture publish on two different
topics and camera feedback module uses CAM_CAP_FBACK param to choose between
the two.
---
msg/camera_trigger.msg | 2 ++
src/drivers/camera_capture/camera_capture.cpp | 14 ++++----------
src/drivers/camera_capture/camera_capture.hpp | 2 --
src/drivers/camera_trigger/camera_trigger.cpp | 15 ++++-----------
src/modules/camera_feedback/camera_feedback.cpp | 3 ++-
5 files changed, 12 insertions(+), 24 deletions(-)
diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg
index 6c3ffdeb94..b61d331c31 100644
--- a/msg/camera_trigger.msg
+++ b/msg/camera_trigger.msg
@@ -3,3 +3,5 @@ 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
diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp
index 7a9e0edd8a..8aed49cbef 100644
--- a/src/drivers/camera_capture/camera_capture.cpp
+++ b/src/drivers/camera_capture/camera_capture.cpp
@@ -54,7 +54,6 @@ CameraCapture::CameraCapture() :
_command_ack_pub(nullptr),
_command_sub(-1),
_trig_buffer(nullptr),
- _camera_capture_feedback(false),
_camera_capture_mode(0),
_camera_capture_edge(0),
_capture_seq(0),
@@ -69,9 +68,6 @@ CameraCapture::CameraCapture() :
_p_strobe_delay = param_find("CAM_CAP_DELAY");
param_get(_p_strobe_delay, &_strobe_delay);
- _p_camera_capture_feedback = param_find("CAM_CAP_FBACK");
- param_get(_p_camera_capture_feedback, &_camera_capture_feedback);
-
_p_camera_capture_mode = param_find("CAM_CAP_MODE");
param_get(_p_camera_capture_mode, &_camera_capture_mode);
@@ -137,15 +133,13 @@ CameraCapture::publish_trigger()
trigger.seq = _capture_seq++;
trigger.feedback = true;
- if (_camera_capture_feedback) {
- if (_trigger_pub == nullptr) {
+ if (_trigger_pub == nullptr) {
- _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
+ _trigger_pub = orb_advertise(ORB_ID(camera_trigger_feedback), &trigger);
- } else {
+ } else {
- orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger);
- }
+ orb_publish(ORB_ID(camera_trigger_feedback), _trigger_pub, &trigger);
}
_last_exposure_time = trig.edge_time - _last_fall_time;
diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp
index 812baaf5d8..99798dba5f 100644
--- a/src/drivers/camera_capture/camera_capture.hpp
+++ b/src/drivers/camera_capture/camera_capture.hpp
@@ -127,8 +127,6 @@ private:
// Parameters
param_t _p_strobe_delay;
float _strobe_delay;
- param_t _p_camera_capture_feedback;
- int32_t _camera_capture_feedback;
param_t _p_camera_capture_mode;
int32_t _camera_capture_mode;
param_t _p_camera_capture_edge;
diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index a5db64b3ef..f8a9da7d72 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -171,7 +171,6 @@ private:
bool _turning_on;
matrix::Vector2f _last_shoot_position;
bool _valid_position;
- int32_t _camera_capture_feedback;
int _command_sub;
int _lpos_sub;
@@ -184,7 +183,6 @@ private:
param_t _p_interval;
param_t _p_distance;
param_t _p_interface;
- param_t _p_camera_capture_feedback;
trigger_mode_t _trigger_mode;
@@ -248,7 +246,6 @@ CameraTrigger::CameraTrigger() :
_turning_on(false),
_last_shoot_position(0.0f, 0.0f),
_valid_position(false),
- _camera_capture_feedback(false),
_command_sub(-1),
_lpos_sub(-1),
_trigger_pub(nullptr),
@@ -272,14 +269,12 @@ CameraTrigger::CameraTrigger() :
_p_activation_time = param_find("TRIG_ACT_TIME");
_p_mode = param_find("TRIG_MODE");
_p_interface = param_find("TRIG_INTERFACE");
- _p_camera_capture_feedback = 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_camera_capture_feedback, &_camera_capture_feedback);
switch (_camera_interface_mode) {
#ifdef __PX4_NUTTX
@@ -318,11 +313,9 @@ CameraTrigger::CameraTrigger() :
param_set_no_notification(_p_activation_time, &(_activation_time));
}
- if (!_camera_capture_feedback) {
- // 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);
- }
+ // 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);
}
@@ -750,7 +743,7 @@ CameraTrigger::engage(void *arg)
// Trigger the camera
trig->_camera_interface->trigger(true);
- if (trig->_test_shot || trig->_camera_capture_feedback) {
+ if (trig->_test_shot) {
// do not send messages or increment frame count for test shots
return;
}
diff --git a/src/modules/camera_feedback/camera_feedback.cpp b/src/modules/camera_feedback/camera_feedback.cpp
index ef9996af51..5536b2b976 100644
--- a/src/modules/camera_feedback/camera_feedback.cpp
+++ b/src/modules/camera_feedback/camera_feedback.cpp
@@ -122,8 +122,9 @@ CameraFeedback::stop()
void
CameraFeedback::task_main()
{
- if(!_camera_capture_feedback){
+ if (!_camera_capture_feedback) {
_trigger_sub = orb_subscribe(ORB_ID(camera_trigger));
+
} else {
_trigger_sub = orb_subscribe(ORB_ID(camera_trigger_feedback));
}
--
GitLab