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);
}
/// single entry point to take pictures
/// set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components
void
AP_Camera::trigger_pic(bool send_mavlink_msg)
{
_image_index++;
switch (_trigger_type)
{
case AP_CAMERA_TRIGGER_TYPE_SERVO:
servo_pic(); // Servo operated camera
break;
case AP_CAMERA_TRIGGER_TYPE_RELAY:
relay_pic(); // basic relay activation
break;
}
if (send_mavlink_msg) {
// create command long mavlink message
mavlink_command_long_t cmd_msg;
memset(&cmd_msg, 0, sizeof(cmd_msg));
cmd_msg.command = MAV_CMD_DO_DIGICAM_CONTROL;
cmd_msg.param5 = 1;
// create message
mavlink_message_t msg;
mavlink_msg_command_long_encode(0, 0, &msg, &cmd_msg);
// forward 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;
}
// Mission command processing
void AP_Camera::configure_cmd(const AP_Mission::Mission_Command& cmd)
{
// we cannot process the configure command so convert to mavlink message
// and send to all components in case they and process it
mavlink_message_t msg;
mavlink_command_long_t mav_cmd_long = {};
// convert mission 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_CONFIGURE;
mav_cmd_long.confirmation = 0;
mav_cmd_long.param1 = cmd.content.digicam_configure.shooting_mode;
mav_cmd_long.param2 = cmd.content.digicam_configure.shutter_speed;
mav_cmd_long.param3 = cmd.content.digicam_configure.aperture;
mav_cmd_long.param4 = cmd.content.digicam_configure.ISO;
mav_cmd_long.param5 = cmd.content.digicam_configure.exposure_type;
mav_cmd_long.param6 = cmd.content.digicam_configure.cmd_id;
mav_cmd_long.param7 = cmd.content.digicam_configure.engine_cutoff_time;
// 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);
}
void ofxMavlink::sendArmCommand(ArmCommand arm)
{
char buf[MAVLINK_MAX_PACKET_LEN];
mavlink_message_t message;
mavlink_command_long_t cmd;
cmd.command = (uint16_t) MAV_CMD_COMPONENT_ARM_DISARM;
cmd.confirmation = 0;
cmd.param1 = arm;
cmd.param2 = 0.0f;
cmd.param3 = 0.0f;
cmd.param4 = 0.0f;
cmd.param5 = 0.0f;
cmd.param6 = 0.0f;
cmd.param7 = 0.0f;
cmd.target_component = MAV_COMP_ID_SYSTEM_CONTROL;
cmd.target_system = targetId;
mavlink_msg_command_long_encode(sysId, compId, &message, &cmd);
cout << "Send ARM request to drone" << endl;
unsigned len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
/* write packet via serial link */
int returnval = write(fd, buf, len);
if(returnval < 0) ofLogError("ofxMavlink") << "Error writing to serial port";
/* wait until all data has been written */
tcdrain(fd);
}
// ------------------------------------------------------------------------------
// Set nav waypoint
// ------------------------------------------------------------------------------
int
Autopilot_Interface::
set_nav_waypoint( bool flag, u_int64_t latitude, u_int64_t longitude, u_int64_t altitude )
{
// Prepare command for off-board mode
mavlink_command_long_t com = { 0 };
com.target_system = system_id;
com.target_component = autopilot_id;
com.command = MAV_CMD_NAV_WAYPOINT;
com.confirmation = true;
com.param1 = 100; // flag >0.5 => start, <0.5 => stop
com.param2 = 5; //triger radius/m
com.param5 = latitude;
com.param6 = longitude;
com.param7 = altitude;
// Encode
mavlink_message_t message;
mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);
// Send the message
int len = serial_port->write_message(message);
// Done!
return len;
}
// ------------------------------------------------------------------------------
// take_off
// ------------------------------------------------------------------------------
int
Autopilot_Interface::
take_off(bool flag)
{
// Prepare command for off-board mode
mavlink_command_long_t com = { 0 };
com.target_system = system_id;
com.target_component = autopilot_id;
com.command = MAV_CMD_NAV_TAKEOFF;
com.confirmation = true;
//com.param1 = (float) flag; // flag >0.5 => start, <0.5 => stop
//com.param5 =0;
//com.param6 =0;
com.param7 =10;
// Encode
mavlink_message_t message;
mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);
// Send the message
int len = serial_port->write_message(message);
// Done!
return len;
}
void _Mavlink::command_long_doSetMode(int mode)
{
mavlink_message_t message;
mavlink_command_long_t ds;
ds.target_system = m_systemID;
ds.target_component = m_targetComponentID;
ds.command = MAV_CMD_DO_SET_MODE;
ds.param1 = mode;
mavlink_msg_command_long_encode(m_systemID, m_componentID, &message, &ds);
writeMessage(message);
LOG_I("<- COMMAND_LONG: MAV_CMD_DO_SET_MODE");
return;
}
int _MavlinkInterface::toggleOffboardControl(bool bEnable)
{
if(m_bControlling == bEnable)return -1;
// Prepare command for off-board mode
mavlink_command_long_t com;
com.target_system = system_id;
com.target_component = autopilot_id;
com.command = MAV_CMD_NAV_GUIDED_ENABLE;
com.confirmation = true;
com.param1 = (float) bEnable; // flag >0.5 => start, <0.5 => stop
// Encode
mavlink_message_t message;
mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);
// Send the message
return writeMessage(message);
}
void _Mavlink::command_long_doSetPositionYawThrust(float steer, float thrust)
{
mavlink_message_t message;
mavlink_command_long_t ds;
ds.target_system = m_systemID;
ds.target_component = m_targetComponentID;
ds.command = 213; //MAV_CMD_DO_SET_POSITION_YAW_THRUST;
ds.confirmation = 0;
ds.param1 = steer;
ds.param2 = thrust;
mavlink_msg_command_long_encode(m_systemID, m_targetComponentID, &message,
&ds);
writeMessage(message);
LOG_I("<- COMMAND_LONG: MAV_CMD_DO_SET_POSITION_YAW_THRUST");
return;
}
// Check's each smart battery instance for its powering off state and broadcasts notifications
void AP_BattMonitor::checkPoweringOff(void)
{
for (uint8_t i = 0; i < _num_instances; i++) {
if (state[i].is_powering_off && !state[i].powerOffNotified) {
// Set the AP_Notify flag, which plays the power off tones
AP_Notify::flags.powering_off = true;
// Send a Mavlink broadcast announcing the shutdown
mavlink_message_t msg;
mavlink_command_long_t cmd_msg{};
cmd_msg.command = MAV_CMD_POWER_OFF_INITIATED;
cmd_msg.param1 = i+1;
mavlink_msg_command_long_encode(mavlink_system.sysid, MAV_COMP_ID_ALL, &msg, &cmd_msg);
GCS_MAVLINK::send_to_components(&msg);
gcs().send_text(MAV_SEVERITY_WARNING, "Vehicle %d battery %d is powering off", mavlink_system.sysid, i+1);
// only send this once
state[i].powerOffNotified = true;
}
}
}
// ------------------------------------------------------------------------------
// Toggle Off-Board Mode
// ------------------------------------------------------------------------------
int
Autopilot_Interface::
toggle_offboard_control( bool flag )
{
// Prepare command for off-board mode
mavlink_command_long_t com;
com.target_system = system_id;
com.target_component = autopilot_id;
com.param1 = (float) flag; // flag >0.5 => start, <0.5 => stop
com.command = MAV_CMD_NAV_GUIDED_ENABLE;
// Encode
mavlink_message_t message;
mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);
// Send the message
int len = serial_port->write_message(message);
// Done!
return len;
}
// ----------------------------------------------------------------------------------
// Enable aircraft landing
// ----------------------------------------------------------------------------------
void
Autopilot_Interface::
set_landing()
{
// Prepare command for landing
mavlink_command_long_t com = { 0 };
com.target_system = system_id;
com.target_component = autopilot_id;
com.command = MAV_CMD_NAV_LAND;
com.confirmation = true;
com.param1 = (float) flag; // flag >0.5 => start, <0.5 => stop
// Encode
mavlink_message_t message;
mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);
// Send the message
int len = serial_port->write_message(message);
// Done!
return;
}
void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
{
// we cannot process the configure command so convert to mavlink message
// and send to all components in case they and process it
mavlink_message_t msg;
mavlink_command_long_t mav_cmd_long = {};
// convert mission command to mavlink command_long
mav_cmd_long.command = MAV_CMD_DO_DIGICAM_CONFIGURE;
mav_cmd_long.param1 = shooting_mode;
mav_cmd_long.param2 = shutter_speed;
mav_cmd_long.param3 = aperture;
mav_cmd_long.param4 = ISO;
mav_cmd_long.param5 = exposure_type;
mav_cmd_long.param6 = cmd_id;
mav_cmd_long.param7 = engine_cutoff_time;
// 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);
}