00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <stdio.h>
00032 #include <errno.h>
00033 #include <string.h>
00034 #include <sys/socket.h>
00035 #include <sys/types.h>
00036 #include <netinet/in.h>
00037 #include <unistd.h>
00038 #include <stdlib.h>
00039 #include <fcntl.h>
00040 #include <time.h>
00041 #if (defined __QNX__) | (defined __QNXNTO__)
00042
00043 #include <unix.h>
00044 #else
00045
00046 #include <sys/time.h>
00047 #include <time.h>
00048 #include <arpa/inet.h>
00049 #endif
00050
00051
00052
00053 #include <mavlink.h>
00054
00055
00056 #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
00057
00058 uint64_t microsSinceEpoch();
00059
00060 int main(int argc, char* argv[])
00061 {
00062
00063 char help[] = "--help";
00064
00065
00066 char target_ip[100];
00067
00068 float position[6] = {};
00069 int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00070 struct sockaddr_in gcAddr;
00071 struct sockaddr_in locAddr;
00072
00073 uint8_t buf[BUFFER_LENGTH];
00074 ssize_t recsize;
00075 socklen_t fromlen;
00076 int bytes_sent;
00077 mavlink_message_t msg;
00078 uint16_t len;
00079 int i = 0;
00080
00081 unsigned int temp = 0;
00082
00083
00084 if ((argc == 2) && (strcmp(argv[1], help) == 0))
00085 {
00086 printf("\n");
00087 printf("\tUsage:\n\n");
00088 printf("\t");
00089 printf("%s", argv[0]);
00090 printf(" <ip address of QGroundControl>\n");
00091 printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
00092 exit(EXIT_FAILURE);
00093 }
00094
00095
00096
00097 strcpy(target_ip, "127.0.0.1");
00098 if (argc == 2)
00099 {
00100 strcpy(target_ip, argv[1]);
00101 }
00102
00103
00104 memset(&locAddr, 0, sizeof(locAddr));
00105 locAddr.sin_family = AF_INET;
00106 locAddr.sin_addr.s_addr = INADDR_ANY;
00107 locAddr.sin_port = htons(14551);
00108
00109
00110 if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
00111 {
00112 perror("error bind failed");
00113 close(sock);
00114 exit(EXIT_FAILURE);
00115 }
00116
00117
00118 if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
00119 {
00120 fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
00121 close(sock);
00122 exit(EXIT_FAILURE);
00123 }
00124
00125
00126 memset(&gcAddr, 0, sizeof(gcAddr));
00127 gcAddr.sin_family = AF_INET;
00128 gcAddr.sin_addr.s_addr = inet_addr(target_ip);
00129 gcAddr.sin_port = htons(14550);
00130
00131
00132
00133 for (;;)
00134 {
00135
00136
00137 mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
00138 len = mavlink_msg_to_send_buffer(buf, &msg);
00139 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00140
00141
00142 mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
00143 len = mavlink_msg_to_send_buffer(buf, &msg);
00144 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
00145
00146
00147 mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(),
00148 position[0], position[1], position[2],
00149 position[3], position[4], position[5]);
00150 len = mavlink_msg_to_send_buffer(buf, &msg);
00151 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00152
00153
00154 mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
00155 len = mavlink_msg_to_send_buffer(buf, &msg);
00156 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00157
00158
00159 memset(buf, 0, BUFFER_LENGTH);
00160 recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
00161 if (recsize > 0)
00162 {
00163
00164 mavlink_message_t msg;
00165 mavlink_status_t status;
00166
00167 printf("Bytes Received: %d\nDatagram: ", (int)recsize);
00168 for (i = 0; i < recsize; ++i)
00169 {
00170 temp = buf[i];
00171 printf("%02x ", (unsigned char)temp);
00172 if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
00173 {
00174
00175 printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
00176 }
00177 }
00178 printf("\n");
00179 }
00180 memset(buf, 0, BUFFER_LENGTH);
00181 sleep(1);
00182 }
00183 }
00184
00185
00186
00187 #if (defined __QNX__) | (defined __QNXNTO__)
00188 uint64_t microsSinceEpoch()
00189 {
00190
00191 struct timespec time;
00192
00193 uint64_t micros = 0;
00194
00195 clock_gettime(CLOCK_REALTIME, &time);
00196 micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;
00197
00198 return micros;
00199 }
00200 #else
00201 uint64_t microsSinceEpoch()
00202 {
00203
00204 struct timeval tv;
00205
00206 uint64_t micros = 0;
00207
00208 gettimeofday(&tv, NULL);
00209 micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
00210
00211 return micros;
00212 }
00213 #endif