diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index b41dca7899..66863c79b3 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/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/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 new file mode 100644 index 0000000000..bfbd770c8b --- /dev/null +++ b/src/modules/camera_trigger/camera_trigger.cpp @@ -0,0 +1,392 @@ +/**************************************************************************** + * + * 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 +#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; + + int _sensor_sub; + int _vcommand_sub; + + orb_advert_t _trigger_pub; + + struct camera_trigger_s _trigger; + struct sensor_combined_s _sensor; + 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. + */ + 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), + _pollcall{}, + _firecall{}, + _gpio_fd(-1), + _polarity(0), + _activation_time(0.0f), + _integration_time(0.0f), + _transfer_time(0.0f), + _trigger_seq(0), + _trigger_enabled(false), + _sensor_sub(-1), + _vcommand_sub(-1), + _trigger_pub(nullptr), + _trigger{}, + _sensor{}, + _command{} +{ + memset(&_trigger, 0, sizeof(_trigger)); + memset(&_sensor, 0, sizeof(_sensor)); + memset(&_command, 0, sizeof(_command)); + + memset(&_pollcall, 0, sizeof(_pollcall)); + memset(&_firecall, 0, sizeof(_firecall)); + + // 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() +{ + camera_trigger::g_camera_trigger = nullptr; +} + +void +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); + + stm32_configgpio(GPIO_GPIO0_OUTPUT); + + if(_polarity == 0) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // GPIO pin pull high + } + else if(_polarity == 1) { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); // GPIO pin pull low + } + else { + warnx(" invalid trigger polarity setting. stopping."); + stop(); + } + + poll(this); // Trampoline call + +} + +void +CameraTrigger::stop() +{ + hrt_cancel(&_firecall); + hrt_cancel(&_pollcall); + + if (camera_trigger::g_camera_trigger != nullptr) { + delete (camera_trigger::g_camera_trigger); + } +} + +void +CameraTrigger::poll(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + bool updated; + orb_check(trig->_vcommand_sub, &updated); + + if (updated) { + + orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); + + if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) + { + if(trig->_command.param1 < 1) + { + if(trig->_trigger_enabled) + { + trig->_trigger_enabled = false ; + } + } + else if(trig->_command.param1 >= 1) + { + if(!trig->_trigger_enabled) + { + trig->_trigger_enabled = true ; + } + } + + // Set trigger rate from command + if(trig->_command.param2 > 0) + { + trig->_integration_time = trig->_command.param2; + param_set(trig->integration_time, &(trig->_integration_time)); + } + } + } + + if(!trig->_trigger_enabled) { + hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); + return; + } + else + { + engage(trig); + 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); + + trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp + trig->_trigger.seq = trig->_trigger_seq++; + + 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); + } + + hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig); + } + +} + +void +CameraTrigger::engage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + stm32_configgpio(GPIO_GPIO0_OUTPUT); + + if(trig->_polarity == 0) // ACTIVE_LOW + { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); + } + else if(trig->_polarity == 1) // ACTIVE_HIGH + { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + } + + +} + +void +CameraTrigger::disengage(void *arg) +{ + + CameraTrigger *trig = reinterpret_cast(arg); + + stm32_configgpio(GPIO_GPIO0_OUTPUT); + + if(trig->_polarity == 0) // ACTIVE_LOW + { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); + } + else if(trig->_polarity == 1) // ACTIVE_HIGH + { + stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); + } + +} + +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..54098cc855 --- /dev/null +++ b/src/modules/camera_trigger/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ed95b5e85..2489dc3cad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -784,6 +784,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; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0edf7e4149..c79b923c5e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1622,6 +1622,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 b5e9201db7..fca0e91cbb 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/objects_common.cpp b/src/modules/uORB/objects_common.cpp index cc83b932cf..81aeec8967 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -259,3 +259,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);