diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e4082a1133..bdee5971d9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -376,6 +376,13 @@ else camera_feedback start fi + # Camera capture driver + if camera_capture start + then + set FMU_MODE pwm4 + set AUX_MODE pwm4 + fi + # # Check if UAVCAN is enabled, default to it for ESCs. # diff --git a/src/drivers/camera_capture/CMakeLists.txt b/src/drivers/camera_capture/CMakeLists.txt new file mode 100644 index 0000000000..c07905b6e2 --- /dev/null +++ b/src/drivers/camera_capture/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__camera_capture + MAIN camera_capture + COMPILE_FLAGS + SRCS + camera_capture.cpp + DEPENDS + platforms__common + EXTERNAL + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp new file mode 100644 index 0000000000..0d3ad9c4c4 --- /dev/null +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -0,0 +1,286 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file camera_capture.cpp + * + * Online and offline geotagging from camera feedback + * + * @author Mohammed Kabir + */ + +#include "camera_capture.hpp" + +#define commandParamToInt(n) static_cast(n >= 0 ? n + 0.5f : n - 0.5f) + +namespace camera_capture +{ +CameraCapture *g_camera_capture; +} + +CameraCapture::CameraCapture() : + _capture_enabled(false), + _trigger_pub(nullptr), + _command_ack_pub(nullptr), + _command_sub(-1), + _capture_seq(0), + _last_fall_time(0), + _last_exposure_time(0), + _capture_overflows(0) +{ + + memset(&_work, 0, sizeof(_work)); + + // Parameters + _p_strobe_delay = param_find("CAM_CAP_DELAY"); + param_get(_p_strobe_delay, &_strobe_delay); + + struct camera_trigger_s trigger = {}; + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); +} + +CameraCapture::~CameraCapture() +{ + camera_capture::g_camera_capture = nullptr; +} + +void +CameraCapture::capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + + if (edge_state == 0) { // Falling edge + // Timestamp and compensate for strobe delay + _last_fall_time = edge_time - uint64_t(1000 * _strobe_delay); + + } else if (edge_state == 1 && _last_fall_time > 0) { // Falling edge and got rising before + struct camera_trigger_s trigger {}; + + trigger.timestamp = edge_time - ((edge_time - _last_fall_time) / 2); // Get timestamp of mid-exposure + trigger.seq = _capture_seq++; + + orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); + + _last_exposure_time = edge_time - _last_fall_time; + } + + _capture_overflows = overflow; + +} + +void +CameraCapture::capture_trampoline(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); +} + +void +CameraCapture::cycle_trampoline(void *arg) +{ + + CameraCapture *cap = reinterpret_cast(arg); + + if (cap->_command_sub < 0) { + cap->_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + } + + bool updated = false; + orb_check(cap->_command_sub, &updated); + + // Command handling + if (updated) { + + vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), cap->_command_sub, &cmd); + + // TODO : this should eventuallly be a capture control command + if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { + + // Enable/disable signal capture + if (commandParamToInt(cmd.param1) == 1) { + cap->set_capture_control(true); + + } else if (commandParamToInt(cmd.param1) == 0) { + cap->set_capture_control(false); + + } + + // Reset capture sequence + if (commandParamToInt(cmd.param2) == 1) { + cap->reset_statistics(true); + + } + + // Acknowledge the command + vehicle_command_ack_s command_ack = { + .timestamp = 0, + .result_param2 = 0, + .command = cmd.command, + .result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, + .from_external = false, + .result_param1 = 0, + .target_system = cmd.source_system, + .target_component = cmd.source_component + }; + + if (cap->_command_ack_pub == nullptr) { + cap->_command_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), cap->_command_ack_pub, &command_ack); + + } + } + + } + + work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture, + USEC2TICK(100000)); // 100ms +} + +void +CameraCapture::set_capture_control(bool enabled) +{ + if (enabled) { + // register callbacks + //up_input_capture_set(4, Both, 0, &CameraCapture::capture_trampoline, this); + up_input_capture_set(5, Both, 0, &CameraCapture::capture_trampoline, this); + _capture_enabled = true; + + } else { + //up_input_capture_set(4, Disabled, 0, NULL, NULL); + up_input_capture_set(5, Disabled, 0, NULL, NULL); + _capture_enabled = false; + } + + reset_statistics(false); +} + +void +CameraCapture::reset_statistics(bool reset_seq) +{ + if (reset_seq) { _capture_seq = 0; } + + _last_fall_time = 0; + _last_exposure_time = 0; + _capture_overflows = 0; +} + +void +CameraCapture::start() +{ + // start to monitor at low rates for capture control commands + work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this, USEC2TICK(1)); // TODO : is this low rate??! +} + +void +CameraCapture::stop() +{ + + work_cancel(LPWORK, &_work); + + if (camera_capture::g_camera_capture != nullptr) { + delete (camera_capture::g_camera_capture); + } +} + +void +CameraCapture::status() +{ + PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO"); + PX4_INFO("Frame sequence : %u", _capture_seq); + PX4_INFO("Last fall timestamp : %llu", _last_fall_time); + PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0); + PX4_INFO("Number of overflows : %u", _capture_overflows); +} + +static int usage() +{ + PX4_INFO("usage: camera_capture {start|stop|on|off|reset}\n"); + return 1; +} + +extern "C" __EXPORT int camera_capture_main(int argc, char *argv[]); + +int camera_capture_main(int argc, char *argv[]) +{ + if (argc < 2) { + return usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (camera_capture::g_camera_capture != nullptr) { + PX4_WARN("already running"); + return 0; + } + + camera_capture::g_camera_capture = new CameraCapture(); + + if (camera_capture::g_camera_capture == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + camera_capture::g_camera_capture->start(); + return 0; + } + + if (camera_capture::g_camera_capture == nullptr) { + PX4_WARN("not running"); + return 1; + + } else if (!strcmp(argv[1], "stop")) { + camera_capture::g_camera_capture->stop(); + + } else if (!strcmp(argv[1], "status")) { + camera_capture::g_camera_capture->status(); + + } else if (!strcmp(argv[1], "on")) { + camera_capture::g_camera_capture->set_capture_control(true); + + } else if (!strcmp(argv[1], "off")) { + camera_capture::g_camera_capture->set_capture_control(false); + + } else if (!strcmp(argv[1], "reset")) { + camera_capture::g_camera_capture->set_capture_control(false); + camera_capture::g_camera_capture->reset_statistics(true); + + } else { + return usage(); + } + + return 0; +} diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp new file mode 100644 index 0000000000..1405f7d696 --- /dev/null +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file camera_capture.hpp + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +class CameraCapture +{ +public: + /** + * Constructor + */ + CameraCapture(); + + /** + * Destructor, also kills task. + */ + ~CameraCapture(); + + /** + * Start the task. + */ + void start(); + + /** + * Stop the task. + */ + void stop(); + + void status(); + + static void capture_trampoline(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, + uint32_t overflow); + + void set_capture_control(bool enabled); + + void reset_statistics(bool reset_seq); + + + static struct work_s _work; + +private: + + bool _capture_enabled; + + // Publishers + orb_advert_t _trigger_pub; + orb_advert_t _command_ack_pub; + + // Subscribers + int _command_sub; + + // Parameters + param_t _p_strobe_delay; + float _strobe_delay; + + // Signal capture statistics + uint32_t _capture_seq; + hrt_abstime _last_fall_time; + hrt_abstime _last_exposure_time; + uint32_t _capture_overflows; + + // Signal capture callback + void capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + + // Low-rate command handling loop + static void cycle_trampoline(void *arg); + +}; +struct work_s CameraCapture::_work; diff --git a/src/drivers/camera_capture/camera_capture_params.c b/src/drivers/camera_capture/camera_capture_params.c new file mode 100644 index 0000000000..b4d5ff1432 --- /dev/null +++ b/src/drivers/camera_capture/camera_capture_params.c @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file camera_capture_params.c + * Camera capture parameters + * + * @author Mohammed Kabir + */ +/** + * Camera strobe delay + * + * This parameter sets the delay between image integration start and strobe firing + * + * @unit ms + * @min 0.0 + * @max 100.0 + * @decimal 1 + * @group Camera Capture + */ +PARAM_DEFINE_FLOAT(CAM_CAP_DELAY, 1.0f); diff --git a/src/drivers/stm32/drv_input_capture.c b/src/drivers/stm32/drv_input_capture.c index c7505087b1..cd748817aa 100644 --- a/src/drivers/stm32/drv_input_capture.c +++ b/src/drivers/stm32/drv_input_capture.c @@ -126,7 +126,7 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, if (channel_handlers[chan_index].callback) { channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index, channel_stats[chan_index].last_time, - channel_stats[chan_index].last_edge, overflow); + channel_stats[chan_index].last_edge, channel_stats[chan_index].overflows); } }