void command_long(bool broadcast, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) { mavlink_message_t msg; const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system(); const uint8_t tgt_comp_id = (broadcast) ? 0 : (use_comp_id_system_control) ? MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component(); const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation; mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg, tgt_sys_id, tgt_comp_id, command, confirmation_fixed, param1, param2, param3, param4, param5, param6, param7); UAS_FCU(uas)->send_message(&msg); }
/** * @brief Send a safety zone (volume), which is defined by two corners of a cube, * to the FCU. * * @note ENU frame. */ void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2) { ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safty area: P1 " << p1 << " P2 " << p2); p1 = ftf::transform_frame_enu_ned(p1); p2 = ftf::transform_frame_enu_ned(p2); mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s; m_uas->msg_set_target(s); // TODO: use enum from lib s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED); // [[[cog: // for p in range(1, 3): // for v in ('x', 'y', 'z'): // cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v)) // ]]] s.p1x = p1.x(); s.p1y = p1.y(); s.p1z = p1.z(); s.p2x = p2.x(); s.p2y = p2.y(); s.p2z = p2.z(); // [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0) UAS_FCU(m_uas)->send_message_ignore_drop(s); }
void override_cb(const mavros_msgs::OverrideRCIn::ConstPtr req) { if (!m_uas->is_ardupilotmega() && !m_uas->is_px4()) ROS_WARN_THROTTLE_NAMED(30, "rc", "RC override not supported by this FCU!"); mavlink::common::msg::RC_CHANNELS_OVERRIDE ovr; ovr.target_system = m_uas->get_tgt_system(); ovr.target_component = m_uas->get_tgt_component(); // [[[cog: // for i in range(1, 9): // cog.outl("ovr.chan%d_raw = req->channels[%d];" % (i, i - 1)) // ]]] ovr.chan1_raw = req->channels[0]; ovr.chan2_raw = req->channels[1]; ovr.chan3_raw = req->channels[2]; ovr.chan4_raw = req->channels[3]; ovr.chan5_raw = req->channels[4]; ovr.chan6_raw = req->channels[5]; ovr.chan7_raw = req->channels[6]; ovr.chan8_raw = req->channels[7]; // [[[end]]] (checksum: bd27f3e85f5ab614ce1332ae3f4c6ebd) UAS_FCU(m_uas)->send_message_ignore_drop(ovr); }
void command_int(bool broadcast, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) { mavlink_message_t msg; const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system(); const uint8_t tgt_comp_id = (broadcast) ? 0 : (use_comp_id_system_control) ? MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component(); mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg, tgt_sys_id, tgt_comp_id, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); UAS_FCU(uas)->send_message(&msg); }
/* * Arming/disarming the UAV */ void arming(const mms::Arm::ConstPtr msg){ mavlink_message_t msg_mav; if (msg->arm_disarm){ enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM; float param1 = 1; //1-->arm 0-->disarm float param2 = 0; //not used float param3 = 0; //not used float param4 = 0; //not used float param5 = 0; //not used float param6 = 0; //not used float param7 = 0; //not used uint8_t confirmation = 1; mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav, uas->get_tgt_system(), uas->get_tgt_component(), command, confirmation, param1, param2, param3, param4, param5, param6, param7); UAS_FCU(uas)->send_message(&msg_mav); ROS_INFO("Arming UAV"); } else { enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM; float param1 = 0; //1-->arm 0-->disarm float param2 = 0; //not used float param3 = 0; //not used float param4 = 0; //not used float param5 = 0; //not used float param6 = 0; //not used float param7 = 0; //not used uint8_t confirmation = 1; mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav, uas->get_tgt_system(), uas->get_tgt_component(), command, confirmation, param1, param2, param3, param4, param5, param6, param7); UAS_FCU(uas)->send_message(&msg_mav); //TODO decide if send or not disarm by software ROS_INFO("Disarming UAV"); } }
void vision_speed_estimate(uint64_t usec, float x, float y, float z) { mavlink_message_t msg; mavlink_msg_vision_speed_estimate_pack_chan(UAS_PACK_CHAN(uas), &msg, usec, x, y, z); UAS_FCU(uas)->send_message(&msg); }
void laser_distance_send(float a, float b, float c, float d) { mavlink_message_t laser_distance_msg; mavlink_msg_laser_distance_pack_chan(UAS_PACK_CHAN(uas),&laser_distance_msg,a,b,c,d); //pack UAS_FCU(uas)->send_message(&laser_distance_msg); //send ROS_INFO("float_b %d %d", laser_distance_msg.seq,laser_distance_msg.len); }
void pump_controller_send(float a, float b){ mavlink_message_t pump_controller_msg; mavlink_msg_pump_controller_pack_chan(UAS_PACK_CHAN(uas),&pump_controller_msg,a,b); //pack UAS_FCU(uas)->send_message(&pump_controller_msg); //send //ROS_INFO("size %d %f", field_size_msg.seq, a); }
void send_timesync_msg(uint64_t tc1, uint64_t ts1) { mavlink_message_t msg; mavlink_msg_timesync_pack_chan(UAS_PACK_CHAN(uas), &msg, tc1, ts1 ); UAS_FCU(uas)->send_message(&msg); }
void sonar_distance_send(float a, float b, float c, float d, float e, float f, float g){ mavlink_message_t sonar_distance_msg; mavlink_msg_sonar_distance_pack_chan(UAS_PACK_CHAN(uas),&sonar_distance_msg,a,b,c,d,e,f,g); //pack UAS_FCU(uas)->send_message(&sonar_distance_msg); //send //mavlink_msg_sonar_distance_send(MAVLINK_COMM_1,a,b,c,d,e,f); ROS_INFO("float_a %d %d", sonar_distance_msg.seq,sonar_distance_msg.len); //uas->mav_link->send_message(&msg); }
void heartbeat_cb(const ros::TimerEvent &event) { mavlink_message_t msg; mavlink_msg_heartbeat_pack_chan(UAS_PACK_CHAN(uas), &msg, MAV_TYPE_ONBOARD_CONTROLLER, MAV_AUTOPILOT_INVALID, MAV_MODE_MANUAL_ARMED, 0, MAV_STATE_ACTIVE ); UAS_FCU(uas)->send_message(&msg); }
void sys_time_cb(const ros::TimerEvent &event) { // For filesystem only mavlink_message_t msg; uint64_t time_unix_usec = ros::Time::now().toNSec() / 1000; // nano -> micro mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg, time_unix_usec, 0 ); UAS_FCU(uas)->send_message(&msg); }
/* * From directive to RC */ void directive_cb(const guidance_node_amsl::Directive::ConstPtr msg){ /* * Initializing values, if safety is on, these values will remain */ velocity_.channels[0]=0; velocity_.channels[1]=0; velocity_.channels[2]=0; velocity_.channels[3]=0; velocity_.channels[4]=0; velocity_.channels[5]=0; velocity_.channels[6]=0; velocity_.channels[7]=0; if(!safetyOn){ /* * If safety is off, I translate velocity in RC values */ uint16_t vx_RC= (uint16_t)400.0f*(-msg->vxBody)/v_xy_max + 1520; //New: 400 + 1520; Old: 500 + 1500 uint16_t vy_RC=(uint16_t)400.0f*(msg->vyBody)/v_xy_max + 1520; //New: 400 + 1520; Old: 500 + 1500 /* * it seems it loiters with 1420 instead of 1500... */ uint16_t vz_RC= 1420; if (msg->vzBody > 0){ //going down, mapped only in 420us vz_RC = vz_RC + (uint16_t)320.0f*(-msg->vzBody)/v_z_max; //New: 320; Old: 420 } else { //going up, mapped in 580us vz_RC = vz_RC + (uint16_t)480.0f*(-msg->vzBody)/v_z_max; //New: 480 Old: 580 } uint16_t v_psi_RC = (uint16_t)400.0f*(msg->yawRate)/v_psi_max + 1520; //New: 400 + 1520; Old: 500 + 1500 velocity_.channels[0]=vy_RC; velocity_.channels[1]=vx_RC; velocity_.channels[2]=vz_RC; velocity_.channels[3]=v_psi_RC; } //DEBUG /*ROS_INFO("RC_OVERRIDE: [CH1:%u, CH2:%u, CH3:%u, CH4:%u, CH5:%u, CH6:%u, CH7:%u, CH8:%u]", velocity_.channels[0], velocity_.channels[1], velocity_.channels[2], velocity_.channels[3], velocity_.channels[4], velocity_.channels[5], velocity_.channels[6], velocity_.channels[7]);*/ //velocity_publisher_.publish(velocity_); mavlink_message_t msg_mav; mavlink_msg_rc_channels_raw_pack_chan(UAS_PACK_CHAN(uas),&msg_mav,0,1,velocity_.channels[0],velocity_.channels[1],velocity_.channels[2],velocity_.channels[3],0,0,0,0,100); //1 is the sequence that we are not considering right now UAS_FCU(uas)->send_message(&msg_mav); }
//! message definiton here: @p https://pixhawk.ethz.ch/mavlink/#SET_ACTUATOR_CONTROL_TARGET void set_actuator_control_target(const uint64_t time_usec, const uint8_t group_mix, const float controls[8]) { mavlink_message_t msg; mavlink_msg_set_actuator_control_target_pack_chan(UAS_PACK_CHAN(uas), &msg, time_usec, group_mix, UAS_PACK_TGT(uas), controls); UAS_FCU(uas)->send_message(&msg); }
bool set_rate_cb(mavros::StreamRate::Request &req, mavros::StreamRate::Response &res) { mavlink_message_t msg; mavlink_msg_request_data_stream_pack_chan(UAS_PACK_CHAN(uas), &msg, UAS_PACK_TGT(uas), req.stream_id, req.message_rate, (req.on_off) ? 1 : 0 ); UAS_FCU(uas)->send_message(&msg); return true; }
// perhaps better add send_ prefix, naming: message name in lower case void vision_position_estimate(float x, float y, float z, float roll, float pitch, float yaw) { mavlink_message_t msg; mavlink_msg_vision_position_estimate_pack(0, 0, &msg, 0, x, y, z, roll, pitch, yaw); //uas->mav_link->send_message(&msg); UAS_FCU(uas)->send_message(&msg); }
void vicon_send( float x, float y, float z, float vx, float vy, float vz) { mavlink_message_t vicon_msg; mavlink_msg_vicon_pack_chan(UAS_PACK_CHAN(uas), &vicon_msg, x, y, z, vx, vy, vz); UAS_FCU(uas)->send_message(&vicon_msg); ROS_INFO("send"); }
void vision_position_estimate(uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_message_t msg; mavlink_msg_vision_position_estimate_pack_chan(UAS_PACK_CHAN(uas), &msg, usec, x, y, z, roll, pitch, yaw); UAS_FCU(uas)->send_message(&msg); }
/* -*- low-level send -*- */ void mocap_pose_send (uint64_t usec, float q[4], float x, float y, float z) { mavlink_message_t msg; mavlink_msg_att_pos_mocap_pack_chan(UAS_PACK_CHAN(uas), &msg, usec, q, x, y, z); UAS_FCU(uas)->send_message(&msg); }
/** * @brief Handle mavros_msgs::RTCM message * It converts the message to the MAVLink GPS_RTCM_DATA message for GPS injection. * Message specification: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA * @param msg Received ROS msg */ void rtcm_cb(const mavros_msgs::RTCM::ConstPtr &msg) { mavlink::common::msg::GPS_RTCM_DATA rtcm_data; const size_t max_frag_len = rtcm_data.data.size(); uint8_t seq_u5 = uint8_t(msg->header.seq & 0x1F) << 3; if (msg->data.size() > 4 * max_frag_len) { ROS_FATAL("gps_rtk: RTCM message received is bigger than the maximal possible size."); return; } auto data_it = msg->data.begin(); auto end_it = msg->data.end(); if (msg->data.size() <= max_frag_len) { rtcm_data.len = msg->data.size(); rtcm_data.flags = seq_u5; std::copy(data_it, end_it, rtcm_data.data.begin()); std::fill(rtcm_data.data.begin() + rtcm_data.len, rtcm_data.data.end(), 0); UAS_FCU(m_uas)->send_message(rtcm_data); } else { for (uint8_t fragment_id = 0; fragment_id < 4 && data_it < end_it; fragment_id++) { uint8_t len = std::min((size_t) std::distance(data_it, end_it), max_frag_len); rtcm_data.flags = 1; // LSB set indicates message is fragmented rtcm_data.flags |= fragment_id << 1; // Next 2 bits are fragment id rtcm_data.flags |= seq_u5; // Next 5 bits are sequence id rtcm_data.len = len; std::copy(data_it, data_it + len, rtcm_data.data.begin()); std::fill(rtcm_data.data.begin() + len, rtcm_data.data.end(), 0); UAS_FCU(m_uas)->send_message(rtcm_data); std::advance(data_it, len); } } }
void set_attitude_target(uint32_t time_boot_ms, uint8_t type_mask, float q[4], float roll_rate, float pitch_rate, float yaw_rate, float thrust) { mavlink_message_t msg; mavlink_msg_set_attitude_target_pack_chan(UAS_PACK_CHAN(uas), &msg, time_boot_ms, UAS_PACK_TGT(uas), type_mask, q, roll_rate, pitch_rate, yaw_rate, thrust); UAS_FCU(uas)->send_message(&msg); }
void vision_speed_estimate(uint64_t usec, Eigen::Vector3d &v) { mavlink::common::msg::VISION_SPEED_ESTIMATE vs{}; vs.usec = usec; // [[[cog: // for f in "xyz": // cog.outl("vs.%s = v.%s();" % (f, f)) // ]]] vs.x = v.x(); vs.y = v.y(); vs.z = v.z(); // [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb) UAS_FCU(m_uas)->send_message_ignore_drop(vs); }
/* -*- low-level send -*- */ void distance_sensor(uint32_t time_boot_ms, uint32_t min_distance, uint32_t max_distance, uint32_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) { mavlink_message_t msg; mavlink_msg_distance_sensor_pack_chan(UAS_PACK_CHAN(uas), &msg, time_boot_ms, min_distance, max_distance, current_distance, type, id, orientation, covariance); UAS_FCU(uas)->send_message(&msg); }
bool set_mode_cb(mavros::SetMode::Request &req, mavros::SetMode::Response &res) { mavlink_message_t msg; uint8_t base_mode = req.base_mode; uint32_t custom_mode = 0; if (req.custom_mode != "") { if (!uas->cmode_from_str(req.custom_mode, custom_mode)) { res.success = false; return true; } base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg, uas->get_tgt_system(), base_mode, custom_mode); UAS_FCU(uas)->send_message(&msg); res.success = true; return true; }
void vision_position_estimate(uint64_t usec, Eigen::Vector3d &position, Eigen::Vector3d &rpy) { mavlink::common::msg::VISION_POSITION_ESTIMATE vp{}; vp.usec = usec; // [[[cog: // for f in "xyz": // cog.outl("vp.%s = position.%s();" % (f, f)) // for a, b in zip("xyz", ('roll', 'pitch', 'yaw')): // cog.outl("vp.%s = rpy.%s();" % (b, a)) // ]]] vp.x = position.x(); vp.y = position.y(); vp.z = position.z(); vp.roll = rpy.x(); vp.pitch = rpy.y(); vp.yaw = rpy.z(); // [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3) UAS_FCU(m_uas)->send_message_ignore_drop(vp); }
void send(bool a, float b, float c, float d, float e){ mavlink_message_t msg; mavlink_msg_ardrone_position_pack_chan(UAS_PACK_CHAN(uas),&msg,a,b,c,d,e); //pack UAS_FCU(uas)->send_message(&msg); //send ROS_INFO("Send!\nrobot_x:%f\nrobot_y:%f\ncatch_x:%f\ncatch_y:%f\n", b, c, d, e); }