manual_control: enable sending camera commands

msg/manual_control_switches.msg

@@ -26,4 +26,7 @@ uint8 kill_switch                # throttle kill: _NORMAL_, KILL
 uint8 gear_switch                # landing gear switch: _DOWN_, UP
 uint8 transition_switch          # VTOL transition switch: _HOVER, FORWARD_FLIGHT
 
+uint8 photo_switch               # Photo trigger switch
+uint8 video_switch               # Photo trigger switch
+
 uint32 switch_changes            # number of switch changes

msg/vehicle_command.msg

@@ -80,7 +80,10 @@ uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531                # Set camera zoom
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control
+uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000   # Start image capture sequence.
 uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-board camera triggering system
+uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500   # Start a video capture.
+uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501    # Stop the current video capture.
 uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
 uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600	# control starting/stopping transmitting data over the high latency link

src/modules/manual_control/ManualControl.cpp

@@ -33,6 +33,7 @@
 
 #include "ManualControl.hpp"
 #include <uORB/topics/commander_state.h>
+#include <uORB/topics/vehicle_command.h>
 
 ManualControl::ManualControl() :
 	ModuleParams(nullptr),
@@ -200,6 +201,20 @@ void ManualControl::Run()
 						}
 					}
 
+					if (switches.photo_switch != _previous_switches.photo_switch) {
+						if (switches.photo_switch == manual_control_switches_s::SWITCH_POS_ON) {
+							send_camera_mode_command(CameraMode::Image);
+							send_photo_command();
+						}
+					}
+
+					if (switches.video_switch != _previous_switches.video_switch) {
+						if (switches.video_switch == manual_control_switches_s::SWITCH_POS_ON) {
+							send_camera_mode_command(CameraMode::Video);
+							send_video_command();
+						}
+					}
+
 				} else {
 					// Send an initial request to switch to the mode requested by RC
 					evaluateModeSlot(switches.mode_slot);
@@ -341,6 +356,55 @@ void ManualControl::publishLandingGear(int8_t action)
 	_landing_gear_pub.publish(landing_gear);
 }
 
+void ManualControl::send_camera_mode_command(CameraMode camera_mode)
+{
+	vehicle_command_s command{};
+	command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE;
+	command.param2 = static_cast<float>(camera_mode);
+	command.target_system = _param_mav_sys_id.get();
+	command.target_component = 100; // any camera
+
+	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
+	command.timestamp = hrt_absolute_time();
+	command_pub.publish(command);
+}
+
+void ManualControl::send_photo_command()
+{
+	vehicle_command_s command{};
+	command.command = vehicle_command_s::VEHICLE_CMD_IMAGE_START_CAPTURE;
+	command.param3 = 1; // one picture
+	command.param4 = _image_sequence++;
+	command.target_system = _param_mav_sys_id.get();
+	command.target_component = 100; // any camera
+
+	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
+	command.timestamp = hrt_absolute_time();
+	command_pub.publish(command);
+}
+
+void ManualControl::send_video_command()
+{
+	vehicle_command_s command{};
+
+	if (_video_recording) {
+		command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_STOP_CAPTURE;
+		command.param2 = 1; // status at 1 Hz
+
+	} else {
+		command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_START_CAPTURE;
+	}
+
+	command.target_system = _param_mav_sys_id.get();
+	command.target_component = 100; // any camera
+
+	uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
+	command.timestamp = hrt_absolute_time();
+	command_pub.publish(command);
+
+	_video_recording = !_video_recording;
+}
+
 int ManualControl::task_spawn(int argc, char *argv[])
 {
 	ManualControl *instance = new ManualControl();

src/modules/manual_control/ManualControl.hpp

@@ -85,6 +85,15 @@ private:
 
 	uORB::Publication<action_request_s> _action_request_pub{ORB_ID(action_request)};
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
+
+	enum class CameraMode {
+		Image = 0,
+		Video = 1
+	};
+	void send_camera_mode_command(CameraMode camera_mode);
+	void send_photo_command();
+	void send_video_command();
+
 	uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -128,6 +137,10 @@ private:
 		(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
 		(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
 		(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
-		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
+		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id
 	)
+
+	unsigned _image_sequence {0};
+	bool _video_recording {false}; // TODO: hopefully there is a command soon to toggle without keeping state
 };

src/modules/rc_update/rc_update.cpp

@@ -54,7 +54,9 @@ static bool operator ==(const manual_control_switches_s &a, const manual_control
 		a.kill_switch == b.kill_switch &&
 		a.arm_switch == b.arm_switch &&
 		a.transition_switch == b.transition_switch &&
-		a.gear_switch == b.gear_switch);
+		a.gear_switch == b.gear_switch &&
+		a.photo_switch == b.photo_switch &&
+		a.video_switch == b.video_switch);
 }
 
 static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); }