From 7365701d1354a09a6cbc3f3f218c97ef8fb2303c Mon Sep 17 00:00:00 2001 From: squilter Date: Sun, 6 Sep 2015 16:44:33 -0400 Subject: [PATCH] Plane: Support do_digicam_x via command_long --- ArduPlane/GCS_Mavlink.cpp | 29 +++++++++++++++++++++++++++++ ArduPlane/commands_logic.cpp | 15 +++++++++++++-- 2 files changed, 42 insertions(+), 2 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index fd6717355d..e4ea43710e 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1161,6 +1161,31 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; #endif +#if CAMERA == ENABLED + case MAV_CMD_DO_DIGICAM_CONFIGURE: + plane.camera.configure(packet.param1, + packet.param2, + packet.param3, + packet.param4, + packet.param5, + packet.param6, + packet.param7); + + result = MAV_RESULT_ACCEPTED; + break; + + case MAV_CMD_DO_DIGICAM_CONTROL: + plane.camera.control(packet.param1, + packet.param2, + packet.param3, + packet.param4, + packet.param5, + packet.param6); + + result = MAV_RESULT_ACCEPTED; + break; +#endif // CAMERA == ENABLED + case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED plane.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); @@ -1714,11 +1739,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } #if CAMERA == ENABLED + //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { break; } + //deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL case MAVLINK_MSG_ID_DIGICAM_CONTROL: { plane.camera.control_msg(msg); @@ -1728,12 +1755,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // CAMERA == ENABLED #if MOUNT == ENABLED + //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { plane.camera_mount.configure_msg(msg); break; } + //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { plane.camera_mount.control_msg(msg); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7762e9cb7a..45f1d083ac 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -863,7 +863,13 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) void Plane::do_digicam_configure(const AP_Mission::Mission_Command& cmd) { #if CAMERA == ENABLED - camera.configure_cmd(cmd); + camera.configure(cmd.content.digicam_configure.shooting_mode, + cmd.content.digicam_configure.shutter_speed, + cmd.content.digicam_configure.aperture, + cmd.content.digicam_configure.ISO, + cmd.content.digicam_configure.exposure_type, + cmd.content.digicam_configure.cmd_id, + cmd.content.digicam_configure.engine_cutoff_time); #endif } @@ -871,7 +877,12 @@ void Plane::do_digicam_configure(const AP_Mission::Mission_Command& cmd) void Plane::do_digicam_control(const AP_Mission::Mission_Command& cmd) { #if CAMERA == ENABLED - camera.control_cmd(cmd); + camera.control(cmd.content.digicam_control.session, + cmd.content.digicam_control.zoom_pos, + cmd.content.digicam_control.zoom_step, + cmd.content.digicam_control.focus_lock, + cmd.content.digicam_control.shooting_cmd, + cmd.content.digicam_control.cmd_id); log_picture(); #endif }