diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7ed8babd9a..6c0367f33b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -505,6 +505,9 @@ then set AUX_MODE none fi camera_trigger start + + param set CAM_FBACK_MODE 1 + camera_feedback start fi # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output diff --git a/src/modules/camera_feedback/CMakeLists.txt b/src/modules/camera_feedback/CMakeLists.txt new file mode 100644 index 0000000000..ca2b089103 --- /dev/null +++ b/src/modules/camera_feedback/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 modules__camera_feedback + MAIN camera_feedback + COMPILE_FLAGS + SRCS + camera_feedback.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/camera_feedback/camera_feedback.cpp b/src/modules/camera_feedback/camera_feedback.cpp new file mode 100644 index 0000000000..d11f03fcc0 --- /dev/null +++ b/src/modules/camera_feedback/camera_feedback.cpp @@ -0,0 +1,368 @@ +/**************************************************************************** + * + * 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_feedback.cpp + * + * Online and offline geotagging from camera feedback + * + * @author Mohammed Kabir + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +typedef enum : uint8_t { + CAMERA_FEEDBACK_MODE_NONE = 0, + CAMERA_FEEDBACK_MODE_TRIGGER, + CAMERA_FEEDBACK_MODE_PWM +} camera_feedback_mode_t; + +class CameraFeedback +{ +public: + /** + * Constructor + */ + CameraFeedback(); + + /** + * Destructor, also kills task. + */ + ~CameraFeedback(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Stop the task. + */ + void stop(); + +private: + + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ + + int _trigger_sub; + int _lpos_sub; + int _gpos_sub; + int _att_sub; + + orb_advert_t _capture_pub; + + param_t _p_feedback; + + camera_feedback_mode_t _camera_feedback_mode; + + void task_main(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + +}; + +namespace camera_feedback +{ +CameraFeedback *g_camera_feedback; +} + +CameraFeedback::CameraFeedback() : + _task_should_exit(false), + _main_task(-1), + _trigger_sub(-1), + _lpos_sub(-1), + _gpos_sub(-1), + _att_sub(-1), + _capture_pub(nullptr), + _camera_feedback_mode(CAMERA_FEEDBACK_MODE_NONE) +{ + + // Parameters + _p_feedback = param_find("CAM_FBACK_MODE"); + + param_get(_p_feedback, &_camera_feedback_mode); + +} + +CameraFeedback::~CameraFeedback() +{ + + if (_main_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + px4_task_delete(_main_task); + break; + } + } while (_main_task != -1); + } + + camera_feedback::g_camera_feedback = nullptr; +} + +int +CameraFeedback::start() +{ + + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = px4_task_spawn_cmd("camera_feedback", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 15, + 1600, + (px4_main_t)&CameraFeedback::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; + +} + +void +CameraFeedback::stop() +{ + if (camera_feedback::g_camera_feedback != nullptr) { + delete (camera_feedback::g_camera_feedback); + } +} + + +void +CameraFeedback::task_main() +{ + + // We only support trigger feedback for now + // This will later be extended to support hardware feedback from the camera. + if (_camera_feedback_mode != CAMERA_FEEDBACK_MODE_TRIGGER) { + return; + } + + // Polling sources + _trigger_sub = orb_subscribe(ORB_ID(camera_trigger)); + struct camera_trigger_s trig = {}; + + px4_pollfd_struct_t fds[1] = {}; + fds[0].fd = _trigger_sub; + fds[0].events = POLLIN; + + // Geotagging subscriptions + _lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _gpos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_local_position_s lpos = {}; + struct vehicle_global_position_s gpos = {}; + struct vehicle_attitude_s att = {}; + + bool updated = false; + + while (!_task_should_exit) { + + /* wait for up to 20ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20); + + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + continue; + } + + /* trigger subscription updated */ + if (fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(camera_trigger), _trigger_sub, &trig); + + /* update geotagging subscriptions */ + orb_check(_gpos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position), _gpos_sub, &gpos); + } + + orb_check(_lpos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _lpos_sub, &lpos); + } + + orb_check(_att_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + } + + if (gpos.timestamp == 0 || + lpos.timestamp == 0 || + att.timestamp == 0 || + !lpos.xy_valid) { + // reject until we have valid data + continue; + } + + struct camera_capture_s capture = {}; + + // Fill timestamps + capture.timestamp = trig.timestamp; + + capture.timestamp_utc = trig.timestamp_utc; + + // Fill image sequence + capture.seq = trig.seq; + + // Fill position data + capture.lat = gpos.lat; + + capture.lon = gpos.lon; + + capture.alt = gpos.alt; + + capture.ground_distance = lpos.dist_bottom_valid ? lpos.dist_bottom : -1.0f; + + // Fill attitude data + // TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available + capture.q[0] = att.q[0]; + + capture.q[1] = att.q[1]; + + capture.q[2] = att.q[2]; + + capture.q[3] = att.q[3]; + + // Indicate that no capture feedback from camera is available + capture.result = -1; + + int instance_id; + + orb_publish_auto(ORB_ID(camera_capture), &_capture_pub, &capture, &instance_id, ORB_PRIO_DEFAULT); + + } + + } + + PX4_INFO("Exiting."); + _main_task = -1; + +} + +void +CameraFeedback::task_main_trampoline(int argc, char *argv[]) +{ + camera_feedback::g_camera_feedback->task_main(); +} + +static int usage() +{ + PX4_INFO("usage: camera_feedback {start|stop}\n"); + return 1; +} + +extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[]); + +int camera_feedback_main(int argc, char *argv[]) +{ + if (argc < 2) { + return usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (camera_feedback::g_camera_feedback != nullptr) { + PX4_WARN("already running"); + return 0; + } + + camera_feedback::g_camera_feedback = new CameraFeedback(); + + if (camera_feedback::g_camera_feedback == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + camera_feedback::g_camera_feedback->start(); + return 0; + } + + if (camera_feedback::g_camera_feedback == nullptr) { + PX4_WARN("not running"); + return 1; + + } else if (!strcmp(argv[1], "stop")) { + camera_feedback::g_camera_feedback->stop(); + + } else { + return usage(); + } + + return 0; +} \ No newline at end of file diff --git a/src/modules/camera_feedback/camera_feedback_params.c b/src/modules/camera_feedback/camera_feedback_params.c new file mode 100644 index 0000000000..b6edcff77f --- /dev/null +++ b/src/modules/camera_feedback/camera_feedback_params.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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_feedback_params.c + * Camera feedback parameters + * + * @author Mohammed Kabir + */ + +/** + * Camera feedback mode + * + * Sets the camera feedback mode. + * + * @value 0 Disabled + * @value 1 Feedback on trigger + * @min 0 + * @max 1 + * @group Camera Control + */ +PARAM_DEFINE_INT32(CAM_FBACK_MODE, 0);