dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

camera_trigger: cleanup 

- add copyright headers and update year
 - move initializers to header
 - add PX4_DEBUG messages
This commit is contained in:
Daniel Agar 2021-03-15 12:35:41 -04:00 committed by GitHub
parent 517a1d0116
commit 6cdc034f08
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 391 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/camera_trigger/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2015-2021 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
@ -36,10 +36,10 @@ px4_add_module(
COMPILE_FLAGS
SRCS
camera_trigger.cpp
interfaces/src/camera_interface.cpp
interfaces/src/pwm.cpp
interfaces/src/seagull_map2.cpp
interfaces/src/gpio.cpp
interfaces/src/camera_interface.cpp
interfaces/src/pwm.cpp
interfaces/src/seagull_map2.cpp
interfaces/src/gpio.cpp
DEPENDS
px4_work_queue
)

224
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2021 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
@ -69,8 +69,6 @@
#include "interfaces/src/pwm.h"
#include "interfaces/src/seagull_map2.h"
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
typedef enum : int32_t {
CAMERA_INTERFACE_MODE_NONE = 0,
CAMERA_INTERFACE_MODE_GPIO,
@ -154,41 +152,41 @@ public:
private:
struct hrt_call _engagecall;
struct hrt_call _disengagecall;
struct hrt_call _engage_turn_on_off_call;
struct hrt_call _disengage_turn_on_off_call;
struct hrt_call _keepalivecall_up;
struct hrt_call _keepalivecall_down;
struct hrt_call _engagecall {};
struct hrt_call _disengagecall {};
struct hrt_call _engage_turn_on_off_call {};
struct hrt_call _disengage_turn_on_off_call {};
struct hrt_call _keepalivecall_up {};
struct hrt_call _keepalivecall_down {};
float _activation_time;
float _interval;
float _min_interval;
float _distance;
uint32_t _trigger_seq;
hrt_abstime _last_trigger_timestamp;
bool _trigger_enabled;
bool _trigger_paused;
bool _one_shot;
bool _test_shot;
bool _turning_on;
matrix::Vector2f _last_shoot_position;
bool _valid_position;
float _activation_time{0.5f};
float _interval{100.f};
float _min_interval{1.f};
float _distance{25.f};
uint32_t _trigger_seq{0};
hrt_abstime _last_trigger_timestamp{0};
bool _trigger_enabled{false};
bool _trigger_paused{false};
bool _one_shot{false};
bool _test_shot{false};
bool _turning_on{false};
matrix::Vector2f _last_shoot_position{0.f, 0.f};
bool _valid_position{false};
//Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
uint32_t _CAMPOS_num_poses;
uint32_t _CAMPOS_pose_counter;
float _CAMPOS_roll_angle;
float _CAMPOS_angle_interval;
float _CAMPOS_pitch_angle;
bool _CAMPOS_updated_roll_angle;
uint32_t _target_system;
uint32_t _target_component;
uint32_t _CAMPOS_num_poses{0};
uint32_t _CAMPOS_pose_counter{0};
float _CAMPOS_roll_angle{0.f};
float _CAMPOS_angle_interval{0.f};
float _CAMPOS_pitch_angle{-90.f};
bool _CAMPOS_updated_roll_angle{false};
uint32_t _target_system{0};
uint32_t _target_component{0};
uORB::Subscription _command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
orb_advert_t _trigger_pub;
orb_advert_t _trigger_pub{nullptr};
uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
@ -200,11 +198,11 @@ private:
param_t _p_interface;
param_t _p_cam_cap_fback;
trigger_mode_t _trigger_mode;
int32_t _cam_cap_fback;
trigger_mode_t _trigger_mode{TRIGGER_MODE_NONE};
int32_t _cam_cap_fback{0};
camera_interface_mode_t _camera_interface_mode;
CameraInterface *_camera_interface; ///< instance of camera interface
camera_interface_mode_t _camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
CameraInterface *_camera_interface{nullptr}; ///< instance of camera interface
/**
* Vehicle command handler
@ -248,39 +246,7 @@ CameraTrigger *g_camera_trigger;
}
CameraTrigger::CameraTrigger() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_engagecall {},
_disengagecall {},
_engage_turn_on_off_call {},
_disengage_turn_on_off_call {},
_keepalivecall_up {},
_keepalivecall_down {},
_activation_time(0.5f /* ms */),
_interval(100.0f /* ms */),
_min_interval(1.0f /* ms */),
_distance(25.0f /* m */),
_trigger_seq(0),
_last_trigger_timestamp(0),
_trigger_enabled(false),
_trigger_paused(false),
_one_shot(false),
_test_shot(false),
_turning_on(false),
_last_shoot_position(0.0f, 0.0f),
_valid_position(false),
_CAMPOS_num_poses(0),
_CAMPOS_pose_counter(0),
_CAMPOS_roll_angle(0.0f),
_CAMPOS_angle_interval(0.0f),
_CAMPOS_pitch_angle(-90),
_CAMPOS_updated_roll_angle(false),
_target_system(0),
_target_component(0),
_trigger_pub(nullptr),
_trigger_mode(TRIGGER_MODE_NONE),
_cam_cap_fback(0),
_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
_camera_interface(nullptr)
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
// Initiate camera interface based on camera_interface_mode
if (_camera_interface != nullptr) {
@ -338,13 +304,14 @@ CameraTrigger::CameraTrigger() :
if ((_activation_time < 40.0f) &&
(_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM ||
_camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) {
_activation_time = 40.0f;
PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
param_set_no_notification(_p_activation_time, &(_activation_time));
}
// Advertise critical publishers here, because we cannot advertise in interrupt context
struct camera_trigger_s trigger = {};
camera_trigger_s trigger{};
if (!_cam_cap_fback) {
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
@ -370,13 +337,13 @@ CameraTrigger::update_intervalometer()
// control_intervalometer once on enabling/disabling trigger to schedule the calls.
if (_trigger_enabled && !_trigger_paused) {
// schedule trigger on and off calls
hrt_call_every(&_engagecall, 0, (_interval * 1000),
(hrt_callout)&CameraTrigger::engage, this);
PX4_DEBUG("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused);
// schedule trigger on and off calls
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
(hrt_callout)&CameraTrigger::disengage, this);
hrt_call_every(&_engagecall, 0, (_interval * 1000), &CameraTrigger::engage, this);
// schedule trigger on and off calls
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000), &CameraTrigger::disengage, this);
}
}
@ -392,7 +359,6 @@ CameraTrigger::update_distance()
_lpos_sub.copy(&local);
if (local.xy_valid) {
// Initialize position if not done yet
matrix::Vector2f current_position(local.x, local.y);
@ -426,14 +392,15 @@ void
CameraTrigger::enable_keep_alive(bool on)
{
if (on) {
// schedule keep-alive up and down calls
hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
(hrt_callout)&CameraTrigger::keep_alive_up, this);
PX4_DEBUG("keep alive enable");
hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
(hrt_callout)&CameraTrigger::keep_alive_down, this);
// schedule keep-alive up and down calls
hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000), &CameraTrigger::keep_alive_up, this);
hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000), &CameraTrigger::keep_alive_down, this);
} else {
PX4_DEBUG("keep alive disable");
// cancel all calls
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
@ -443,23 +410,24 @@ CameraTrigger::enable_keep_alive(bool on)
void
CameraTrigger::toggle_power()
{
// schedule power toggle calls
hrt_call_after(&_engage_turn_on_off_call, 0,
(hrt_callout)&CameraTrigger::engange_turn_on_off, this);
PX4_DEBUG("toggle power");
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
(hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
// schedule power toggle calls
hrt_call_after(&_engage_turn_on_off_call, 0, &CameraTrigger::engange_turn_on_off, this);
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000), &CameraTrigger::disengage_turn_on_off, this);
}
void
CameraTrigger::shoot_once()
{
PX4_DEBUG("shoot once");
// schedule trigger on and off calls
hrt_call_after(&_engagecall, 0,
(hrt_callout)&CameraTrigger::engage, this);
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
(hrt_callout)&CameraTrigger::disengage, this);
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000), &CameraTrigger::disengage, this);
}
bool
@ -527,13 +495,13 @@ void
CameraTrigger::test()
{
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param5 = 1.0;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
vcmd.target_system = 1;
vcmd.target_component = 1;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
vcmd_pub.publish(vcmd);
}
@ -559,6 +527,7 @@ CameraTrigger::Run()
// Command handling
if (updated) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
PX4_DEBUG("received DO_DIGICAM_CONTROL");
need_ack = true;
hrt_abstime now = hrt_absolute_time();
@ -571,20 +540,18 @@ CameraTrigger::Run()
if (commandParamToInt(cmd.param7) == 1) {
// test shots are not logged or forwarded to GCS for geotagging
_test_shot = true;
}
if (commandParamToInt((float)cmd.param5) == 1) {
// Schedule shot
_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
PX4_DEBUG("received DO_TRIGGER_CONTROL");
need_ack = true;
if (commandParamToInt(cmd.param3) == 1) {
@ -610,13 +577,12 @@ CameraTrigger::Run()
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
PX4_DEBUG("received DO_SET_CAM_TRIGG_DIST");
need_ack = true;
/*
* TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
*/
if (cmd.param1 > 0.0f) {
_distance = cmd.param1;
param_set_no_notification(_p_distance, &_distance);
@ -649,7 +615,7 @@ CameraTrigger::Run()
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
PX4_DEBUG("received DO_SET_CAM_TRIGG_INTERVAL");
need_ack = true;
if (cmd.param1 > 0.0f) {
@ -668,7 +634,7 @@ CameraTrigger::Run()
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
PX4_INFO("received OBLIQUE_SURVEY");
// Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
need_ack = true;
@ -792,7 +758,6 @@ unknown_cmd:
// One shot command-based capture handling
if (_one_shot && !_turning_on) {
// One-shot trigger
shoot_once();
_one_shot = false;
@ -804,20 +769,45 @@ unknown_cmd:
// Command ACK handling
if (updated && need_ack) {
PX4_DEBUG("acknowledging command %d, result=%d", cmd.command, cmd_result);
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.result = (uint8_t)cmd_result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.timestamp = hrt_absolute_time();
_cmd_ack_pub.publish(command_ack);
}
ScheduleDelayed(poll_interval_usec);
}
void
CameraTrigger::adjust_roll()
{
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
vcmd.target_system = _target_system;
vcmd.target_component = _target_component;
//param1 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is pitch
vcmd.param1 = _CAMPOS_pitch_angle;
//param2 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is roll
if (++_CAMPOS_pose_counter == _CAMPOS_num_poses) {
_CAMPOS_pose_counter = 0;
}
vcmd.param2 = _CAMPOS_angle_interval * _CAMPOS_pose_counter - _CAMPOS_roll_angle;
vcmd.param7 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
vcmd_pub.publish(vcmd);
}
void
CameraTrigger::engage(void *arg)
{
@ -841,18 +831,15 @@ CameraTrigger::engage(void *arg)
// Send camera trigger message. This messages indicates that we sent
// the camera trigger request. Does not guarantee capture.
camera_trigger_s trigger{};
struct camera_trigger_s trigger = {};
// Set timestamp the instant after the trigger goes off
trigger.timestamp = now;
timespec tv = {};
timespec tv{};
px4_clock_gettime(CLOCK_REALTIME, &tv);
trigger.timestamp_utc = (uint64_t) tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
trigger.seq = trig->_trigger_seq;
trigger.feedback = false;
trigger.timestamp = hrt_absolute_time();
if (!trig->_cam_cap_fback) {
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
@ -935,7 +922,7 @@ static int usage()
return 1;
}
int camera_trigger_main(int argc, char *argv[])
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[])
{
if (argc < 2) {
return usage();
@ -985,28 +972,3 @@ int camera_trigger_main(int argc, char *argv[])
return 0;
}
void
CameraTrigger::adjust_roll()
{
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
vcmd.target_system = _target_system;
vcmd.target_component = _target_component;
//param1 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is pitch
vcmd.param1 = _CAMPOS_pitch_angle;
//param2 of VEHICLE_CMD_DO_MOUNT_CONTROL in VEHICLE_MOUNT_MODE_MAVLINK_TARGETING mode is roll
if (++_CAMPOS_pose_counter == _CAMPOS_num_poses) {
_CAMPOS_pose_counter = 0;
}
vcmd.param2 = _CAMPOS_angle_interval * _CAMPOS_pose_counter - _CAMPOS_roll_angle;
vcmd.param7 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
}

2
src/drivers/camera_trigger/camera_trigger_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2021 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

45
src/drivers/camera_trigger/interfaces/src/camera_interface.cpp
View File

@ -1,20 +1,41 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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.
*
****************************************************************************/
#include "camera_interface.h"
#include <px4_platform_common/log.h>
/**
* @file camera_interface.cpp
*
*/
CameraInterface::CameraInterface():
_p_pin(PARAM_INVALID),
_pins{}
{
}
void CameraInterface::get_pins()
{
// Get parameter handle
_p_pin = param_find("TRIG_PINS");

47
src/drivers/camera_trigger/interfaces/src/camera_interface.h
View File

@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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_interface.h
*/
@ -12,15 +45,7 @@
class CameraInterface
{
public:
/**
* Constructor
*/
CameraInterface();
/**
* Destructor.
*/
CameraInterface() = default;
virtual ~CameraInterface() = default;
/**
@ -72,8 +97,8 @@ protected:
*/
void get_pins();
param_t _p_pin;
param_t _p_pin{PARAM_INVALID};
int _pins[6];
int _pins[6] {};
};

49
src/drivers/camera_trigger/interfaces/src/gpio.cpp
View File

@ -1,18 +1,49 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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.
*
****************************************************************************/
#ifdef __PX4_NUTTX
#include "gpio.h"
#include <cstring>
#include <px4_arch/io_timer.h>
CameraInterfaceGPIO::CameraInterfaceGPIO():
CameraInterface(),
_trigger_invert(false),
_triggers{}
CameraInterfaceGPIO::CameraInterfaceGPIO()
{
_p_polarity = param_find("TRIG_POLARITY");
// polarity of the trigger (0 = active low, 1 = active high )
int32_t polarity;
int32_t polarity = 0;
param_get(_p_polarity, &polarity);
_trigger_invert = (polarity == 0);
@ -23,7 +54,6 @@ CameraInterfaceGPIO::CameraInterfaceGPIO():
void CameraInterfaceGPIO::setup()
{
for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) {
// Pin range is from 0 to num_gpios - 1
if (_pins[i] >= 0 && t < (int)arraySize(_triggers)) {
uint32_t gpio = io_timer_channel_get_gpio_output(_pins[i]);
@ -39,10 +69,9 @@ void CameraInterfaceGPIO::trigger(bool trigger_on_true)
bool trigger_state = trigger_on_true ^ _trigger_invert;
for (unsigned i = 0; i < arraySize(_triggers); i++) {
if (_triggers[i] == 0) { break; }
px4_arch_gpiowrite(_triggers[i], trigger_state);
if (_triggers[i] != 0) {
px4_arch_gpiowrite(_triggers[i], trigger_state);
}
}
}

41
src/drivers/camera_trigger/interfaces/src/gpio.h
View File

@ -1,5 +1,38 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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 relay.h
* @file gpio.h
*
* Interface with cameras via FMU auxiliary pins.
*
@ -27,11 +60,11 @@ private:
void setup();
param_t _p_polarity;
param_t _p_polarity{PARAM_INVALID};
bool _trigger_invert;
bool _trigger_invert{false};
uint32_t _triggers[num_gpios];
uint32_t _triggers[num_gpios] {};
};
#endif /* ifdef __PX4_NUTTX */

36
src/drivers/camera_trigger/interfaces/src/pwm.cpp
View File

@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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.
*
****************************************************************************/
#ifdef __PX4_NUTTX
#include <sys/ioctl.h>
@ -8,7 +41,6 @@
#include "drivers/drv_pwm_trigger.h"
#include "pwm.h"
CameraInterfacePWM::CameraInterfacePWM():
CameraInterface()
{
@ -27,7 +59,7 @@ CameraInterfacePWM::~CameraInterfacePWM()
void CameraInterfacePWM::setup()
{
// Precompute the bitmask for enabled channels
uint8_t pin_bitmask = 0;
uint32_t pin_bitmask = 0;
for (unsigned i = 0; i < arraySize(_pins); i++) {
if (_pins[i] >= 0) {

37
src/drivers/camera_trigger/interfaces/src/pwm.h
View File

@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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 pwm.h
*
@ -25,10 +58,10 @@ public:
void info();
private:
int32_t _pwm_camera_shoot = 0;
int32_t _pwm_camera_neutral = 0;
void setup();
int32_t _pwm_camera_shoot{0};
int32_t _pwm_camera_neutral{0};
};
#endif /* ifdef __PX4_NUTTX */

54
src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp
View File

@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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.
*
****************************************************************************/
#ifdef __PX4_NUTTX
#include <sys/ioctl.h>
@ -17,9 +50,7 @@
#define PWM_2_CAMERA_KEEP_ALIVE 1700
#define PWM_2_CAMERA_ON_OFF 1900
CameraInterfaceSeagull::CameraInterfaceSeagull():
CameraInterface(),
_camera_is_on(false)
CameraInterfaceSeagull::CameraInterfaceSeagull()
{
get_pins();
setup();
@ -37,12 +68,15 @@ void CameraInterfaceSeagull::setup()
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
// Initialize the interface
uint8_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
uint32_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]);
up_pwm_trigger_init(pin_bitmask);
// Set both interface pins to disarmed
up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED);
int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
PX4_DEBUG("pwm trigger set %d %d=%d, ret=%d", i + 1, _pins[i + 1], PWM_CAMERA_DISARMED, ret1);
int ret2 = up_pwm_trigger_set(_pins[i], PWM_CAMERA_DISARMED);
PX4_DEBUG("pwm trigger set %d %d=%d, ret=%d", i, _pins[i], PWM_CAMERA_DISARMED, ret2);
// We only support 2 consecutive pins while using the Seagull MAP2
return;
@ -54,7 +88,6 @@ void CameraInterfaceSeagull::setup()
void CameraInterfaceSeagull::trigger(bool trigger_on_true)
{
if (!_camera_is_on) {
return;
}
@ -70,7 +103,6 @@ void CameraInterfaceSeagull::trigger(bool trigger_on_true)
void CameraInterfaceSeagull::send_keep_alive(bool enable)
{
// This should alternate between enable and !enable to keep the camera alive
if (!_camera_is_on) {
return;
}
@ -85,9 +117,7 @@ void CameraInterfaceSeagull::send_keep_alive(bool enable)
void CameraInterfaceSeagull::send_toggle_power(bool enable)
{
// This should alternate between enable and !enable to toggle camera power
for (unsigned i = 0; i < arraySize(_pins); i = i + 2) {
if (_pins[i] >= 0 && _pins[i + 1] >= 0) {
// Set channel 1 to neutral
@ -97,7 +127,9 @@ void CameraInterfaceSeagull::send_toggle_power(bool enable)
}
}
if (!enable) { _camera_is_on = !_camera_is_on; }
if (!enable) {
_camera_is_on = !_camera_is_on;
}
}
void CameraInterfaceSeagull::info()

35
src/drivers/camera_trigger/interfaces/src/seagull_map2.h
View File

@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 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 seagull_map2.h
*
@ -33,7 +66,7 @@ public:
private:
void setup();
bool _camera_is_on;
bool _camera_is_on{false};
};