camera_trigger: cleanup

- add copyright headers and update year
 - move initializers to header
 - add PX4_DEBUG messages

src/drivers/camera_trigger/CMakeLists.txt

@@ -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/camera_interface.cpp
-                interfaces/src/pwm.cpp
+		interfaces/src/pwm.cpp
-                interfaces/src/seagull_map2.cpp
+		interfaces/src/seagull_map2.cpp
-                interfaces/src/gpio.cpp
+		interfaces/src/gpio.cpp
 	DEPENDS
+		px4_work_queue
 	)
-

src/drivers/camera_trigger/camera_trigger.cpp

@@ -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 _engagecall {};
-	struct hrt_call		_disengagecall;
+	struct hrt_call _disengagecall {};
-	struct hrt_call     _engage_turn_on_off_call;
+	struct hrt_call _engage_turn_on_off_call {};
-	struct hrt_call     _disengage_turn_on_off_call;
+	struct hrt_call _disengage_turn_on_off_call {};
-	struct hrt_call		_keepalivecall_up;
+	struct hrt_call _keepalivecall_up {};
-	struct hrt_call		_keepalivecall_down;
+	struct hrt_call _keepalivecall_down {};
 
-	float			_activation_time;
+	float			_activation_time{0.5f};
-	float			_interval;
+	float			_interval{100.f};
-	float			_min_interval;
+	float			_min_interval{1.f};
-	float 			_distance;
+	float 			_distance{25.f};
-	uint32_t 		_trigger_seq;
+	uint32_t 		_trigger_seq{0};
-	hrt_abstime		_last_trigger_timestamp;
+	hrt_abstime		_last_trigger_timestamp{0};
-	bool			_trigger_enabled;
+	bool			_trigger_enabled{false};
-	bool			_trigger_paused;
+	bool			_trigger_paused{false};
-	bool			_one_shot;
+	bool			_one_shot{false};
-	bool			_test_shot;
+	bool			_test_shot{false};
-	bool 			_turning_on;
+	bool 			_turning_on{false};
-	matrix::Vector2f	_last_shoot_position;
+	matrix::Vector2f	_last_shoot_position{0.f, 0.f};
-	bool			_valid_position;
+	bool			_valid_position{false};
 
 	//Camera Auto Mount Pivoting Oblique Survey (CAMPOS)
-	uint32_t		_CAMPOS_num_poses;
+	uint32_t		_CAMPOS_num_poses{0};
-	uint32_t		_CAMPOS_pose_counter;
+	uint32_t		_CAMPOS_pose_counter{0};
-	float			_CAMPOS_roll_angle;
+	float			_CAMPOS_roll_angle{0.f};
-	float			_CAMPOS_angle_interval;
+	float			_CAMPOS_angle_interval{0.f};
-	float			_CAMPOS_pitch_angle;
+	float			_CAMPOS_pitch_angle{-90.f};
-	bool			_CAMPOS_updated_roll_angle;
+	bool			_CAMPOS_updated_roll_angle{false};
-	uint32_t		_target_system;
+	uint32_t		_target_system{0};
-	uint32_t		_target_component;
+	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;
+	trigger_mode_t		_trigger_mode{TRIGGER_MODE_NONE};
-	int32_t _cam_cap_fback;
+	int32_t _cam_cap_fback{0};
 
-	camera_interface_mode_t	_camera_interface_mode;
+	camera_interface_mode_t	_camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
-	CameraInterface		*_camera_interface;  ///< instance of camera interface
+	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),
+	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)
 {
 	// 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
+		PX4_DEBUG("update intervalometer, trigger enabled: %d, trigger paused: %d", _trigger_enabled, _trigger_paused);
-		hrt_call_every(&_engagecall, 0, (_interval * 1000),
-			       (hrt_callout)&CameraTrigger::engage, this);
 
 		// schedule trigger on and off calls
-		hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
+		hrt_call_every(&_engagecall, 0, (_interval * 1000), &CameraTrigger::engage, this);
-			       (hrt_callout)&CameraTrigger::disengage, 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
+		PX4_DEBUG("keep alive enable");
-		hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
-			       (hrt_callout)&CameraTrigger::keep_alive_up, this);
 
-		hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
+		// schedule keep-alive up and down calls
-			       (hrt_callout)&CameraTrigger::keep_alive_down, this);
+		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
+	PX4_DEBUG("toggle power");
-	hrt_call_after(&_engage_turn_on_off_call, 0,
-		       (hrt_callout)&CameraTrigger::engange_turn_on_off, this);
 
-	hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
+	// schedule power toggle calls
-		       (hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
+	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_call_after(&_disengagecall, 0 + (_activation_time * 1000), &CameraTrigger::disengage, this);
-		       (hrt_callout)&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 = {};
+	timespec tv{};
-
-	// Set timestamp the instant after the trigger goes off
-	trigger.timestamp = now;
-
-	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);
-}

src/drivers/camera_trigger/camera_trigger_params.c

@@ -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

src/drivers/camera_trigger/interfaces/src/camera_interface.cpp

@@ -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");
 

src/drivers/camera_trigger/interfaces/src/camera_interface.h

@@ -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:
-
+	CameraInterface() = default;
-	/**
-	 * Constructor
-	 */
-	CameraInterface();
-
-	/**
-	 * Destructor.
-	 */
 	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] {};
 
 };

src/drivers/camera_trigger/interfaces/src/gpio.cpp

@@ -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():
+CameraInterfaceGPIO::CameraInterfaceGPIO()
-	CameraInterface(),
-	_trigger_invert(false),
-	_triggers{}
 {
 	_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) {
-		if (_triggers[i] == 0) { break; }
+			px4_arch_gpiowrite(_triggers[i], trigger_state);
-
+		}
-		px4_arch_gpiowrite(_triggers[i], trigger_state);
 	}
 }
 

src/drivers/camera_trigger/interfaces/src/gpio.h

@@ -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 */

src/drivers/camera_trigger/interfaces/src/pwm.cpp

@@ -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) {

src/drivers/camera_trigger/interfaces/src/pwm.h

@@ -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 */

src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp

@@ -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():
+CameraInterfaceSeagull::CameraInterfaceSeagull()
-	CameraInterface(),
-	_camera_is_on(false)
 {
 	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);
+			int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED);
-			up_pwm_trigger_set(_pins[i], 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()

src/drivers/camera_trigger/interfaces/src/seagull_map2.h

@@ -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};
 
 };