From a8537b8818d8c7839548a93e9c479d0e45d80194 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 08:20:18 +0530 Subject: [PATCH 01/17] 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 From 239c8dc7dc7e6ad9cc20cf7c02eec96350f87ee3 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 09:14:39 +0530 Subject: [PATCH 02/17] camera trigger : implement trigerring and command --- makefiles/nuttx/config_px4fmu-v2_default.mk | 1 + src/modules/camera_trigger/camera_trigger.cpp | 88 +++++++++++-------- src/modules/uORB/objects_common.cpp | 3 + 3 files changed, 56 insertions(+), 36 deletions(-) diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index 4c8f60e693..33fcf4d508 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -73,6 +73,7 @@ MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan MODULES += modules/land_detector +MODULES += modules/camera_trigger # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index f0c86e42f2..5a2970edea 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -53,9 +53,11 @@ #include #include #include +#include #include #include #include +#include extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); @@ -95,6 +97,7 @@ private: struct hrt_call _firecall; int _gpio_fd; + int _mavlink_fd; /**< mavlink log device handle */ int _polarity; float _activation_time; @@ -112,7 +115,12 @@ private: struct camera_trigger_s _trigger; struct sensor_combined_s _sensor; - struct vechicle_command_s _command; + struct vehicle_command_s _command; + + param_t polarity ; + param_t activation_time ; + param_t integration_time ; + param_t transfer_time ; /** * Topic poller to check for fire info. @@ -138,15 +146,28 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : pin(1), _gpio_fd(-1), + _mavlink_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_enabled(true), + _trigger_timestamp(0), + _sensor_sub(-1), + _vcommand_sub(-1), + _trigger_pub(-1), _trigger{} { + memset(&_trigger, 0, sizeof(_trigger)); + memset(&_command, 0, sizeof(_command)); + memset(&_command, 0, sizeof(_sensor)); + + /* Parameters */ + polarity = param_find("TRIG_POLARITY"); + activation_time = param_find("TRIG_ACT_TIME"); + integration_time = param_find("TRIG_INT_TIME"); + transfer_time = param_find("TRIG_TRANS_TIME"); } CameraTrigger::~CameraTrigger() @@ -158,18 +179,8 @@ 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); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (_gpio_fd < 0) { @@ -182,6 +193,11 @@ CameraTrigger::start() _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + + param_get(polarity, &_polarity); + param_get(activation_time, &_activation_time); + param_get(integration_time, &_integration_time); + param_get(transfer_time, &_transfer_time); ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); @@ -218,59 +234,59 @@ CameraTrigger::poll(void *arg) CameraTrigger *trig = reinterpret_cast(arg); bool updated; - orb_check(_vcommand_sub, &updated); + orb_check(trig->_vcommand_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_command), _vcommand_sub, &_command); + orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); - if(_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) + if(trig->_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) { - if(_command.param1 < 1) + if(trig->_command.param1 < 1) { - if(_trigger_enabled) + if(trig->_trigger_enabled) { - mavlink_log_info(_mavlink_fd, "camera trigger disabled"); - _trigger_enabled = false ; + mavlink_log_info(trig->_mavlink_fd, "camera trigger disabled"); + trig->_trigger_enabled = false ; } } - else if(_command.param1 >= 1) + else if(trig->_command.param1 >= 1) { - if(!_camera_trigger_enabled) + if(!trig->_trigger_enabled) { - mavlink_log_info(_mavlink_fd, "camera trigger enabled"); - _trigger_enabled = true ; + mavlink_log_info(trig->_mavlink_fd, "camera trigger enabled"); + trig->_trigger_enabled = true ; } } // Set trigger rate from command - if(_command.param2 > 0) + if(trig->_command.param2 > 0) { - _trig->integration_time = _command.param2; - param_set(_trig->integration_time, &(_trig->_integration_time)); + trig->_integration_time = trig->_command.param2; + param_set(trig->integration_time, &(trig->_integration_time)); } } } - if(!_trigger_enabled) + if(!trig->_trigger_enabled) return; - if (hrt_elapsed_time(&_trigger_timestamp) > (_trig->_transfer_time + _trig->_integration_time)*1000 ) { + if (hrt_elapsed_time(&trig->_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(); + 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++; + trig->_trigger.timestamp = trig->_sensor.timestamp; + trig->_trigger.seq = trig->_trigger_seq++; - if (_camera_trigger_pub > 0) { - orb_publish(ORB_ID(camera_trigger), _camera_trigger_pub, &_trigger); + if (trig->_trigger_pub > 0) { + orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger); } else { - _camera_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &_trigger); + trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 9f980eebea..a911168d92 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -256,3 +256,6 @@ ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); #include "topics/distance_sensor.h" ORB_DEFINE(distance_sensor, struct distance_sensor_s); + +#include "topics/camera_trigger.h" +ORB_DEFINE(camera_trigger, struct camera_trigger_s); From ecd2762281eede4d642b50bd028d46843f086b04 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 10:03:00 +0530 Subject: [PATCH 03/17] camera trigger : fix memset --- src/modules/camera_trigger/camera_trigger.cpp | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 5a2970edea..564617bef7 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -97,7 +97,6 @@ private: struct hrt_call _firecall; int _gpio_fd; - int _mavlink_fd; /**< mavlink log device handle */ int _polarity; float _activation_time; @@ -146,7 +145,6 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : pin(1), _gpio_fd(-1), - _mavlink_fd(-1), _polarity(0), _activation_time(0.0f), _integration_time(0.0f), @@ -161,7 +159,7 @@ CameraTrigger::CameraTrigger() : { memset(&_trigger, 0, sizeof(_trigger)); memset(&_command, 0, sizeof(_command)); - memset(&_command, 0, sizeof(_sensor)); + memset(&_sensor, 0, sizeof(_sensor)); /* Parameters */ polarity = param_find("TRIG_POLARITY"); @@ -180,11 +178,11 @@ CameraTrigger::start() { _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (_gpio_fd < 0) { - warnx("GPIO device open fail"); // TODO: errx + warnx("GPIO device open fail"); // TODO + stop(); } else { @@ -214,8 +212,6 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } - - hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); } void @@ -246,7 +242,6 @@ CameraTrigger::poll(void *arg) { if(trig->_trigger_enabled) { - mavlink_log_info(trig->_mavlink_fd, "camera trigger disabled"); trig->_trigger_enabled = false ; } } @@ -254,7 +249,6 @@ CameraTrigger::poll(void *arg) { if(!trig->_trigger_enabled) { - mavlink_log_info(trig->_mavlink_fd, "camera trigger enabled"); trig->_trigger_enabled = true ; } } @@ -262,14 +256,17 @@ CameraTrigger::poll(void *arg) // Set trigger rate from command if(trig->_command.param2 > 0) { - trig->_integration_time = trig->_command.param2; - param_set(trig->integration_time, &(trig->_integration_time)); + trig->_integration_time = trig->_command.param2; + param_set(trig->integration_time, &(trig->_integration_time)); } } } - if(!trig->_trigger_enabled) + if(!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; + } + if (hrt_elapsed_time(&trig->_trigger_timestamp) > (trig->_transfer_time + trig->_integration_time)*1000 ) { @@ -280,7 +277,7 @@ CameraTrigger::poll(void *arg) orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - trig->_trigger.timestamp = trig->_sensor.timestamp; + trig->_trigger.timestamp = trig->_sensor.timestamp; /* get IMU timestamp */ trig->_trigger.seq = trig->_trigger_seq++; if (trig->_trigger_pub > 0) { @@ -288,9 +285,10 @@ CameraTrigger::poll(void *arg) } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - + + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000 , (hrt_callout)&CameraTrigger::poll, trig); } - + } void From 34809e0aa37b3b76afcf23894f974b5db1635b66 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 18 May 2015 13:43:10 +0530 Subject: [PATCH 04/17] camera trigger : add message --- src/modules/uORB/topics/camera_trigger.h | 68 ++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 src/modules/uORB/topics/camera_trigger.h diff --git a/src/modules/uORB/topics/camera_trigger.h b/src/modules/uORB/topics/camera_trigger.h new file mode 100644 index 0000000000..6994dffe5e --- /dev/null +++ b/src/modules/uORB/topics/camera_trigger.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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.h + * Camera-IMU synchronisation and triggering + */ + +#ifndef TOPIC_CAMERA_TRIGGER_H_ +#define TOPIC_CAMERA_TRIGGER_H_ + +#include +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Camera-IMU synchronisation message + */ +struct camera_trigger_s { + + uint64_t timestamp; /**< Timestamp when camera was triggered */ + uint32_t seq; /**< Image sequence - reset to zero on getting trigger reset command */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(camera_trigger); + +#endif /* TOPIC_CAMERA_TRIGGER_H_ */ + From be89a7262eae8ac95e421464a8fc98f2fd16d570 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 19 May 2015 22:46:04 +0530 Subject: [PATCH 05/17] camera trigger : add missing call to trampoline --- src/modules/camera_trigger/camera_trigger.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 564617bef7..89947c8fcf 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -212,6 +212,9 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } + + poll(this); + } void From 2dde99f0fc007520fadbf4ef4f45cd21d54528eb Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 20 May 2015 12:43:44 +0530 Subject: [PATCH 06/17] camera trigger : memset --- src/modules/camera_trigger/camera_trigger.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 89947c8fcf..b59443193a 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -161,6 +161,9 @@ CameraTrigger::CameraTrigger() : memset(&_command, 0, sizeof(_command)); memset(&_sensor, 0, sizeof(_sensor)); + memset(&_pollcall, 0, sizeof(_pollcall)); + memset(&_firecall, 0, sizeof(_firecall)); + /* Parameters */ polarity = param_find("TRIG_POLARITY"); activation_time = param_find("TRIG_ACT_TIME"); @@ -212,16 +215,16 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } - - poll(this); + + hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); } void CameraTrigger::stop() -{ - hrt_cancel(&_pollcall); +{ hrt_cancel(&_firecall); + hrt_cancel(&_pollcall); delete camera_trigger::g_camera_trigger; } @@ -266,17 +269,15 @@ CameraTrigger::poll(void *arg) } if(!trig->_trigger_enabled) { - hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; } - if (hrt_elapsed_time(&trig->_trigger_timestamp) > (trig->_transfer_time + trig->_integration_time)*1000 ) { + if (hrt_elapsed_time(&trig->_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); - trig->_trigger_timestamp = hrt_absolute_time(); + hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); @@ -289,7 +290,6 @@ CameraTrigger::poll(void *arg) trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000 , (hrt_callout)&CameraTrigger::poll, trig); } } From 5ff38089e91034b3b6880b43c1fd391ff4ee1eac Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 21 May 2015 17:23:19 +0530 Subject: [PATCH 07/17] camera trigger : fix handling of fds in hrt callbacks --- src/modules/camera_trigger/camera_trigger.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index b59443193a..934945ada7 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -215,8 +215,9 @@ CameraTrigger::start() warnx(" invalid trigger polarity setting. stopping."); stop(); } - - hrt_call_every(&_pollcall, 0, 1000, (hrt_callout)&CameraTrigger::poll, this); + close(_gpio_fd); + + poll(this); /* Trampoline call */ } @@ -234,7 +235,7 @@ CameraTrigger::poll(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + bool updated; orb_check(trig->_vcommand_sub, &updated); @@ -269,6 +270,7 @@ CameraTrigger::poll(void *arg) } if(!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; } @@ -289,7 +291,7 @@ CameraTrigger::poll(void *arg) } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } - + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); } } @@ -299,7 +301,9 @@ CameraTrigger::engage(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + + trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + if(trig->_polarity == 0) /* ACTIVE_LOW */ { ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); @@ -308,6 +312,8 @@ CameraTrigger::engage(void *arg) { ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); } + + close(trig->_gpio_fd); } @@ -316,7 +322,9 @@ CameraTrigger::disengage(void *arg) { CameraTrigger *trig = reinterpret_cast(arg); - + + trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); + if(trig->_polarity == 0) /* ACTIVE_LOW */ { ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); @@ -326,6 +334,8 @@ CameraTrigger::disengage(void *arg) ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); } + close(trig->_gpio_fd); + } void From 95a8e29cfeba7bf15c51e46db408bfc817f616c4 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 21 May 2015 17:41:52 +0530 Subject: [PATCH 08/17] camera trigger : mavlink stream --- msg/camera_trigger.msg | 4 ++ src/modules/camera_trigger/camera_trigger.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 54 +++++++++++++++++++ src/modules/uORB/topics/camera_trigger.h | 38 ++++++------- 5 files changed, 80 insertions(+), 19 deletions(-) create mode 100644 msg/camera_trigger.msg diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg new file mode 100644 index 0000000000..b4dcfe8ef3 --- /dev/null +++ b/msg/camera_trigger.msg @@ -0,0 +1,4 @@ + +uint64 timestamp # Timestamp when camera was triggered +uint32 seq # Image sequence + diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 934945ada7..0ee93128e4 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -184,7 +184,7 @@ CameraTrigger::start() if (_gpio_fd < 0) { - warnx("GPIO device open fail"); // TODO + warnx("GPIO device open fail"); stop(); } else diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ce2b5cb8bb..4e9e8a65f1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1611,6 +1611,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); + configure_stream("CAMERA_TRIGGER", 30.0f); break; case MAVLINK_MODE_OSD: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 24f04fd74f..2b34345a6b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include #include #include @@ -1062,6 +1063,59 @@ protected: } }; + +class MavlinkStreamCameraTrigger : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamCameraTrigger::get_name_static(); + } + + static const char *get_name_static() { + return "CAMERA_TRIGGER"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_CAMERA_TRIGGER; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamCameraTrigger(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_camera_trigger_sub; + uint64_t _trig_time; + + /* do not allow top copying this class */ + MavlinkStreamCameraTrigger(MavlinkStreamCameraTrigger &); + MavlinkStreamCameraTrigger &operator = (const MavlinkStreamCameraTrigger &); + +protected: + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink), + _camera_trigger_sub(_mavlink->add_orb_subscription(ORB_ID(camera_trigger))), + _trig_time(0) + {} + + void send(const hrt_abstime t) { + struct camera_trigger_s trigger; + + if (_camera_trigger_sub->update(&_trig_time, &trigger)) { + + mavlink_camera_trigger_t msg; + + msg.time_usec = trigger.timestamp; + msg.seq = trigger.seq; + + _mavlink->send_message(MAVLINK_MSG_ID_CAMERA_TRIGGER, &msg); + } + } +}; + class MavlinkStreamGlobalPositionInt : public MavlinkStream { public: diff --git a/src/modules/uORB/topics/camera_trigger.h b/src/modules/uORB/topics/camera_trigger.h index 6994dffe5e..a0d7e4cef8 100644 --- a/src/modules/uORB/topics/camera_trigger.h +++ b/src/modules/uORB/topics/camera_trigger.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -31,30 +31,35 @@ * ****************************************************************************/ -/** - * @file camera_trigger.h - * Camera-IMU synchronisation and triggering - */ +/* Auto-generated by genmsg_cpp from file /home/kabir/fork/Firmware/msg/camera_trigger.msg */ -#ifndef TOPIC_CAMERA_TRIGGER_H_ -#define TOPIC_CAMERA_TRIGGER_H_ -#include +#pragma once + #include +#include + + +#ifndef __cplusplus + +#endif /** * @addtogroup topics * @{ */ -/** - * Camera-IMU synchronisation message - */ -struct camera_trigger_s { - uint64_t timestamp; /**< Timestamp when camera was triggered */ - uint32_t seq; /**< Image sequence - reset to zero on getting trigger reset command */ - +#ifdef __cplusplus +struct __EXPORT camera_trigger_s { +#else +struct camera_trigger_s { +#endif + uint64_t timestamp; + uint32_t seq; +#ifdef __cplusplus + +#endif }; /** @@ -63,6 +68,3 @@ struct camera_trigger_s { /* register this as object request broker structure */ ORB_DECLARE(camera_trigger); - -#endif /* TOPIC_CAMERA_TRIGGER_H_ */ - From a1c2f24837301365d9731df644bf0138bc170342 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 21 May 2015 18:02:45 +0530 Subject: [PATCH 09/17] camera trigger : remove autogen message --- src/modules/uORB/topics/camera_trigger.h | 70 ------------------------ 1 file changed, 70 deletions(-) delete mode 100644 src/modules/uORB/topics/camera_trigger.h diff --git a/src/modules/uORB/topics/camera_trigger.h b/src/modules/uORB/topics/camera_trigger.h deleted file mode 100644 index a0d7e4cef8..0000000000 --- a/src/modules/uORB/topics/camera_trigger.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/kabir/fork/Firmware/msg/camera_trigger.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT camera_trigger_s { -#else -struct camera_trigger_s { -#endif - uint64_t timestamp; - uint32_t seq; -#ifdef __cplusplus - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(camera_trigger); From df037d97c152a16ca17ad0d84e5bacdd264093ba Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Fri, 22 May 2015 14:24:54 +0530 Subject: [PATCH 10/17] camera trigger : remove redundant timestamps --- src/modules/camera_trigger/camera_trigger.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 0ee93128e4..883048ba0c 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -105,8 +105,6 @@ private: uint32_t _trigger_seq; bool _trigger_enabled; - hrt_abstime _trigger_timestamp; - int _sensor_sub; int _vcommand_sub; @@ -151,7 +149,6 @@ CameraTrigger::CameraTrigger() : _transfer_time(0.0f), _trigger_seq(0), _trigger_enabled(true), - _trigger_timestamp(0), _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(-1), @@ -273,12 +270,9 @@ CameraTrigger::poll(void *arg) hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); return; } - - - if (hrt_elapsed_time(&trig->_trigger_timestamp) >= (trig->_transfer_time + trig->_integration_time)*1000 ) { - + else + { engage(trig); - trig->_trigger_timestamp = hrt_absolute_time(); hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig); orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); @@ -291,6 +285,7 @@ CameraTrigger::poll(void *arg) } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); } + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); } From 72e2224d1e87f32110c75b78554d4dc6130c797d Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 3 Jun 2015 11:48:37 +0530 Subject: [PATCH 11/17] camera trigger : master rebase --- msg/vehicle_command.msg | 3 ++- src/modules/camera_trigger/camera_trigger.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 391dc01aa0..2f223fbc2e 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -48,7 +48,8 @@ uint32 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutd uint32 VEHICLE_CMD_OVERRIDE_GOTO = 252 # Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| uint32 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| -uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 883048ba0c..c0a43f9255 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -151,7 +151,7 @@ CameraTrigger::CameraTrigger() : _trigger_enabled(true), _sensor_sub(-1), _vcommand_sub(-1), - _trigger_pub(-1), + _trigger_pub(nullptr), _trigger{} { memset(&_trigger, 0, sizeof(_trigger)); From af62e74d4a3f5a348cb98c95e688daed389db9dd Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Wed, 3 Jun 2015 11:51:02 +0530 Subject: [PATCH 12/17] camera trigger : command fix --- src/modules/camera_trigger/camera_trigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index c0a43f9255..a694778014 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -240,7 +240,7 @@ CameraTrigger::poll(void *arg) orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); - if(trig->_command.command == VEHICLE_CMD_DO_TRIGGER_CONTROL) + if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { if(trig->_command.param1 < 1) { From e6752e43b264a97bec830c442df518f7200fd854 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Fri, 5 Jun 2015 18:30:40 +0530 Subject: [PATCH 13/17] camera trigger : cleanup - still crashes --- src/modules/camera_trigger/camera_trigger.cpp | 55 +++++++++++-------- 1 file changed, 31 insertions(+), 24 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index a694778014..7c7e5a14d1 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -142,6 +142,8 @@ CameraTrigger *g_camera_trigger; CameraTrigger::CameraTrigger() : pin(1), + _pollcall{}, + _firecall{}, _gpio_fd(-1), _polarity(0), _activation_time(0.0f), @@ -152,11 +154,13 @@ CameraTrigger::CameraTrigger() : _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(nullptr), - _trigger{} + _trigger{}, + _sensor{}, + _command{} { memset(&_trigger, 0, sizeof(_trigger)); - memset(&_command, 0, sizeof(_command)); memset(&_sensor, 0, sizeof(_sensor)); + memset(&_command, 0, sizeof(_command)); memset(&_pollcall, 0, sizeof(_pollcall)); memset(&_firecall, 0, sizeof(_firecall)); @@ -180,7 +184,6 @@ CameraTrigger::start() _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); if (_gpio_fd < 0) { - warnx("GPIO device open fail"); stop(); } @@ -197,15 +200,15 @@ CameraTrigger::start() param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); + px4_ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); if(_polarity == 0) { - ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ } else if(_polarity == 1) { - ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ } else { @@ -224,7 +227,9 @@ CameraTrigger::stop() hrt_cancel(&_firecall); hrt_cancel(&_pollcall); - delete camera_trigger::g_camera_trigger; + if (camera_trigger::g_camera_trigger != nullptr) { + delete (camera_trigger::g_camera_trigger); + } } void @@ -280,7 +285,7 @@ CameraTrigger::poll(void *arg) trig->_trigger.timestamp = trig->_sensor.timestamp; /* get IMU timestamp */ trig->_trigger.seq = trig->_trigger_seq++; - if (trig->_trigger_pub > 0) { + if (trig->_trigger_pub != nullptr) { orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger); } else { trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger); @@ -298,17 +303,18 @@ CameraTrigger::engage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - - if(trig->_polarity == 0) /* ACTIVE_LOW */ + if(trig->_gpio_fd == -1) return; + + if(trig->_polarity == 0) // ACTIVE_LOW { - ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); } - else if(trig->_polarity == 1) /* ACTIVE_HIGH */ + else if(trig->_polarity == 1) // ACTIVE_HIGH { - ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); } - - close(trig->_gpio_fd); + + close(trig->_gpio_fd); } @@ -319,16 +325,17 @@ CameraTrigger::disengage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - - 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); - } + if(trig->_gpio_fd == -1) return; + if(trig->_polarity == 0) // ACTIVE_LOW + { + px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + } + else if(trig->_polarity == 1) // ACTIVE_HIGH + { + px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + } + close(trig->_gpio_fd); } From 0dc6e65d7a9814196331736cfa4f019a8e56dcef Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Mon, 15 Jun 2015 23:24:14 +0530 Subject: [PATCH 14/17] camera_trigger : direct GPIO access. finally working. --- src/modules/camera_trigger/camera_trigger.cpp | 39 ++++++------------- src/modules/camera_trigger/module.mk | 1 - 2 files changed, 11 insertions(+), 29 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 7c7e5a14d1..2a1632d1e4 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -59,6 +59,8 @@ #include #include +#include + extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); class CameraTrigger @@ -181,17 +183,6 @@ void CameraTrigger::start() { - _gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - - if (_gpio_fd < 0) { - warnx("GPIO device open fail"); - stop(); - } - else - { - warnx("GPIO device opened"); - } - _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -199,24 +190,21 @@ CameraTrigger::start() param_get(activation_time, &_activation_time); param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - - px4_ioctl(_gpio_fd, GPIO_SET_OUTPUT, pin); if(_polarity == 0) { - px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + //px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ } else if(_polarity == 1) { - px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + //px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ } else { warnx(" invalid trigger polarity setting. stopping."); stop(); } - close(_gpio_fd); - + poll(this); /* Trampoline call */ } @@ -302,19 +290,17 @@ CameraTrigger::engage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); - trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - if(trig->_gpio_fd == -1) return; + stm32_configgpio(GPIO_GPIO0_OUTPUT); if(trig->_polarity == 0) // ACTIVE_LOW { - px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); } else if(trig->_polarity == 1) // ACTIVE_HIGH { - px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); } - close(trig->_gpio_fd); } @@ -324,19 +310,16 @@ CameraTrigger::disengage(void *arg) CameraTrigger *trig = reinterpret_cast(arg); - trig->_gpio_fd = open(PX4FMU_DEVICE_PATH, 0); - if(trig->_gpio_fd == -1) return; + stm32_configgpio(GPIO_GPIO0_OUTPUT); if(trig->_polarity == 0) // ACTIVE_LOW { - px4_ioctl(trig->_gpio_fd, GPIO_SET, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); } else if(trig->_polarity == 1) // ACTIVE_HIGH { - px4_ioctl(trig->_gpio_fd, GPIO_CLEAR, trig->pin); + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); } - - close(trig->_gpio_fd); } diff --git a/src/modules/camera_trigger/module.mk b/src/modules/camera_trigger/module.mk index 5bba057c5b..54098cc855 100644 --- a/src/modules/camera_trigger/module.mk +++ b/src/modules/camera_trigger/module.mk @@ -39,5 +39,4 @@ MODULE_COMMAND = camera_trigger SRCS = camera_trigger.cpp \ camera_trigger_params.c -MODULE_STACKSIZE = 1000 MAXOPTIMIZATION = -Os From ba89883fb012d4685b948a9afcb07a4fa5536e76 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 15:44:58 +0530 Subject: [PATCH 15/17] camera trigger: minor cleanup --- src/modules/camera_trigger/camera_trigger.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index 2a1632d1e4..a81607e950 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -58,7 +58,6 @@ #include #include #include - #include extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); @@ -152,7 +151,7 @@ CameraTrigger::CameraTrigger() : _integration_time(0.0f), _transfer_time(0.0f), _trigger_seq(0), - _trigger_enabled(true), + _trigger_enabled(false), _sensor_sub(-1), _vcommand_sub(-1), _trigger_pub(nullptr), @@ -191,16 +190,15 @@ CameraTrigger::start() param_get(integration_time, &_integration_time); param_get(transfer_time, &_transfer_time); - if(_polarity == 0) - { - //px4_ioctl(_gpio_fd, GPIO_SET, pin); /* GPIO pin pull high */ + stm32_configgpio(GPIO_GPIO0_OUTPUT); + + if(_polarity == 0) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); /* GPIO pin pull high */ } - else if(_polarity == 1) - { - //px4_ioctl(_gpio_fd, GPIO_CLEAR, pin); /* GPIO pin pull low */ + else if(_polarity == 1) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); /* GPIO pin pull low */ } - else - { + else { warnx(" invalid trigger polarity setting. stopping."); stop(); } From 6a818ae05315824085ed89598d33a3e3b9e147bc Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 15:47:55 +0530 Subject: [PATCH 16/17] commander : ignore handling camera_trigger command --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d47b45d89f..19e9b93ab9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -783,6 +783,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE: + case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL: /* ignore commands that handled in low prio loop */ break; From 3d92364d9eb391d3f0d615df7092d96194e2d5b0 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Jun 2015 22:55:05 +0530 Subject: [PATCH 17/17] camera trigger : increase free cycling time when we are not enabled --- src/modules/camera_trigger/camera_trigger.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/camera_trigger/camera_trigger.cpp b/src/modules/camera_trigger/camera_trigger.cpp index a81607e950..bfbd770c8b 100644 --- a/src/modules/camera_trigger/camera_trigger.cpp +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -166,7 +166,7 @@ CameraTrigger::CameraTrigger() : memset(&_pollcall, 0, sizeof(_pollcall)); memset(&_firecall, 0, sizeof(_firecall)); - /* Parameters */ + // Parameters polarity = param_find("TRIG_POLARITY"); activation_time = param_find("TRIG_ACT_TIME"); integration_time = param_find("TRIG_INT_TIME"); @@ -193,17 +193,17 @@ CameraTrigger::start() stm32_configgpio(GPIO_GPIO0_OUTPUT); if(_polarity == 0) { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); /* GPIO pin pull high */ + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // GPIO pin pull high } else if(_polarity == 1) { - stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); /* GPIO pin pull low */ + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); // GPIO pin pull low } else { warnx(" invalid trigger polarity setting. stopping."); stop(); } - poll(this); /* Trampoline call */ + poll(this); // Trampoline call } @@ -258,7 +258,7 @@ CameraTrigger::poll(void *arg) } if(!trig->_trigger_enabled) { - hrt_call_after(&trig->_pollcall, 1000, (hrt_callout)&CameraTrigger::poll, trig); + hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); return; } else @@ -268,7 +268,7 @@ CameraTrigger::poll(void *arg) orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor); - trig->_trigger.timestamp = trig->_sensor.timestamp; /* get IMU timestamp */ + trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp trig->_trigger.seq = trig->_trigger_seq++; if (trig->_trigger_pub != nullptr) {