From a8537b8818d8c7839548a93e9c479d0e45d80194 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 08:20:18 +0530 Subject: [PATCH] camera trigger : initial import --- src/modules/camera_trigger/camera_trigger.cpp | 382 ++++++++++++++++++ .../camera_trigger/camera_trigger_params.c | 89 ++++ src/modules/camera_trigger/module.mk | 43 ++ 3 files changed, 514 insertions(+) create mode 100644 src/modules/camera_trigger/camera_trigger.cpp create mode 100644 src/modules/camera_trigger/camera_trigger_params.c create mode 100644 src/modules/camera_trigger/module.mk diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp new file mode 100644 index 0000000000..f0c86e42f2 --- /dev/null +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -0,0 +1,382 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Author: Mohammed Kabir + * + * 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_trigger.cpp + * + * External camera-IMU synchronisation and triggering via FMU auxillary pins. + * + * @author Mohammed Kabir + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); + +class CameraTrigger +{ +public: + /** + * Constructor + */ + CameraTrigger(); + + /** + * Destructor, also kills task. + */ + ~CameraTrigger(); + + /** + * Start the task. + */ + void start(); + + /** + * Stop the task. + */ + void stop(); + + /** + * Display info. + */ + void info(); + + int pin; + +private: + + struct hrt_call _pollcall; + struct hrt_call _firecall; + + int _gpio_fd; + + int _polarity; + float _activation_time; + float _integration_time; + float _transfer_time; + uint32_t _trigger_seq; + bool _trigger_enabled; + + hrt_abstime _trigger_timestamp; + + int _sensor_sub; + int _vcommand_sub; + + orb_advert_t _trigger_pub; + + struct camera_trigger_s _trigger; + struct sensor_combined_s _sensor; + struct vechicle_command_s _command; + + /** + * Topic poller to check for fire info. + */ + static void poll(void *arg); + /** + * Fires trigger + */ + static void engage(void *arg); + /** + * Resets trigger + */ + static void disengage(void *arg); + +}; + +namespace camera_trigger +{ + +CameraTrigger *g_camera_trigger; +} + +CameraTrigger::CameraTrigger() : + pin(1), + _gpio_fd(-1), + _polarity(0), + _activation_time(0.0f), + _integration_time(0.0f), + _transfer_time(0.0f), + _camera_trigger_sub(-1), + _trigger_seq(0), + _trigger_enabled(false), + _trigger{} +{ +} + +CameraTrigger::~CameraTrigger() +{ + camera_trigger::g_camera_trigger = nullptr; +} + +void +CameraTrigger::start() +{ + + /* Pull parameters */ + param_t polarity = param_find("TRIG_POLARITY"); + param_t activation_time = param_find("TRIG_ACT_TIME"); + param_t integration_time = param_find("TRIG_INT_TIME"); + param_t transfer_time = param_find("TRIG_TRANS_TIME"); + + param_get(polarity, &_polarity); + param_get(activation_time, &_activation_time); + param_get(integration_time, &_integration_time); + param_get(transfer_time, &_transfer_time); + + _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + + if (_gpio_fd < 0) { + + warnx("GPIO device open fail"); // TODO: errx + } + else + { + warnx("GPIO device opened"); + } + + _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + + ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); + + if(_polarity == 0) + { + ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + } + else if(_polarity == 1) + { + ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + } + else + { + warnx(" invalid trigger polarity setting. stopping."); + stop(); + } + + hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); +} + +void +CameraTrigger::stop() +{ + hrt_cancel(&_pollcall); + hrt_cancel(&_firecall); + + delete camera_trigger::g_camera_trigger; +} + +void +CameraTrigger::poll(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + bool updated; + orb_check(_vcommand_sub, &updated); + + if (updated) { + + orb_copy(ORB_ID(vehicle_command), _vcommand_sub, &_command); + + if(_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) + { + if(_command.param1 < 1) + { + if(_trigger_enabled) + { + mavlink_log_info(_mavlink_fd, "camera trigger disabled"); + _trigger_enabled = false ; + } + } + else if(_command.param1 >= 1) + { + if(!_camera_trigger_enabled) + { + mavlink_log_info(_mavlink_fd, "camera trigger enabled"); + _trigger_enabled = true ; + } + } + + // Set trigger rate from command + if(_command.param2 > 0) + { + _trig->integration_time = _command.param2; + param_set(_trig->integration_time, &(_trig->_integration_time)); + } + } + } + + if(!_trigger_enabled) + return; + + if (hrt_elapsed_time(&_trigger_timestamp) > (_trig->_transfer_time + _trig->_integration_time)*1000 ) { + + engage(trig); + hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); + + _trigger_timestamp = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); + + _trigger.timestamp = _sensor.timestamp; + _trigger.seq = _trigger_seq++; + + if (_camera_trigger_pub > 0) { + orb_publish(ORB_ID(camera_trigger), _camera_trigger_pub, &_trigger); + } else { + _camera_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &_trigger); + } + + } + +} + +void +CameraTrigger::engage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + if(trig->_polarity == 0) /* ACTIVE_LOW */ + { + ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + } + else if(trig->_polarity == 1) /* ACTIVE_HIGH */ + { + ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + } + +} + +void +CameraTrigger::disengage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + if(trig->_polarity == 0) /* ACTIVE_LOW */ + { + ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + } + else if(trig->_polarity == 1) /* ACTIVE_HIGH */ + { + ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + } + +} + +void +CameraTrigger::info() +{ + warnx("Trigger state : %s", _trigger_enabled ? "enabled" : "disabled"); + warnx("Trigger pin : %i", pin); + warnx("Trigger polarity : %s", _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); + warnx("Shutter integration time : %.2f", (double)_integration_time); +} + +static void usage() +{ + errx(1, "usage: camera_trigger {start|stop|info} [-p ]\n" + "\t-p \tUse specified AUX OUT pin number (default: 1)" + ); +} + +int camera_trigger_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (camera_trigger::g_camera_trigger != nullptr) { + errx(0, "already running"); + } + + camera_trigger::g_camera_trigger = new CameraTrigger; + + if (camera_trigger::g_camera_trigger == nullptr) { + errx(1, "alloc failed"); + } + + if (argc > 3) { + + camera_trigger::g_camera_trigger->pin = (int)argv[3]; + if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) { + warnx("starting trigger on pin : %li ", atoi(argv[3])); + camera_trigger::g_camera_trigger->pin = atoi(argv[3]); + } + else + { + usage(); + } + } + camera_trigger::g_camera_trigger->start(); + + return 0; + } + + if (camera_trigger::g_camera_trigger == nullptr) { + errx(1, "not running"); + } + + else if (!strcmp(argv[1], "stop")) { + camera_trigger::g_camera_trigger->stop(); + + } + else if (!strcmp(argv[1], "info")) { + camera_trigger::g_camera_trigger->info(); + + } else { + usage(); + } + + return 0; +} + diff --git a/src/modules/camera_trigger/camera_trigger_params.c b/src/modules/camera_trigger/camera_trigger_params.c new file mode 100644 index 0000000000..f236e5aa2a --- /dev/null +++ b/src/modules/camera_trigger/camera_trigger_params.c @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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_trigger_params.c + * Camera trigger parameters + * + * @author Mohammed Kabir + */ + +#include +#include + +/** + * Camera trigger shutter integration time + * + * This parameter sets the time the shutter is open on the camera. + * + * @unit milliseconds + * @min 0.0 + * @max 500.0 + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_INT_TIME, 300.0f); + +/** + * Camera trigger transfer time + * + * This parameter sets the time the image transfer takes (PointGrey mode_0) + * + * @unit milliseconds + * @min 15.0 + * @max 33.0 + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_TRANS_TIME, 15.0f); + +/** + * Camera trigger polarity + * + * This parameter sets the polarity of the trigger (0 = ACTIVE_LOW, 1 = ACTIVE_HIGH ) + * + * @min 0 + * @max 1 + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_POLARITY, 0); + +/** + * Camera trigger activation time + * + * This parameter sets the time the trigger needs to pulled high or low to start light + * integration. + * + * @unit milliseconds + * @default 4.0 ms + * @group Camera trigger + */ +PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 5.0f); diff --git a/src/modules/camera_trigger/module.mk b/src/modules/camera_trigger/module.mk new file mode 100644 index 0000000000..5bba057c5b --- /dev/null +++ b/src/modules/camera_trigger/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (C) 2015 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. +# +############################################################################ + +# +# External camera-IMU synchronisation via GPIO +# + +MODULE_COMMAND = camera_trigger +SRCS = camera_trigger.cpp \ + camera_trigger_params.c + +MODULE_STACKSIZE = 1000 +MAXOPTIMIZATION = -Os