diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 97288db32f..307198d6ce 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -77,7 +77,7 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - if (_camera_capture_feedback != 0) { + if (_camera_capture_feedback) { struct camera_trigger_s trigger = {}; _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); } @@ -97,7 +97,7 @@ CameraCapture::capture_callback(uint32_t chan_index, struct camera_trigger_s trigger {}; if (_camera_capture_mode == 0) { - trigger.timestamp = hrt_absolute_time(); + trigger.timestamp = edge_time; } else { trigger.timestamp = edge_time - ((edge_time - _last_fall_time) / 2); // Get timestamp of mid-exposure @@ -105,7 +105,7 @@ CameraCapture::capture_callback(uint32_t chan_index, trigger.seq = _capture_seq++; - if (_camera_capture_feedback != 0) { + if (_camera_capture_feedback) { orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); } @@ -196,21 +196,12 @@ CameraCapture::set_capture_control(bool enabled) { if (enabled) { // register callbacks - switch (_camera_capture_edge) { - case 0: - up_input_capture_set(5, Rising, 0, &CameraCapture::capture_trampoline, this); - break; - - case 1: + if (!_camera_capture_edge) { up_input_capture_set(5, Falling, 0, &CameraCapture::capture_trampoline, this); - break; - case 2: - up_input_capture_set(5, Both, 0, &CameraCapture::capture_trampoline, this); - break; + } else { + up_input_capture_set(5, Rising, 0, &CameraCapture::capture_trampoline, this); - default: - break; } _capture_enabled = true; diff --git a/src/drivers/camera_capture/camera_capture_params.c b/src/drivers/camera_capture/camera_capture_params.c index 757a007210..f0aa292786 100644 --- a/src/drivers/camera_capture/camera_capture_params.c +++ b/src/drivers/camera_capture/camera_capture_params.c @@ -77,9 +77,8 @@ PARAM_DEFINE_INT32(CAM_CAP_MODE, 0); /** * Camera capture edge * - * @value 0 Rising edge - * @value 1 Falling edge - * @value 2 Both edges + * @value 0 Falling edge + * @value 1 Rising edge * * @group Camera Control * @reboot_required true diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 58e0dd18a1..3762300dbb 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -248,7 +248,7 @@ CameraTrigger::CameraTrigger() : _turning_on(false), _last_shoot_position(0.0f, 0.0f), _valid_position(false), - _camera_capture_feedback(0), + _camera_capture_feedback(false), _command_sub(-1), _lpos_sub(-1), _trigger_pub(nullptr), @@ -318,7 +318,7 @@ CameraTrigger::CameraTrigger() : param_set_no_notification(_p_activation_time, &(_activation_time)); } - if (_camera_capture_feedback == 0) { + 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); @@ -750,7 +750,7 @@ CameraTrigger::engage(void *arg) // Trigger the camera trig->_camera_interface->trigger(true); - if (trig->_test_shot || (trig->_camera_capture_feedback != 0)) { + if (trig->_test_shot || trig->_camera_capture_feedback) { // do not send messages or increment frame count for test shots return; }