void poseStampedCallback(const geometry_msgs::PoseStamped& poseStampedMsg) { // set timestamp (get NSec from ROS and convert to us) uint64_t timestamp = poseStampedMsg.header.stamp.toNSec() / 1000; // send MAVLINK attitude and local position messages mavlink_message_t msg; //convert quaternion to euler angles const btQuaternion quat(poseStampedMsg.pose.orientation.x, poseStampedMsg.pose.orientation.y, poseStampedMsg.pose.orientation.z, poseStampedMsg.pose.orientation.w); const btMatrix3x3 mat(quat); double roll, pitch, yaw; mat.getEulerYPR(yaw, pitch, roll); mavlink_msg_attitude_pack(sysid, compid, &msg, timestamp, roll, pitch, yaw, 0.0f, 0.0f, 0.0f); mavlink_message_t_publish(lcm, "MAVLINK", &msg); float x = poseStampedMsg.pose.position.x; float y = poseStampedMsg.pose.position.y; float z = poseStampedMsg.pose.position.z; mavlink_msg_local_position_pack(sysid, compid, &msg, timestamp, x, y, z, 0.0f, 0.0f, 0.0f); mavlink_message_t_publish(lcm, "MAVLINK", &msg); if (verbose) { ROS_INFO("Sent Mavlink local position and attitude messages."); } }
static void postHeartbeat( SOCKET sock, const sockaddr_in& gcAddr, float position[] ) { mavlink_message_t msg; uint16_t len; int bytes_sent; uint8_t buf[BUFFER_LENGTH]; //#define OLD_MAV /*Send Heartbeat */ #ifdef OLD_MAV mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); #else mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(gcAddr)); /* Send Status */ #ifdef OLD_MAV mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 7500, 0, 0, 0); #else mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof (gcAddr)); /* Send Local Position */ #ifdef OLD_MAV mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]); #else mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send attitude */ //old and new, do difference. static float roll = 1.2; float pitch = 1.7; float yaw = 3.14; #ifdef OLD_MAV mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, 0.01, 0.02, 0.03); #else mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, 0.01, 0.02, 0.03); #endif len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, (char *)buf, len, 0, (sockaddr*)&gcAddr, sizeof(gcAddr)); //Beep( 880, 100 ); }
int main(int argc, char* argv[]) { // Initialize MAVLink mavlink_wpm_init(&wpm); mavlink_system.sysid = 1; mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; // Create socket sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); // Check if --help flag was used if ((argc == 2) && (strcmp(argv[1], help) == 0)) { printf("\n"); printf("\tUsage:\n\n"); printf("\t"); printf("%s", argv[0]); printf(" <ip address of QGroundControl>\n"); printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); exit(EXIT_FAILURE); } // Change the target ip if parameter was given strcpy(target_ip, "127.0.0.1"); if (argc == 2) { strcpy(target_ip, argv[1]); } memset(&locAddr, 0, sizeof(locAddr)); locAddr.sin_family = AF_INET; locAddr.sin_addr.s_addr = INADDR_ANY; locAddr.sin_port = htons(14551); /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) { perror("error bind failed"); close(sock); exit(EXIT_FAILURE); } /* Attempt to make it non blocking */ if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) { fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); close(sock); exit(EXIT_FAILURE); } memset(&gcAddr, 0, sizeof(gcAddr)); gcAddr.sin_family = AF_INET; gcAddr.sin_addr.s_addr = inet_addr(target_ip); gcAddr.sin_port = htons(14550); printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); for (;;) { /*Send Heartbeat */ mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send Status */ mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); /* Send Local Position */ mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); /* Send attitude */ mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); memset(buf, 0, BUFFER_LENGTH); recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); if (recsize > 0) { // Something received - print out all bytes and parse packet mavlink_message_t msg; mavlink_status_t status; printf("Bytes Received: %d\nDatagram: ", (int)recsize); for (i = 0; i < recsize; ++i) { temp = buf[i]; printf("%02x ", (unsigned char)temp); if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) { // Packet received printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); // Handle packet with waypoint component mavlink_wpm_message_handler(&msg); // Handle packet with parameter component } } printf("\n"); } memset(buf, 0, BUFFER_LENGTH); usleep(50000); // Sleep one second } }