void AP_Camera::control_cmd(const AP_Mission::Mission_Command& cmd)
{
// take picture
trigger_pic(false);
mavlink_message_t msg;
mavlink_command_long_t mav_cmd_long = {};
// convert command to mavlink command long
mav_cmd_long.target_system = 0;
mav_cmd_long.target_component = 0;
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONTROL;
mav_cmd_long.confirmation = 0;
mav_cmd_long.param1 = cmd.content.digicam_control.session;
mav_cmd_long.param2 = cmd.content.digicam_control.zoom_pos;
mav_cmd_long.param3 = cmd.content.digicam_control.zoom_step;
mav_cmd_long.param4 = cmd.content.digicam_control.focus_lock;
mav_cmd_long.param5 = cmd.content.digicam_control.shooting_cmd;
mav_cmd_long.param6 = cmd.content.digicam_control.cmd_id;
// Encode Command long into MAVLINK msg
mavlink_msg_command_long_encode(0, 0, &msg, &mav_cmd_long);
// send to all components
GCS_MAVLINK::send_to_components(&msg);
}
bool AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
{
bool ret = false;
// take picture
if (is_equal(shooting_cmd,1.0f)) {
trigger_pic(false);
ret = true;
}
mavlink_message_t msg;
mavlink_command_long_t mav_cmd_long = {};
// convert command to mavlink command long
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONTROL;
mav_cmd_long.param1 = session;
mav_cmd_long.param2 = zoom_pos;
mav_cmd_long.param3 = zoom_step;
mav_cmd_long.param4 = focus_lock;
mav_cmd_long.param5 = shooting_cmd;
mav_cmd_long.param6 = cmd_id;
// Encode Command long into MAVLINK msg
mavlink_msg_command_long_encode(0, 0, &msg, &mav_cmd_long);
// send to all components
GCS_MAVLINK::send_to_components(&msg);
return ret;
}
/// decode MavLink that controls camera
void
AP_Camera::control_msg(mavlink_message_t* msg)
{
__mavlink_digicam_control_t packet;
mavlink_msg_digicam_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// This values may or not be used by APM (the shot is)
// They are bypassed as "echo" to a external specialized board
/*
* packet.command_id
* packet.extra_param
* packet.extra_value
* packet.focus_lock
* packet.session
* packet.shot
* packet.zoom_pos
* packet.zoom_step
*/
if (packet.shot)
{
trigger_pic();
}
// echo the message to the ArduCam OSD camera control board
// for more info see: http://code.google.com/p/arducam-osd/
// TODO is it connected to MAVLINK_COMM_3 ?
mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value);
}
/// decode MavLink that controls camera
void
AP_Camera::control_msg(mavlink_message_t* msg)
{
__mavlink_digicam_control_t packet;
mavlink_msg_digicam_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// This values may or not be used by APM (the shot is)
// They are bypassed as "echo" to a external specialized board
/*
* packet.command_id
* packet.extra_param
* packet.extra_value
* packet.focus_lock
* packet.session
* packet.shot
* packet.zoom_pos
* packet.zoom_step
*/
if (packet.shot)
{
trigger_pic();
}
}
/// decode MavLink that controls camera
void
AP_Camera::control_msg(mavlink_message_t* msg)
{
__mavlink_digicam_control_t packet;
mavlink_msg_digicam_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
// TODO do something with these values
/*
packet.command_id
packet.extra_param
packet.extra_value
packet.focus_lock
packet.session
packet.shot
packet.zoom_pos
packet.zoom_step
*/
if (packet.shot)
{
trigger_pic();
}
// echo the message to the ArduCam OSD camera control board
// for more info see: http://code.google.com/p/arducam-osd/
// TODO is it connected to MAVLINK_COMM_3 ?
mavlink_msg_digicam_control_send(MAVLINK_COMM_3, packet.target_system, packet.target_component, packet.session, packet.zoom_pos, packet.zoom_step, packet.focus_lock, packet.shot, packet.command_id, packet.extra_param, packet.extra_value);
}
