Plane: support MAV_CMD_DO_SET_CAM_TRIGG_DIST

This commit is contained in:
Andrew Tridgell 2013-10-11 21:30:01 +11:00
parent 0f0b040891
commit 0ec322ee47
2 changed files with 13 additions and 0 deletions

View File

@ -1385,6 +1385,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,
@ -1677,6 +1681,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_DO_DIGICAM_CONTROL:
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
// use alt so we can support 32 bit values
tell_command.alt = packet.param1;
break;
default:
result = MAV_MISSION_UNSUPPORTED;
break;

View File

@ -116,6 +116,10 @@ static void handle_process_do_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(next_nonnav_command.alt);
break;
#endif
#if MOUNT == ENABLED