From 3b9566bc09a8ebdbb2cec9e961888f15a3e44b56 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Oct 2013 21:34:23 +1100 Subject: [PATCH] Copter: support MAV_CMD_DO_SET_CAM_TRIGG_DIST --- ArduCopter/GCS_Mavlink.pde | 9 +++++++++ ArduCopter/commands_logic.pde | 4 ++++ 2 files changed, 13 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 090ab6a1fa..26336e8f92 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1406,6 +1406,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) param2 = tell_command.alt; param1 = tell_command.p1; break; + + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + param1 = tell_command.alt; + break; } mavlink_msg_mission_item_send(chan,msg->sysid, @@ -1707,6 +1711,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) tell_command.alt = packet.param2; tell_command.p1 = packet.param1; break; + + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + // use alt so we can support 32 bit values + tell_command.alt = packet.param1; + break; } if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index a7831ef934..a5193b03fa 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -117,6 +117,10 @@ static void process_now_command() case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| do_take_picture(); break; + + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + camera.set_trigger_distance(command_cond_queue.alt); + break; #endif #if MOUNT == ENABLED