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
00032
00033
00034
00035 #include <stdio.h>
00036 #include <errno.h>
00037 #include <string.h>
00038 #include <sys/socket.h>
00039 #include <sys/types.h>
00040 #include <netinet/in.h>
00041 #include <unistd.h>
00042 #include <stdlib.h>
00043 #include <fcntl.h>
00044 #include <time.h>
00045 #include <math.h>
00046 #if (defined __QNX__) | (defined __QNXNTO__)
00047
00048 #include <unix.h>
00049 #else
00050
00051 #include <sys/time.h>
00052 #include <time.h>
00053 #include <arpa/inet.h>
00054 #endif
00055
00056
00057
00058
00059
00060
00061
00062 #include "mavlink_types.h"
00063
00064
00065 mavlink_system_t mavlink_system;
00066
00067
00068 #include "mavlink.h"
00069
00070
00071 void mavlink_wpm_send_message(mavlink_message_t* msg);
00072 void mavlink_wpm_send_gcs_string(const char* string);
00073 uint64_t mavlink_wpm_get_system_timestamp();
00074 void mavlink_missionlib_send_message(mavlink_message_t* msg);
00075 void mavlink_missionlib_send_gcs_string(const char* string);
00076 uint64_t mavlink_missionlib_get_system_timestamp();
00077
00078
00079 #include "waypoints.h"
00080 mavlink_wpm_storage wpm;
00081
00082
00083 #include "mavlink_missionlib_data.h"
00084 #include "mavlink_parameters.h"
00085
00086 mavlink_pm_storage pm;
00087
00092 void mavlink_pm_reset_params(mavlink_pm_storage* pm)
00093 {
00094 pm->size = MAVLINK_PM_MAX_PARAM_COUNT;
00095
00096 pm->param_values[MAVLINK_PM_PARAM_SYSTEM_ID] = 12;
00097 strcpy(pm->param_names[MAVLINK_PM_PARAM_SYSTEM_ID], "SYS_ID");
00098
00099 pm->param_values[MAVLINK_PM_PARAM_ATT_K_D] = 0.3f;
00100 strcpy(pm->param_names[MAVLINK_PM_PARAM_ATT_K_D], "ATT_K_D");
00101 }
00102
00103
00104 #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
00105
00106 char help[] = "--help";
00107
00108
00109 char target_ip[100];
00110
00111 float position[6] = {};
00112 int sock;
00113 struct sockaddr_in gcAddr;
00114 struct sockaddr_in locAddr;
00115 uint8_t buf[BUFFER_LENGTH];
00116 ssize_t recsize;
00117 socklen_t fromlen;
00118 int bytes_sent;
00119 mavlink_message_t msg;
00120 uint16_t len;
00121 int i = 0;
00122 unsigned int temp = 0;
00123
00124 uint64_t microsSinceEpoch();
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 void mavlink_missionlib_send_message(mavlink_message_t* msg)
00136 {
00137 uint8_t buf[MAVLINK_MAX_PACKET_LEN];
00138 uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
00139 uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
00140
00141 printf("SENT %d bytes\n", bytes_sent);
00142 }
00143
00144 void mavlink_missionlib_send_gcs_string(const char* string)
00145 {
00146 const int len = 50;
00147 mavlink_statustext_t status;
00148 int i = 0;
00149 while (i < len - 1)
00150 {
00151 status.text[i] = string[i];
00152 if (string[i] == '\0')
00153 break;
00154 i++;
00155 }
00156 status.text[i] = '\0';
00157 mavlink_message_t msg;
00158
00159 mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &status);
00160 mavlink_missionlib_send_message(&msg);
00161 }
00162
00163 uint64_t mavlink_missionlib_get_system_timestamp()
00164 {
00165 struct timeval tv;
00166 gettimeofday(&tv, NULL);
00167 return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
00168 }
00169
00170 float roll, pitch, yaw;
00171 uint32_t latitude, longitude, altitude;
00172 uint16_t speedx, speedy, speedz;
00173 float rollspeed, pitchspeed, yawspeed;
00174 bool hilEnabled = false;
00175
00176 int main(int argc, char* argv[])
00177 {
00178
00179 mavlink_wpm_init(&wpm);
00180 mavlink_system.sysid = 5;
00181 mavlink_system.compid = 20;
00182 mavlink_pm_reset_params(&pm);
00183
00184 int32_t ground_distance;
00185 uint32_t time_ms;
00186
00187
00188
00189
00190 sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
00191
00192
00193 if ((argc == 2) && (strcmp(argv[1], help) == 0))
00194 {
00195 printf("\n");
00196 printf("\tUsage:\n\n");
00197 printf("\t");
00198 printf("%s", argv[0]);
00199 printf(" <ip address of QGroundControl>\n");
00200 printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
00201 exit(EXIT_FAILURE);
00202 }
00203
00204
00205
00206 strcpy(target_ip, "127.0.0.1");
00207 if (argc == 2)
00208 {
00209 strcpy(target_ip, argv[1]);
00210 }
00211
00212
00213 memset(&locAddr, 0, sizeof(locAddr));
00214 locAddr.sin_family = AF_INET;
00215 locAddr.sin_addr.s_addr = INADDR_ANY;
00216 locAddr.sin_port = htons(14551);
00217
00218
00219 if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
00220 {
00221 perror("error bind failed");
00222 close(sock);
00223 exit(EXIT_FAILURE);
00224 }
00225
00226
00227 if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
00228 {
00229 fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
00230 close(sock);
00231 exit(EXIT_FAILURE);
00232 }
00233
00234
00235 memset(&gcAddr, 0, sizeof(gcAddr));
00236 gcAddr.sin_family = AF_INET;
00237 gcAddr.sin_addr.s_addr = inet_addr(target_ip);
00238 gcAddr.sin_port = htons(14550);
00239
00240
00241 printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
00242
00243
00244 for (;;)
00245 {
00246 bytes_sent = 0;
00247
00248
00249 mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
00250 len = mavlink_msg_to_send_buffer(buf, &msg);
00251 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00252
00253
00254 mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0);
00255 len = mavlink_msg_to_send_buffer(buf, &msg);
00256 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
00257
00258
00259 mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
00260 position[0], position[1], position[2],
00261 position[3], position[4], position[5]);
00262 len = mavlink_msg_to_send_buffer(buf, &msg);
00263 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00264
00265
00266 if (hilEnabled)
00267 {
00268 mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100);
00269 len = mavlink_msg_to_send_buffer(buf, &msg);
00270 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00271 }
00272
00273
00274 mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
00275 len = mavlink_msg_to_send_buffer(buf, &msg);
00276 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00277
00278
00279 float roll_ailerons = 0;
00280 float pitch_elevator = 0.2;
00281 float yaw_rudder = 0.1;
00282 float throttle = 0.9;
00283 mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0);
00284 len = mavlink_msg_to_send_buffer(buf, &msg);
00285 bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
00286
00287 memset(buf, 0, BUFFER_LENGTH);
00288 recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
00289 if (recsize > 0)
00290 {
00291
00292 mavlink_message_t msg;
00293 mavlink_status_t status;
00294
00295 printf("Bytes Received: %d\nDatagram: ", (int)recsize);
00296 for (i = 0; i < recsize; ++i)
00297 {
00298 temp = buf[i];
00299 printf("%02x ", (unsigned char)temp);
00300 if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
00301 {
00302
00303 printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
00304
00305
00306 mavlink_wpm_message_handler(&msg);
00307
00308
00309 mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
00310
00311
00312 if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE)
00313 {
00314 mavlink_hil_state_t hil;
00315 mavlink_msg_hil_state_decode(&msg, &hil);
00316 printf("Received HIL state:\n");
00317 printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw);
00318 roll = hil.roll;
00319 pitch = hil.pitch;
00320 yaw = hil.yaw;
00321 rollspeed = hil.rollspeed;
00322 pitchspeed = hil.pitchspeed;
00323 yawspeed = hil.yawspeed;
00324 speedx = hil.vx;
00325 speedy = hil.vy;
00326 speedz = hil.vz;
00327 latitude = hil.lat;
00328 longitude = hil.lon;
00329 altitude = hil.alt;
00330 hilEnabled = true;
00331 }
00332 }
00333 }
00334 printf("\n");
00335 }
00336 memset(buf, 0, BUFFER_LENGTH);
00337 usleep(10000);
00338
00339
00340
00341 mavlink_pm_queued_send();
00342 }
00343 }
00344
00345
00346
00347 #if (defined __QNX__) | (defined __QNXNTO__)
00348 uint64_t microsSinceEpoch()
00349 {
00350
00351 struct timespec time;
00352
00353 uint64_t micros = 0;
00354
00355 clock_gettime(CLOCK_REALTIME, &time);
00356 micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000;
00357
00358 return micros;
00359 }
00360 #else
00361 uint64_t microsSinceEpoch()
00362 {
00363
00364 struct timeval tv;
00365
00366 uint64_t micros = 0;
00367
00368 gettimeofday(&tv, NULL);
00369 micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
00370
00371 return micros;
00372 }
00373 #endif