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); }