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