source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp @ 79

Last change on this file since 79 was 78, checked in by Thomas Fuhrmann, 5 years ago

Add the mavlink udp com class

File size: 45.1 KB
Line 
1//  created:    20/04/2016
2//  updated:    20/09/2016
3//  filename:   MavlinkUDP.cpp
4//
5//  author:     Milan Erdelj, <milan.erdelj@hds.utc.fr>
6//              Copyright Heudiasyc UMR UTC/CNRS 7253
7//
8//              Current_messages and Time_Stamps:
9//              Copyright (c) 2014 MAVlink Development Team. All rights reserved.
10//              Trent Lukaczyk, <aerialhedgehog@gmail.com>
11//              Jaycee Lock,    <jaycee.lock@gmail.com>
12//              Lorenz Meier,   <lm@inf.ethz.ch>
13//
14//  version:    $Id: $
15//
16//  purpose:    MAVLink communication block class that used UDP sockets
17//
18//
19/*********************************************************************/
20
21#include "MavlinkUDP.h"
22#include <sstream>
23#include <iostream>
24
25using namespace std;
26
27MavlinkUDP::MavlinkUDP(const std::string& addr, int port) : m_port(port), m_addr(addr), me(addr), target(GCS_ADDR) {
28    // initialize attributes
29    write_count = 0;
30    home_position_set=false;
31    ack=false;
32    request=false;
33    seq=-1;
34    seqr=-1;
35    seqold=-1;
36    waypoint=false;
37    Waycount=0;
38    Wayseq=-1;
39    message_interval = 1000; // ms
40    state=-1;
41    control_status = 0;      // whether the autopilot is in offboard control mode
42    arming_status = 0;      // whether the autopilot is in arming control mode
43    debug_messages = true;
44
45    resetCommandAck();
46    resetMissionAck();
47
48    // period function flags
49    sendingAttitude = false;
50    sendingPosition = false;
51    sendingSystemStatus = false;
52    sendingAttitudeInterval = -1;
53    sendingPositionInterval = -1;
54    sendingSystemStatusInterval = -1;
55
56    // mission items
57    missionFirst = 0;
58    missionLast = 0;
59    mission_items_loop = false;
60    missionActive = false; // if the mission is launched
61
62    shutdown_flag = false;
63
64    // TODO read the system ID from the file based on the IP address
65    system_id    = 0; // system id
66    autopilot_id = 0; // autopilot component id
67    component_id = 0; // companion computer component id
68
69   // mavlink_command_long_t com = { 0 };
70    compt=0;
71    current_messages.sysid  = system_id;
72    current_messages.compid = autopilot_id;
73    Xtimes=0;
74    Xtimesync=false;
75
76    something_to_send = false; // check if something needs to be sent
77    stop_recv = false;
78    stop_send = false;
79    startThreads(); // start both sending and receiving thread
80}
81
82MavlinkUDP::~MavlinkUDP() {
83    try {
84        stopThreads();
85    } catch (int error) {
86        fprintf(stderr,"MavlinkUDP: Warning! Could not stop threads!\n");
87    }
88}
89
90// starting send and receive threads
91void MavlinkUDP::startThreads() {
92    // start receive thread
93    recv_th = std::thread(&MavlinkUDP::recv_thread, this);
94    printf("MavlinkUDP: Receiver thread created.\n");
95    // start sending thread
96    send_th = std::thread(&MavlinkUDP::send_thread, this);
97    printf("MavlinkUDP: Sender thread created.\n");
98}
99
100void MavlinkUDP::stopThreads() {
101    stop_recv = true;
102    stop_send = true;
103    usleep(100);
104    if(recv_th.joinable()) recv_th.join();
105    if(send_th.joinable()) send_th.join();
106    printf("MavlinkUDP: Threads stopped.\n");
107    // close the sockets
108    close(m_socket_in);
109    close(m_socket_out);
110    printf("MavlinkUDP: Sockets closed.\n");
111}
112
113// quit handler
114void MavlinkUDP::handleQuit(int sig) {
115    try {
116        stopThreads();
117    } catch (int error) {
118        fprintf(stderr,"MavlinkUDP: Warning, could not stop communication block!\n");
119    }
120}
121
122inline void MavlinkUDP::clearBuffer(uint8_t *buffer, int len) {
123    memset(buffer, 0, len);
124}
125
126void MavlinkUDP::clearData() {
127    clearBuffer(buff_in, BUFF_IN_LEN);
128}
129
130void MavlinkUDP::recv_thread() {
131    // UDP socket setup
132    socklen_t clilen=sizeof(client_in);
133    if((m_socket_in=socket(AF_INET,SOCK_DGRAM,0))<0)
134    {
135        throw SocketRuntimeError("[ERRR] RECV socket error!\n");
136    }
137    memset((char *)&myaddr_in, 0, sizeof(myaddr_in));
138    myaddr_in.sin_family=AF_INET;
139    // receiving messages from all addresses
140    myaddr_in.sin_addr.s_addr=htonl(INADDR_ANY);
141    myaddr_in.sin_port=htons(m_port);
142    if(bind(m_socket_in,(struct sockaddr *)&myaddr_in,sizeof(myaddr_in))<0)
143    {
144        throw SocketRuntimeError("[ERRR] UDP in bind error!\n");
145    }
146    //std::cout << "MavlinkUDP: RECV socket bind OK.\n";
147    std::cout << "MavlinkUDP: RECV thread launched.\n";
148    clearBuffer(buff_in, BUFF_IN_LEN);
149    // receive data
150    while(!stop_recv) {
151        recsize = recvfrom(m_socket_in, (void *)buff_in, BUFF_IN_LEN, 0, (struct sockaddr *)&client_in, &clilen);
152        if(recsize > 0) {
153            // parse the received data
154            //print_message(sockUDP.buff_in, sockUDP.recsize);
155            decode_message(buff_in, recsize);
156            clearBuffer(buff_in, BUFF_IN_LEN);
157        }
158        //usleep(10000);
159        std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_IN));
160    }
161}
162
163void MavlinkUDP::send_thread() {
164    // socket setup
165    int broadcast = 1;
166    // socket for broadcast
167    if((m_socket_out=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0) {
168        throw SocketRuntimeError("[ERRR] udp out socket error\n");
169    }
170    if((setsockopt(m_socket_out,SOL_SOCKET,SO_BROADCAST,&broadcast,sizeof broadcast)) == -1) {
171        throw SocketRuntimeError("[ERRR] udp out setsockopt error\n");
172    }
173    cout << "MavlinkUDP: SEND thread launched.\n";
174    memset((char *) &myaddr_out, 0, sizeof(myaddr_out));
175    myaddr_out.sin_family=AF_INET;
176    myaddr_out.sin_port=htons(m_port);
177    myaddr_out.sin_addr.s_addr=inet_addr(m_addr.c_str());
178    clearBuffer(buff_out, BUFF_OUT_LEN);
179    // send data to the socket
180    while(!stop_send) {
181        if(something_to_send) {
182            // send quickly and reset the flag
183            sendto(m_socket_out, buff_out, length_to_send, 0, (struct sockaddr *) &myaddr_out, sizeof(myaddr_out));
184            clearBuffer(buff_out,BUFF_OUT_LEN);
185            something_to_send = false;
186        }
187        //clearBuffer(buff_out, BUFF_OUT_LEN);
188        std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_OUT));
189        //usleep(10000);
190    }
191}
192
193// read mission configuration file
194// TODO use the Qt XML library
195//
196//
197
198void MavlinkUDP::updateSetpoint(mavlink_set_position_target_local_ned_t setpoint) {
199    current_setpoint = setpoint;
200}
201
202//
203// MESSAGES
204//
205
206// send Heartbeat message
207void MavlinkUDP::sendHeartbeat() {
208    mavlink_message_t message;
209    mavlink_msg_heartbeat_pack(me.getSysID(), me.getCompID(), &message, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_AUTO_ARMED, 0, MAV_STATE_ACTIVE);
210    length_to_send = mavlink_msg_to_send_buffer(buff_out, &message);
211    something_to_send = true;
212    printf("[SENT] Heartbeat\n");
213}
214
215// send SystemStatus message
216void MavlinkUDP::sendSystemStatus(uint32_t onboardSensorsPresent, uint32_t onboardSensorsEnabled, uint32_t onboardSensorsHealth, uint16_t load, uint16_t voltage, int16_t current, int8_t batteryRemaining, uint16_t dropRateComm, uint16_t errorsComm, uint16_t errors1, uint16_t errors2, uint16_t errors3, uint16_t errors4) {
217    mavlink_message_t message;
218    mavlink_msg_sys_status_pack(me.getSysID(), me.getCompID(), &message, onboardSensorsPresent, onboardSensorsEnabled, onboardSensorsHealth, load, voltage, current, batteryRemaining, dropRateComm, errorsComm, errors1, errors2, errors3, errors4);
219    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
220    something_to_send = true;
221    printf("[SENT] System status\n");
222}
223
224// send BatteryStatus message
225void MavlinkUDP::sendBatteryStatus(uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, uint16_t *voltages, int16_t current, int32_t currentConsumed, int32_t energyConsumed, int8_t batteryRemaining) {
226    mavlink_message_t message;
227    mavlink_msg_battery_status_pack(me.getSysID(), me.getCompID(), &message, id, battery_function, type, temperature, voltages, current, currentConsumed, energyConsumed, batteryRemaining);
228    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
229    something_to_send = true;
230}
231
232// send SystemTime message
233void MavlinkUDP::sendSystemTime() {
234    mavlink_system_time_t sys_time;
235    sys_time.time_unix_usec = get_time_usec();
236    mavlink_message_t message;
237    mavlink_msg_system_time_pack(me.getSysID(), me.getCompID(), &message, sys_time.time_unix_usec, sys_time.time_boot_ms);
238    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
239    something_to_send = true;
240    printf("[SENT] System time\n");
241}
242
243// send LocalPostiionNED message
244void MavlinkUDP::sendLocalPositionNED(float x, float y, float z, float vx, float vy, float vz) {
245    mavlink_message_t message;
246    mavlink_msg_local_position_ned_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), x, y, z, vx, vy, vz);
247    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
248    something_to_send = true;
249}
250
251// send Attitude message
252void MavlinkUDP::sendAttitude(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) {
253    mavlink_message_t message;
254    mavlink_msg_attitude_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
255    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
256    something_to_send = true;
257}
258
259// send MissionAck message
260void MavlinkUDP::sendMissionAck(uint8_t targetSystem, uint8_t targetComponent, uint8_t type) {
261    mavlink_message_t message;
262    mavlink_msg_mission_ack_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, type);
263    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
264    something_to_send = true;
265    printf("[SENT] Mission ACK\n");
266}
267
268// send commandAck message
269void MavlinkUDP::sendCommandAck(uint16_t command, uint8_t result) {
270    mavlink_message_t message;
271    mavlink_msg_command_ack_pack(me.getSysID(), me.getCompID(), &message, command, result);
272    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
273    something_to_send = true;
274    printf("[SENT] Command ACK: %d\n", command);
275}
276
277// send autopilot version
278void MavlinkUDP::sendAutopilotVersion(uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, uint8_t *flight_custom_version, uint8_t *middleware_custom_version, uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) {
279    mavlink_message_t message;
280    // TODO get these values from autopilot
281    mavlink_msg_autopilot_version_pack(me.getSysID(), me.getCompID(), &message, capabilities, flight_sw_version, middleware_sw_version, os_sw_version, board_version, flight_custom_version, middleware_custom_version, os_custom_version, vendor_id, product_id, uid);
282    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
283    something_to_send = true;
284    printf("[SENT] Autopilot vesrion\n");
285}
286
287// send MissionCount message
288void MavlinkUDP::sendMissionCount(uint8_t targetSystem, uint8_t targetComponent, uint16_t count) {
289    mavlink_message_t message;
290    mavlink_msg_mission_count_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, count);
291    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
292    something_to_send = true;
293}
294
295// send CommandLong message
296void MavlinkUDP::sendCommandLong(uint8_t targetSystem, uint8_t targetComponent, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) {
297    mavlink_message_t message;
298    //    uint8_t confirmation;
299    //    float param1, param2, param3, param4, param5, param6, param7;
300    mavlink_msg_command_long_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, command, confirmation, param1, param2, param3, param4, param5, param6, param7);
301    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
302    something_to_send = true;
303    printf("[SENT] Command long: %d\n", command);
304}
305
306// send MissionWritePartialList message
307void MavlinkUDP::sendMissionWritePartialList(uint8_t targetSystem, uint8_t targetComponent, uint16_t startIndex, uint16_t endIndex) {
308    mavlink_message_t message;
309    mavlink_msg_mission_write_partial_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, startIndex, endIndex);
310    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
311    something_to_send = true;
312}
313
314// send MissionItem message
315void MavlinkUDP::sendMissionItem(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) {
316    mavlink_message_t message;
317    mavlink_msg_mission_item_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
318    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
319    something_to_send = true;
320}
321
322// send MissionRequestList message
323void MavlinkUDP::sendMissionRequestList(uint8_t targetSystem, uint8_t targetComponent) {
324    mavlink_message_t message;
325    mavlink_msg_mission_request_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
326    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
327    something_to_send = true;
328}
329
330// send MissionItemReached message
331void MavlinkUDP::sendMissionItemReached(uint16_t seq) {
332    mavlink_message_t message;
333    mavlink_msg_mission_item_reached_pack(me.getSysID(), me.getCompID(), &message, seq);
334    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
335    something_to_send = true;
336}
337
338// send MissionSetCurrent message
339void MavlinkUDP::sendMissionSetCurrent(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq) {
340    mavlink_message_t message;
341    mavlink_msg_mission_set_current_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq);
342    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
343    something_to_send = true;
344}
345
346// send MissionClearAll message
347void MavlinkUDP::sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent) {
348    mavlink_message_t message;
349    mavlink_msg_mission_clear_all_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
350    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
351    something_to_send = true;
352    printf("[SENT] Mission clear all\n");
353}
354
355// send SetPositionTargetLocalNED message
356void MavlinkUDP::sendSetPositionTargetLocalNED(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) {
357    mavlink_message_t message;
358    mavlink_msg_set_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
359    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
360    something_to_send = true;
361}
362
363// send SetPositionTargetGlobalInt message
364void MavlinkUDP::sendSetPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) {
365    mavlink_message_t message;
366    mavlink_msg_set_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
367    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
368    something_to_send = true;
369}
370
371//
372// COMMANDS
373//
374
375// command Waypoint
376void MavlinkUDP::cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {
377    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_WAYPOINT, 0, holdTime, proximityRadius, passRadius, desiredYaw, latitude, longitude, altitude);
378}
379
380// command SetMessageInterval
381void MavlinkUDP::cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec) {
382    sendCommandLong(targetSystem, targetComponent, MAV_CMD_SET_MESSAGE_INTERVAL, 0, (float)messageID, (float)interval_usec, 0, 0, 0, 0, 0);
383}
384
385// command Land
386void MavlinkUDP::cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
387    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_LAND, 0, abortAlt, 0, 0, desiredYaw, latitude, longitude, altitude);
388}
389
390// command LandStart
391void MavlinkUDP::cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude) {
392    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_LAND_START, 0, 0, 0, 0, 0, latitude, longitude, 0);
393}
394
395// command TakeOff
396void MavlinkUDP::cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
397    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_TAKEOFF, 0, desiredPitch, 0, 0, magnetometerYaw, latitude, longitude, altitude);
398}
399
400// command DoSetMode
401void MavlinkUDP::cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode) {
402    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_MODE, 0, mavMode, 0, 0, 0, 0, 0, 0);
403}
404
405// command DoSetHome
406void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) {
407    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_HOME, 0, (float)useCurrent, 0, 0, 0, 0, 0, 0);
408}
409
410// command MissionStart
411void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) {
412    sendCommandLong(targetSystem, targetComponent, MAV_CMD_MISSION_START, 0, (float)firstItem, (float)lastItem, 0, 0, 0, 0, 0);
413}
414
415// command DoSetParameter
416void MavlinkUDP::cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue) {
417    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_PARAMETER, 0, (float)paramNumber, paramValue, 0, 0, 0, 0, 0);
418}
419
420// command RequestAutopilotCapabilities
421void MavlinkUDP::cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent) {
422    sendCommandLong(targetSystem, targetComponent, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, 0, 1, 0, 0, 0, 0, 0, 0);
423}
424
425// command ReturnToLaunch
426void MavlinkUDP::cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
427    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0);
428}
429
430// command Shutdown
431void MavlinkUDP::cmdRebootShutdown(uint8_t targetSystem, uint8_t targetComponent, uint8_t autopilot, uint8_t onboardComputer) {
432    // for autopilot and onboardComputer, see TMAV_REBOOT_SHUTDOWN_PARAM
433    sendCommandLong(targetSystem, targetComponent, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, autopilot, onboardComputer, 0, 0, 0, 0, 0);
434}
435
436// check the received message CRC, return true if OK
437bool MavlinkUDP::check_mavlink_crc(u_int8_t *buff_in, ssize_t recsize, u_int8_t msgid) {
438    u_int16_t *crc_accum = new u_int16_t(X25_INIT_CRC);
439    u_int16_t recv_crc;
440    for (int i = 1; i < (recsize-2); ++i)
441        crc_accumulate(buff_in[i],crc_accum);
442    crc_accumulate(hds_mavlink_message_crcs[msgid],crc_accum);
443    recv_crc = buff_in[recsize-1]<<8 ^ buff_in[recsize-2];
444    //cout << "CRC(recv): " << hex << setw(4) << recv_crc << endl;
445    //cout << "CRC(calc): " << hex << setw(4) << *crc_accum << endl;
446    // if the CRCs are the same, the subtraction result should be 0:
447    recv_crc -= *crc_accum;
448    delete crc_accum;
449    if(!recv_crc) return true;
450    return false;
451}
452
453// parse the received message
454void MavlinkUDP::print_message(u_int8_t *buff_in, ssize_t recsize) {
455    uint8_t temp;
456    mavlink_message_t msg;
457    mavlink_status_t status;
458    printf("\n[PRINT MESSAGE]\n");
459    printf("Timestamp: %ld\n", get_time_usec());
460    printf("Bytes received: %ld\n", recsize);
461    printf("Datagram: ");
462    for(int i=0; i<recsize; ++i) {
463        temp = buff_in[i];
464        //cout << base(10) << temp << " ";
465        printf("%02x ", temp);
466        if(mavlink_parse_char(MAVLINK_COMM_0, temp, &msg, &status)) {
467            printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
468            //printf("Iteration: %d\n", i);
469        }
470    }
471    printf("\n");
472
473}
474
475void MavlinkUDP::decode_message(u_int8_t *buff_in, ssize_t msg_size) {
476    mavlink_message_t message;
477    mavlink_status_t status;
478    Time_Stamps this_timestamps;
479    // parse message
480    for(int i=0; i<msg_size; ++i) mavlink_parse_char(MAVLINK_COMM_0, buff_in[i], &message, &status);
481    // handle message ID
482    current_messages.sysid  = message.sysid;
483    current_messages.compid = message.compid;
484//    printf("[DECODE]\n");
485//    printf("System ID: %d\n", message.sysid);
486//    printf("Component ID: %d\n", message.compid);
487//    printf("Message ID: %d\n", message.msgid);
488//    printf("Length: %d\n", message.len);
489
490    if(check_mavlink_crc(buff_in, msg_size, message.msgid))
491    {
492        switch (message.msgid)
493        {
494        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
495        {
496            printf("[RECV] MAVLINK_MSG_ID_MISSION_CLEAR_ALL\n");
497            mavlink_msg_mission_clear_all_decode(&message,&(current_messages.clear_all));
498            current_messages.time_stamps.clear_all = get_time_usec();
499            this_timestamps.clear_all = current_messages.time_stamps.clear_all;
500            // clear the queue
501            clearMissionPlan();
502            //
503            // TODO signal the app that the vector is cleared
504            //
505            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
506            break;
507        }
508        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
509        {
510            printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM_REACHED\n");
511            mavlink_msg_mission_item_reached_decode(&message, &(current_messages.mission_reached));
512            current_messages.time_stamps.mission_reached = get_time_usec();
513            this_timestamps.mission_reached = current_messages.time_stamps.mission_reached;
514            seqr=current_messages.mission_reached.seq;
515            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
516            break;
517        }
518        case MAVLINK_MSG_ID_MISSION_COUNT:
519        {
520            printf("[RECV] MAVLINK_MSG_ID_MISSION_COUNT\n");
521            mavlink_msg_mission_count_decode(&message, &(current_messages.mission_count));
522            current_messages.time_stamps.mission_count = get_time_usec();
523            this_timestamps.mission_count = current_messages.time_stamps.mission_count;
524            Waycount=current_messages.mission_count.count;
525            setMissionCount(current_messages.mission_count.count);
526            compt=0;
527            Wayseq=-1;
528            break;
529        }
530        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
531        {
532            printf("[RECV] MAVLINK_MSG_ID_MISSION_REQUEST_LIST\n");
533            mavlink_msg_mission_request_list_decode(&message, &(current_messages.mission_request_list));
534            current_messages.time_stamps.mission_request_list = get_time_usec();
535            this_timestamps.mission_request_list = current_messages.time_stamps.mission_request_list;
536            // send the mission count
537            sendMissionCount(target.getSysID(), target.getCompID(), getMissionCount());
538            break;
539        }
540        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
541        {
542            printf("[RECV] MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST\n");
543            mavlink_msg_mission_write_partial_list_decode(&message, &(current_messages).mission_write_partial_list);
544            current_messages.time_stamps.mission_write_partial_list = get_time_usec();
545            this_timestamps.mission_write_partial_list = current_messages.time_stamps.mission_write_partial_list;
546            //
547            // TODO process the partial list message
548            //
549            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
550            break;
551        }
552        case MAVLINK_MSG_ID_TIMESYNC:
553        {
554            //  printf("MAVLINK_MSG_ID_TIMESYNC\n");
555            mavlink_msg_timesync_decode(&message, &(current_messages.timesync));
556            current_messages.time_stamps.timesync = get_time_usec();
557            this_timestamps.timesync = current_messages.time_stamps.timesync;
558            Xtimec=current_messages.timesync.tc1;
559            Xtimes=current_messages.timesync.ts1;
560            Xtimesync=true;
561            break;
562        }
563        case MAVLINK_MSG_ID_SYSTEM_TIME:
564        {
565            printf("[RECV] MAVLINK_MSG_ID_SYSTEM_TIME\n");
566            mavlink_msg_system_time_decode(&message, &(current_messages.system_time));
567            current_messages.time_stamps.system_time = get_time_usec();
568            this_timestamps.system_time = current_messages.time_stamps.system_time;
569            break;
570        }
571        case MAVLINK_MSG_ID_MISSION_ITEM:
572        {
573            printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM\n");
574            mavlink_msg_mission_item_decode(&message, &(current_messages.mission_item));
575            current_messages.time_stamps.mission_item = get_time_usec();
576            this_timestamps.mission_item = current_messages.time_stamps.mission_item;
577
578            // * Mission items queue:
579            // * 16   MAV_CMD_NAV_WAYPOINT            (hold time, acceptance radius, yaw angle, lat, long, altitude)
580            // * 21   MAV_CMD_NAV_LAND                (abort alt, yaw angle, lat, long, altitude)
581            // * 22   MAV_CMD_NAV_TAKEOFF             (yaw angle, lat, long, altitude)
582            // * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
583            // * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
584
585            //waypoint=true;
586            //setMissionCount(getMissionCount()+1);
587            //Wayseq=current_messages.mission_item.seq;
588            //frame=current_messages.mission_item.frame;
589            command=current_messages.mission_item.command;
590
591            switch(command) {
592            case MAV_CMD_NAV_TAKEOFF:
593            {
594                printf("[RECV] MISSION ITEM: TAKEOFF\n");
595                MissionItem item;
596                item.itemID = command;
597                item.sequence = mavlink_msg_mission_item_get_seq(&message);
598                item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
599                item.latitude = mavlink_msg_mission_item_get_x(&message);
600                item.longitude = mavlink_msg_mission_item_get_y(&message);
601                item.altitude = mavlink_msg_mission_item_get_z(&message);
602                missionPlan.push(item);
603                break;
604            }
605            case MAV_CMD_NAV_LAND:
606            {
607                printf("[RECV] MISSION ITEM: LAND\n");
608                MissionItem item;
609                item.itemID = command;
610                item.sequence = mavlink_msg_mission_item_get_seq(&message);
611                item.abortAltitude = mavlink_msg_mission_item_get_param1(&message);
612                item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
613                item.latitude = mavlink_msg_mission_item_get_x(&message);
614                item.longitude = mavlink_msg_mission_item_get_y(&message);
615                item.altitude = mavlink_msg_mission_item_get_z(&message);
616                missionPlan.push(item);
617                break;
618            }
619            case MAV_CMD_NAV_WAYPOINT:
620            {
621                MissionItem item;
622                item.itemID = command;
623                item.sequence = mavlink_msg_mission_item_get_seq(&message);
624                //item.type = (uint8_t)mavlink_msg_mission_item_get_param3(&message);
625                item.holdTime = mavlink_msg_mission_item_get_param1(&message);
626                item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
627                item.latitude = mavlink_msg_mission_item_get_x(&message);
628                item.longitude = mavlink_msg_mission_item_get_y(&message);
629                item.altitude = mavlink_msg_mission_item_get_z(&message);
630                missionPlan.push(item);
631                //printf("[RECV] WAYPOINT: seq = %d, x = %.2f, y = %.2f, type = %d\n", mavlink_msg_mission_item_get_seq(&message), recvWaypoint.m_x, recvWaypoint.m_y, (int)recvWaypoint.m_type);
632                printf("[RECV] WAYPOINT: seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", item.sequence, item.latitude, item.longitude, item.altitude);
633                break;
634            }
635            }
636            // respond with ACK immediately
637            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
638            break;
639        }
640        case MAVLINK_MSG_ID_MISSION_ACK:
641        {
642            printf("[RECV] MAVLINK_MSG_ID_MISSION_ACK\n");
643            mavlink_msg_mission_ack_decode(&message, &(current_messages.mission_ack));
644            // check if it is the right mission ack
645            current_messages.time_stamps.mission_ack = get_time_usec();
646            this_timestamps.mission_ack = current_messages.time_stamps.mission_ack;
647            setMissionAck();
648            //typeack=current_messages.mission_ack.type;
649            break;
650        }
651        case MAVLINK_MSG_ID_COMMAND_ACK:
652        {
653            printf("[RECV] MAVLINK_MSG_ID_COMMAND_ACK\n");
654            mavlink_msg_command_ack_decode(&message, &(current_messages.command_ack));
655            // check if it is the right command ack
656            current_messages.time_stamps.command_ack = get_time_usec();
657            this_timestamps.command_ack = current_messages.time_stamps.command_ack;
658            setCommandAck();
659            break;
660        }
661        case MAVLINK_MSG_ID_MISSION_REQUEST:
662        {
663            //    printf("MAVLINK_MSG_ID_MISSION_REQUEST\n");
664            mavlink_msg_mission_request_decode(&message, &(current_messages.mission_request));
665            seq=current_messages.mission_request.seq;
666            current_messages.time_stamps.mission_request = get_time_usec();
667            this_timestamps.mission_request = current_messages.time_stamps.mission_request;
668            request=true;
669            break;
670        }
671        case MAVLINK_MSG_ID_HOME_POSITION:
672        {
673            printf("[RECV] MAVLINK_MSG_ID_HOME_POSITION\n");
674            mavlink_msg_home_position_decode(&message, &(current_messages.home_position));
675            current_messages.time_stamps.home_position = get_time_usec();
676            this_timestamps.home_position = current_messages.time_stamps.home_position;
677            home_position_set=true;
678            break;
679        }
680        case MAVLINK_MSG_ID_HEARTBEAT:
681        {
682            printf("[RECV] MAVLINK_MSG_ID_HEARTBEAT\n");
683            mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
684            current_messages.time_stamps.heartbeat = get_time_usec();
685            this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
686            state=current_messages.heartbeat.system_status;
687            break;
688        }
689
690        case MAVLINK_MSG_ID_SYS_STATUS:
691        {
692            printf("[RECV] MAVLINK_MSG_ID_SYS_STATUS\n");
693            mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
694            current_messages.time_stamps.sys_status = get_time_usec();
695            this_timestamps.sys_status = current_messages.time_stamps.sys_status;
696            //USB_port->write_message(message);
697            break;
698        }
699
700        case MAVLINK_MSG_ID_BATTERY_STATUS:
701        {
702            //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
703            mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
704            current_messages.time_stamps.battery_status = get_time_usec();
705            this_timestamps.battery_status = current_messages.time_stamps.battery_status;
706            //USB_port->write_message(message);
707            break;
708        }
709
710        case MAVLINK_MSG_ID_RADIO_STATUS:
711        {
712            //printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
713            mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
714            current_messages.time_stamps.radio_status = get_time_usec();
715            this_timestamps.radio_status = current_messages.time_stamps.radio_status;
716            break;
717        }
718
719        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
720        {
721            printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
722            mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
723            current_messages.time_stamps.local_position_ned = get_time_usec();
724            this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
725            break;
726        }
727
728        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
729        {
730            printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
731            mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
732            current_messages.time_stamps.global_position_int = get_time_usec();
733            this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
734            //USB_port->write_message(message);
735            break;
736        }
737
738        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
739        {
740            printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
741            mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
742            current_messages.time_stamps.position_target_local_ned = get_time_usec();
743            this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
744            break;
745        }
746
747        case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
748        {
749            printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
750            mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
751            current_messages.time_stamps.position_target_global_int = get_time_usec();
752            this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
753            break;
754        }
755
756        case MAVLINK_MSG_ID_HIGHRES_IMU:
757        {
758            //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
759            mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
760            current_messages.time_stamps.highres_imu = get_time_usec();
761            this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
762            break;
763        }
764
765        case MAVLINK_MSG_ID_ATTITUDE:
766        {
767            //printf("MAVLINK_MSG_ID_ATTITUDE\n");
768            mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
769            current_messages.time_stamps.attitude = get_time_usec();
770            this_timestamps.attitude = current_messages.time_stamps.attitude;
771            break;
772        }
773
774        case MAVLINK_MSG_ID_COMMAND_LONG:
775        {
776            //printf("[RECV] MAVLINK_MSG_ID_COMMAND_LONG\n");
777            mavlink_msg_command_long_decode(&message, &(current_messages.command_long));
778            current_messages.time_stamps.command_long = get_time_usec();
779            this_timestamps.command_long = current_messages.time_stamps.command_long;
780            // process the received command
781            printf("Received command %d\n", current_messages.command_long.command);
782            switch(current_messages.command_long.command) {
783            case MAV_CMD_DO_SET_PARAMETER:
784                switch((int)current_messages.command_long.param1) {
785                case MAV_LOOP_MISSION_ITEMS:
786                    if((int)current_messages.command_long.param2) {
787                        itemsLoop();
788                    } else {
789                        itemsNoLoop();
790                    }
791
792                    break;
793                default:
794                    break;
795                }
796
797                break;
798            case MAV_CMD_SET_MESSAGE_INTERVAL:
799                cout << "[DEBUG] callback interval value: " << (int64_t)current_messages.command_long.param2 << endl;
800                int64_t desired_interval;
801                desired_interval = (int64_t)current_messages.command_long.param2;
802                switch((int)current_messages.command_long.param1) {
803                case MAVLINK_MSG_ID_HEARTBEAT:
804                    if(current_messages.command_long.param2 == -1) {
805                        // stop sending position
806                        sendingHeartbeat = false;
807                    } else {
808                        // start sending position
809                        sendingHeartbeatInterval = desired_interval;
810                        sendingHeartbeat = true;
811                    }
812                    break;
813                case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
814                    if(current_messages.command_long.param2 == -1) {
815                        // stop sending position
816                        sendingPosition = false;
817                    } else {
818                        // start sending position
819                        sendingPositionInterval = desired_interval;
820                        sendingPosition = true;
821                    }
822                    break;
823                case MAVLINK_MSG_ID_ATTITUDE:
824                    if(current_messages.command_long.param2 == -1) {
825                        // stop sending attitude
826                        sendingAttitude = false;
827                    } else {
828                        // start sending attitude
829                        sendingAttitudeInterval = desired_interval;
830                        sendingAttitude = true;
831                    }
832                    break;
833                case MAVLINK_MSG_ID_SYS_STATUS:
834                    if(current_messages.command_long.param2 == -1) {
835                        // stop sending status
836                        sendingSystemStatus = false;
837                    } else {
838                        // start sending status
839                        sendingSystemStatusInterval = desired_interval;
840                        sendingSystemStatus = true;
841                    }
842                    break;
843                default:
844                    break;
845                }
846
847                break;
848            case MAV_CMD_MISSION_START:
849            {
850                MissionCommand missCom;
851                missCom.commandId = mavlink_msg_command_long_get_command(&message);
852                missCom.firstItem = (uint16_t)mavlink_msg_command_long_get_param1(&message);
853                missCom.lastItem = (uint16_t)mavlink_msg_command_long_get_param2(&message);
854                missionCommands.push(missCom);
855                printf("[RECV] MAV_CMD_MISSION_START: firstItem = %d, lastItem = %d\n", missCom.firstItem, missCom.lastItem);
856                missionStarted();
857                break;
858            }
859            case MAV_CMD_DO_LAND_START:
860                // TODO do land start
861                break;
862            case MAV_CMD_DO_SET_HOME:
863            {
864                MissionCommand missCom;
865                missCom.commandId = mavlink_msg_command_long_get_command(&message);
866                missCom.useCurrent = (uint8_t)mavlink_msg_command_long_get_param1(&message);
867                missCom.latitude = mavlink_msg_command_long_get_param5(&message);
868                missCom.longitude = mavlink_msg_command_long_get_param6(&message);
869                missCom.altitude = mavlink_msg_command_long_get_param7(&message);
870                missionCommands.push(missCom);
871                printf("[RECV] SET HOME: use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", missCom.useCurrent, missCom.latitude, missCom.longitude, missCom.altitude);
872                break;
873            }
874            case MAV_CMD_GET_HOME_POSITION:
875            {
876                MissionCommand missCom;
877                missCom.commandId = mavlink_msg_command_long_get_command(&message);
878                missionCommands.push(missCom);
879                printf("[RECV] GET HOME POSITION\n");
880                break;
881            }
882            case MAV_CMD_DO_SET_MODE:
883            {
884                MissionCommand missCom;
885                missCom.commandId = mavlink_msg_command_long_get_command(&message);
886                missCom.mode = mavlink_msg_command_long_get_param1(&message);
887                missionCommands.push(missCom);
888                printf("[RECV] SET MODE: %d\n", missCom.mode);
889                break;
890            }
891            case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
892                // TODO request autopilot capabilities
893                break;
894            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
895            {
896                MissionItem missItem;
897                missItem.itemID = mavlink_msg_command_long_get_command(&message);
898                missionPlan.push(missItem);
899                MissionCommand missCom;
900                missCom.commandId = mavlink_msg_command_long_get_command(&message);
901                missionCommands.push(missCom);
902                printf("[RECV] Return to launch.\n");
903                break;
904            }
905            case MAV_CMD_DO_REPOSITION:
906                // TODO reposition
907                printf("[TODO] Do reposition.\n");
908                break;
909            case MAV_CMD_NAV_TAKEOFF:
910                // TODO takeoff
911                printf("[TODO] Execute takeoff.\n");
912                break;
913            case MAV_CMD_NAV_LAND:
914                // TODO emergency landing
915                printf("[TODO] Execute emergency landing.\n");
916                break;
917            case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
918                // shutdown signal received
919                shutdown_flag = true;
920                break;
921            case MAV_CMD_DO_JUMP:
922            {
923                // jump to a desired command in mission list, repeat a specified number of times
924                MissionItem missItem;
925                missItem.itemID = mavlink_msg_command_long_get_command(&message);
926                missItem.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message);
927                missItem.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message);
928                missionPlan.push(missItem);
929                MissionCommand missCom;
930                missCom.commandId = mavlink_msg_command_long_get_command(&message);
931                missCom.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message);
932                missCom.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message);
933                missionCommands.push(missCom);
934                printf("[RECV] MAV_CMD_DO_JUMP: seq = %d, repeat = %d\n", missItem.sequence, missItem.repeatCount);
935            }
936            case MAV_CMD_DO_PAUSE_CONTINUE:
937            {
938                MissionCommand missCom;
939                missCom.commandId = mavlink_msg_command_long_get_command(&message);
940                missCom.pauseContinue = (uint16_t)mavlink_msg_command_long_get_param1(&message);
941                missionCommands.push(missCom);
942                printf("[RECV] MAV_CMD_DO_PAUSE_CONTINUE: pauseContinue = %d\n", missCom.pauseContinue);
943            }
944            default:
945                break;
946            }
947            // respond with ACK immediately
948            sendCommandAck(current_messages.command_long.command, MAV_CMD_ACK_OK);
949            break;
950        }
951        default:
952        {
953            printf("Warning, did not handle message id %i\n",message.msgid);
954            break;
955        }
956        }
957    } else {
958        printf("ERROR: CRC check failed!\n");
959    }
960}
961
962uint64_t MavlinkUDP::get_time_usec() {
963    struct timeval tv;
964    gettimeofday(&tv, NULL);
965    return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
966}
967
968void MavlinkUDP::debug(string debug_msg) {
969    if(debug_messages) cout << "[DEBUG] " << debug_msg << endl;
970}
971
972
973int MavlinkUDP::waitCommandAck(uint64_t timeout) {
974    uint64_t start_time = get_time_usec();
975    uint64_t end_time = start_time + timeout;
976    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
977    while(get_time_usec() < end_time)
978    {
979        if(getCommandAck()){
980            //printf("Command ACK received!\n");
981            resetCommandAck();
982            return 0;
983        }
984        usleep(1000);
985    }
986    printf("[ERROR] Command ACK timeout! %d\n", getCommandAck());
987    return -1;
988}
989
990int MavlinkUDP::waitMissionAck(uint64_t timeout) {
991    uint64_t start_time = get_time_usec();
992    uint64_t end_time = start_time + timeout;
993    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
994    while(get_time_usec() < end_time)
995    {
996        if(getMissionAck()){
997            //printf("Mission ACK received!\n");
998            resetMissionAck();
999            return 0;
1000        }
1001        usleep(1000);
1002    }
1003    printf("[ERROR] Mission ACK timeout!\n");
1004    return -1;
1005}
1006
1007int MavlinkUDP::waitMissionCount(uint64_t timeout) {
1008    uint64_t start_time = get_time_usec();
1009    uint64_t end_time = start_time + timeout;
1010    int mission_count;
1011    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
1012    while(get_time_usec() < end_time)
1013    {
1014        // assignment in if!
1015        if((mission_count = getMissionCount())){
1016            //printf("Mission ACK received!\n");
1017            resetMissionCount();
1018            return mission_count;
1019        }
1020        usleep(1000);
1021    }
1022    printf("[ERROR] Mission ACK timeout!\n");
1023    return -1;
1024}
1025
1026MavlinkComponent::MavlinkComponent(std::string addr) : addrIP(addr) {
1027    // craete the system ID out of the IP address
1028    int byte1, byte2, byte3, byte4;
1029    char dot = '.';
1030    std::istringstream s(addrIP);
1031    s >> byte1 >> dot >> byte2 >> dot >> byte3 >> dot >> byte4;
1032    sysid = byte4;
1033    compid = 0;
1034    printf("Mavlink component created:\tSystem ID %d\tComponent ID %d\n", sysid, compid);
1035}
Note: See TracBrowser for help on using the repository browser.