diff --git a/msg/manual_control_switches.msg b/msg/manual_control_switches.msg index d140743761..d343a3ab52 100644 --- a/msg/manual_control_switches.msg +++ b/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 diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 02e7c3cc8c..1979b9f9db 100644 --- a/msg/vehicle_command.msg +++ b/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 diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index f6f54ad22f..a73073a8c4 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -33,6 +33,7 @@ #include "ManualControl.hpp" #include +#include 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(camera_mode); + command.target_system = _param_mav_sys_id.get(); + command.target_component = 100; // any camera + + uORB::Publication 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 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 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(); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index e6e8b1f3a9..c450ac9518 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -85,6 +85,15 @@ private: uORB::Publication _action_request_pub{ORB_ID(action_request)}; uORB::Publication _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_pub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -128,6 +137,10 @@ private: (ParamInt) _param_fltmode_3, (ParamInt) _param_fltmode_4, (ParamInt) _param_fltmode_5, - (ParamInt) _param_fltmode_6 + (ParamInt) _param_fltmode_6, + (ParamInt) _param_mav_sys_id ) + + unsigned _image_sequence {0}; + bool _video_recording {false}; // TODO: hopefully there is a command soon to toggle without keeping state }; diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 1c40495f0c..7bbb5ea19b 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/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); }