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

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

Update MavlinkUDP files and add pause function

File size: 59.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    heartbeat_period = HEARTBEAT_DEFAULT_PERIOD;
46
47    resetCommandAck();
48    resetMissionAck();
49
50    // period function flags
51    sendingAttitude = false;
52    sendingPosition = false;
53    sendingSystemStatus = false;
54    sendingAttitudeInterval = ATTITUDE_DEFAULT_PERIOD;
55    sendingPositionInterval = POSITION_DEFAULT_PERIOD;
56    sendingSystemStatusInterval = STATUS_DEFAULT_PERIOD;
57
58    // mission items
59    missionFirst = 0;
60    missionLast = 0;
61    //mission_items_loop = false;
62    missionActive = false; // if the mission is launched
63
64    shutdown_flag = false;
65
66    // TODO read the system ID from the file based on the IP address
67    system_id    = 0; // system id
68    autopilot_id = 0; // autopilot component id
69    component_id = 0; // companion computer component id
70
71    // setpoint
72    recvSetpointX = 0;
73    recvSetpointY = 0;
74
75   // mavlink_command_long_t com = { 0 };
76    compt=0;
77    current_messages.sysid  = system_id;
78    current_messages.compid = autopilot_id;
79    Xtimes=0;
80    Xtimesync=false;
81
82    something_to_send = false; // check if something needs to be sent
83    stop_recv = false;
84    stop_send = false;
85    startThreads(); // start both sending and receiving thread
86}
87
88MavlinkUDP::~MavlinkUDP() {
89    try {
90        stopThreads();
91    } catch (int error) {
92        fprintf(stderr,"MavlinkUDP: Warning! Could not stop threads!\n");
93    }
94}
95
96// starting send and receive threads
97void MavlinkUDP::startThreads() {
98    // start receive thread
99    recv_th = std::thread(&MavlinkUDP::recv_thread, this);
100    printf("MavlinkUDP: Receiver thread created.\n");
101    // start sending thread
102    send_th = std::thread(&MavlinkUDP::send_thread, this);
103    printf("MavlinkUDP: Sender thread created.\n");
104}
105
106void MavlinkUDP::stopThreads() {
107    stop_recv = true;
108    stop_send = true;
109    usleep(100);
110    if(recv_th.joinable()) recv_th.join();
111    if(send_th.joinable()) send_th.join();
112    printf("MavlinkUDP: Threads stopped.\n");
113    // close the sockets
114    close(m_socket_in);
115    close(m_socket_out);
116    printf("MavlinkUDP: Sockets closed.\n");
117}
118
119// quit handler
120void MavlinkUDP::handleQuit(int sig) {
121    try {
122        stopThreads();
123    } catch (int error) {
124        fprintf(stderr,"MavlinkUDP: Warning, could not stop communication block!\n");
125    }
126}
127
128inline void MavlinkUDP::clearBuffer(uint8_t *buffer, int len) {
129    memset(buffer, 0, len);
130}
131
132void MavlinkUDP::clearData() {
133    clearBuffer(buff_in, BUFF_IN_LEN);
134}
135
136void MavlinkUDP::recv_thread() {
137    // UDP socket setup
138    socklen_t clilen=sizeof(client_in);
139    if((m_socket_in=socket(AF_INET,SOCK_DGRAM,0))<0)
140    {
141        throw SocketRuntimeError("[ERRR] RECV socket error!\n");
142    }
143    memset((char *)&myaddr_in, 0, sizeof(myaddr_in));
144    myaddr_in.sin_family=AF_INET;
145    // receiving messages from all addresses
146    myaddr_in.sin_addr.s_addr=htonl(INADDR_ANY);
147    myaddr_in.sin_port=htons(m_port);
148    if(bind(m_socket_in,(struct sockaddr *)&myaddr_in,sizeof(myaddr_in))<0)
149    {
150        throw SocketRuntimeError("[ERRR] UDP in bind error!\n");
151    }
152    //std::cout << "MavlinkUDP: RECV socket bind OK.\n";
153    std::cout << "MavlinkUDP: RECV thread launched.\n";
154    clearBuffer(buff_in, BUFF_IN_LEN);
155    // receive data
156    while(!stop_recv) {
157        recsize = recvfrom(m_socket_in, (void *)buff_in, BUFF_IN_LEN, 0, (struct sockaddr *)&client_in, &clilen);
158        if(recsize > 0) {
159            // parse the received data
160            //print_message(sockUDP.buff_in, sockUDP.recsize);
161            decode_message(buff_in, recsize);
162            clearBuffer(buff_in, BUFF_IN_LEN);
163        }
164        //usleep(10000);
165        std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_IN));
166    }
167}
168
169void MavlinkUDP::send_thread() {
170    // socket setup
171    int broadcast = 1;
172    // socket for broadcast
173    if((m_socket_out=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0) {
174        throw SocketRuntimeError("[ERRR] udp out socket error\n");
175    }
176    if((setsockopt(m_socket_out,SOL_SOCKET,SO_BROADCAST,&broadcast,sizeof broadcast)) == -1) {
177        throw SocketRuntimeError("[ERRR] udp out setsockopt error\n");
178    }
179    cout << "MavlinkUDP: SEND thread launched.\n";
180    memset((char *) &myaddr_out, 0, sizeof(myaddr_out));
181    myaddr_out.sin_family=AF_INET;
182    myaddr_out.sin_port=htons(m_port);
183    myaddr_out.sin_addr.s_addr=inet_addr(m_addr.c_str());
184    clearBuffer(buff_out, BUFF_OUT_LEN);
185    // send data to the socket
186    while(!stop_send) {
187        if(something_to_send) {
188            // send quickly and reset the flag
189            sendto(m_socket_out, buff_out, length_to_send, 0, (struct sockaddr *) &myaddr_out, sizeof(myaddr_out));
190            clearBuffer(buff_out,BUFF_OUT_LEN);
191            something_to_send = false;
192        }
193        //clearBuffer(buff_out, BUFF_OUT_LEN);
194        std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_OUT));
195        //usleep(10000);
196    }
197}
198
199// read mission configuration file
200// TODO use the Qt XML library
201//
202//
203
204void MavlinkUDP::updateSetpoint(mavlink_set_position_target_local_ned_t setpoint) {
205    current_setpoint = setpoint;
206}
207
208//
209// MESSAGES
210//
211
212// send Heartbeat message
213void MavlinkUDP::sendHeartbeat() {
214    mavlink_message_t message;
215    mavlink_msg_heartbeat_pack(me.getSysID(), me.getCompID(), &message, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_AUTO_ARMED, 0, MAV_STATE_ACTIVE);
216    length_to_send = mavlink_msg_to_send_buffer(buff_out, &message);
217    something_to_send = true;
218    //printf("[SENT] Heartbeat\n");
219}
220
221// send SystemStatus message
222void 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) {
223    mavlink_message_t message;
224    mavlink_msg_sys_status_pack(me.getSysID(), me.getCompID(), &message, onboardSensorsPresent, onboardSensorsEnabled, onboardSensorsHealth, load, voltage, current, batteryRemaining, dropRateComm, errorsComm, errors1, errors2, errors3, errors4);
225    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
226    something_to_send = true;
227    //printf("[SENT] System status\n");
228}
229
230// send BatteryStatus message
231void 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) {
232    mavlink_message_t message;
233    mavlink_msg_battery_status_pack(me.getSysID(), me.getCompID(), &message, id, battery_function, type, temperature, voltages, current, currentConsumed, energyConsumed, batteryRemaining);
234    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
235    something_to_send = true;
236}
237
238// send SystemTime message
239void MavlinkUDP::sendSystemTime() {
240    mavlink_system_time_t sys_time;
241    sys_time.time_unix_usec = get_time_usec();
242    mavlink_message_t message;
243    mavlink_msg_system_time_pack(me.getSysID(), me.getCompID(), &message, sys_time.time_unix_usec, sys_time.time_boot_ms);
244    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
245    something_to_send = true;
246    printf("[SENT] System time\n");
247}
248
249// send LocalPostiionNED message
250void MavlinkUDP::sendLocalPositionNED(float x, float y, float z, float vx, float vy, float vz) {
251    mavlink_message_t message;
252    mavlink_msg_local_position_ned_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), x, y, z, vx, vy, vz);
253    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
254    something_to_send = true;
255}
256
257// send Attitude message
258void MavlinkUDP::sendAttitude(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) {
259    mavlink_message_t message;
260    mavlink_msg_attitude_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
261    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
262    something_to_send = true;
263}
264
265// send MissionAck message
266void MavlinkUDP::sendMissionAck(uint8_t targetSystem, uint8_t targetComponent, uint8_t type) {
267    mavlink_message_t message;
268    mavlink_msg_mission_ack_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, type);
269    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
270    something_to_send = true;
271    printf("[SENT] Mission ACK\n");
272}
273
274// send commandAck message
275void MavlinkUDP::sendCommandAck(uint16_t command, uint8_t result) {
276    mavlink_message_t message;
277    mavlink_msg_command_ack_pack(me.getSysID(), me.getCompID(), &message, command, result);
278    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
279    something_to_send = true;
280    printf("[SENT] Command ACK: %d\n", command);
281}
282
283// send autopilot version
284void 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) {
285    mavlink_message_t message;
286    // TODO get these values from autopilot
287    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);
288    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
289    something_to_send = true;
290    printf("[SENT] Autopilot vesrion\n");
291}
292
293// send MissionCount message
294void MavlinkUDP::sendMissionCount(uint8_t targetSystem, uint8_t targetComponent, uint16_t count) {
295    mavlink_message_t message;
296    mavlink_msg_mission_count_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, count);
297    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
298    something_to_send = true;
299}
300
301// send CommandLong message
302void 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) {
303    mavlink_message_t message;
304    //    uint8_t confirmation;
305    //    float param1, param2, param3, param4, param5, param6, param7;
306    mavlink_msg_command_long_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, command, confirmation, param1, param2, param3, param4, param5, param6, param7);
307    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
308    something_to_send = true;
309    printf("[SENT] Command long: %d\n", command);
310}
311
312// send MissionWritePartialList message
313void MavlinkUDP::sendMissionWritePartialList(uint8_t targetSystem, uint8_t targetComponent, uint16_t startIndex, uint16_t endIndex) {
314    mavlink_message_t message;
315    mavlink_msg_mission_write_partial_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, startIndex, endIndex);
316    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
317    something_to_send = true;
318}
319
320// send MissionItem message
321void 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) {
322    mavlink_message_t message;
323    mavlink_msg_mission_item_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
324    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
325    something_to_send = true;
326}
327
328// send MissionRequestList message
329void MavlinkUDP::sendMissionRequestList(uint8_t targetSystem, uint8_t targetComponent) {
330    mavlink_message_t message;
331    mavlink_msg_mission_request_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
332    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
333    something_to_send = true;
334}
335
336// send MissionItemReached message
337void MavlinkUDP::sendMissionItemReached(uint16_t seq) {
338    mavlink_message_t message;
339    mavlink_msg_mission_item_reached_pack(me.getSysID(), me.getCompID(), &message, seq);
340    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
341    something_to_send = true;
342}
343
344// send MissionSetCurrent message
345void MavlinkUDP::sendMissionSetCurrent(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq) {
346    mavlink_message_t message;
347    mavlink_msg_mission_set_current_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq);
348    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
349    something_to_send = true;
350}
351
352// send MissionClearAll message
353void MavlinkUDP::sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent) {
354    mavlink_message_t message;
355    mavlink_msg_mission_clear_all_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
356    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
357    something_to_send = true;
358    printf("[SENT] Mission clear all\n");
359}
360
361// send SetPositionTargetLocalNED message
362void 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) {
363    mavlink_message_t message;
364    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);
365    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
366    printf("[SENT] SET_POSITION_TARGET_LOCAL_NED x = %.2f y = %.2f\n", x, y);
367    something_to_send = true;
368}
369
370// send PositionTargetLocalNED message
371void MavlinkUDP::sendPositionTargetLocalNED(uint32_t time_boot_ms, 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) {
372    mavlink_message_t message;
373    mavlink_msg_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, coordinateFrame, typeMask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
374    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
375    something_to_send = true;
376}
377
378// send SetPositionTargetGlobalInt message
379void 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) {
380    mavlink_message_t message;
381    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);
382    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
383    something_to_send = true;
384}
385
386// send PositionTargetGlobalInt message
387void MavlinkUDP::sendPositionTargetGlobalInt(uint32_t time_boot_ms, 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) {
388    mavlink_message_t message;
389    mavlink_msg_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, coordinateFrame, typeMask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
390    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
391    something_to_send = true;
392}
393
394//
395// COMMANDS
396//
397
398// command Waypoint
399void MavlinkUDP::cmdNavWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {
400    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_WAYPOINT, 0, holdTime, proximityRadius, passRadius, desiredYaw, latitude, longitude, altitude);
401}
402
403// command SetMessageInterval
404void MavlinkUDP::cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec) {
405    sendCommandLong(targetSystem, targetComponent, MAV_CMD_SET_MESSAGE_INTERVAL, 0, (float)messageID, (float)interval_usec, 0, 0, 0, 0, 0);
406}
407
408// command Land
409void MavlinkUDP::cmdNavLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
410    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_LAND, 0, abortAlt, 0, 0, desiredYaw, latitude, longitude, altitude);
411}
412
413// command LandStart
414void MavlinkUDP::cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude) {
415    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_LAND_START, 0, 0, 0, 0, 0, latitude, longitude, 0);
416}
417
418// command TakeOff
419void MavlinkUDP::cmdNavTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
420    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_TAKEOFF, 0, desiredPitch, 0, 0, magnetometerYaw, latitude, longitude, altitude);
421}
422
423// command DoSetMode
424void MavlinkUDP::cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode) {
425    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_MODE, 0, mavMode, 0, 0, 0, 0, 0, 0);
426}
427
428// command DoPauseContinue
429void MavlinkUDP::cmdDoPauseContinue(uint8_t targetSystem, uint8_t targetComponent, uint8_t pauseContinue) {
430    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_PAUSE_CONTINUE, 0, pauseContinue, 0, 0, 0, 0, 0, 0);
431}
432
433// command DoSetHome
434void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) {
435    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_HOME, 0, (float)useCurrent, 0, 0, 0, 0, 0, 0);
436}
437
438// command GetHomePosition
439void MavlinkUDP::cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent) {
440    sendCommandLong(targetSystem, targetComponent, MAV_CMD_GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, 0);
441}
442
443// command MissionStart
444void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) {
445    sendCommandLong(targetSystem, targetComponent, MAV_CMD_MISSION_START, 0, (float)firstItem, (float)lastItem, 0, 0, 0, 0, 0);
446}
447
448// command DoSetParameter
449void MavlinkUDP::cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue) {
450    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_PARAMETER, 0, (float)paramNumber, paramValue, 0, 0, 0, 0, 0);
451}
452
453// command RequestAutopilotCapabilities
454void MavlinkUDP::cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent) {
455    sendCommandLong(targetSystem, targetComponent, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, 0, 1, 0, 0, 0, 0, 0, 0);
456}
457
458// command ReturnToLaunch
459void MavlinkUDP::cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
460    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0);
461}
462
463// command Shutdown
464void MavlinkUDP::cmdRebootShutdown(uint8_t targetSystem, uint8_t targetComponent, uint8_t autopilot, uint8_t onboardComputer) {
465    // for autopilot and onboardComputer, see TMAV_REBOOT_SHUTDOWN_PARAM
466    sendCommandLong(targetSystem, targetComponent, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, autopilot, onboardComputer, 0, 0, 0, 0, 0);
467}
468
469
470// callback Heartbeat
471void MavlinkUDP::startHeartbeat(uint64_t period) {
472    timerHeartbeat.start(period, std::bind(&MavlinkUDP::callbackHeartbeat,this));
473}
474void MavlinkUDP::stopHeartbeat() {
475    timerHeartbeat.stop();
476}
477void MavlinkUDP::callbackHeartbeat() {
478    sendHeartbeat();
479}
480
481// callback SystemTime
482void MavlinkUDP::startSystemTime(uint64_t period) {
483    timerSystemTime.start(period, std::bind(&MavlinkUDP::callbackSystemTime,this));
484}
485void MavlinkUDP::stopSystemTime() {
486    timerSystemTime.stop();
487}
488void MavlinkUDP::callbackSystemTime() {
489    sendSystemTime();
490}
491
492// stop the callback function timers
493void MavlinkUDP::stopAllCallbackTimers() {
494    stopHeartbeat();
495    stopSystemTime();
496}
497
498// check the received message CRC, return true if OK
499bool MavlinkUDP::check_mavlink_crc(u_int8_t *buff_in, ssize_t recsize, u_int8_t msgid) {
500    u_int16_t *crc_accum = new u_int16_t(X25_INIT_CRC);
501    u_int16_t recv_crc;
502    for (int i = 1; i < (recsize-2); ++i)
503        crc_accumulate(buff_in[i],crc_accum);
504    crc_accumulate(hds_mavlink_message_crcs[msgid],crc_accum);
505    recv_crc = buff_in[recsize-1]<<8 ^ buff_in[recsize-2];
506    //cout << "CRC(recv): " << hex << setw(4) << recv_crc << endl;
507    //cout << "CRC(calc): " << hex << setw(4) << *crc_accum << endl;
508    // if the CRCs are the same, the subtraction result should be 0:
509    recv_crc -= *crc_accum;
510    delete crc_accum;
511    if(!recv_crc) return true;
512    return false;
513}
514
515// parse the received message
516void MavlinkUDP::print_message(u_int8_t *buff_in, ssize_t recsize) {
517    uint8_t temp;
518    mavlink_message_t msg;
519    mavlink_status_t status;
520    printf("\n[PRINT MESSAGE]\n");
521    printf("Timestamp: %ld\n", get_time_usec());
522    printf("Bytes received: %ld\n", recsize);
523    printf("Datagram: ");
524    for(int i=0; i<recsize; ++i) {
525        temp = buff_in[i];
526        //cout << base(10) << temp << " ";
527        printf("%02x ", temp);
528        if(mavlink_parse_char(MAVLINK_COMM_0, temp, &msg, &status)) {
529            printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
530            //printf("Iteration: %d\n", i);
531        }
532    }
533    printf("\n");
534
535}
536
537void MavlinkUDP::decode_message(u_int8_t *buff_in, ssize_t msg_size) {
538    mavlink_message_t message;
539    mavlink_status_t status;
540    Time_Stamps this_timestamps;
541    // parse message
542    for(int i=0; i<msg_size; ++i) mavlink_parse_char(MAVLINK_COMM_0, buff_in[i], &message, &status);
543    // handle message ID
544    current_messages.sysid  = message.sysid;
545    current_messages.compid = message.compid;
546//    printf("[DECODE]\n");
547//    printf("System ID: %d\n", message.sysid);
548//    printf("Component ID: %d\n", message.compid);
549//    printf("Message ID: %d\n", message.msgid);
550//    printf("Length: %d\n", message.len);
551
552    if(check_mavlink_crc(buff_in, msg_size, message.msgid))
553    {
554        switch (message.msgid)
555        {
556        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
557        {
558            printf("[RECV] MAVLINK_MSG_ID_MISSION_CLEAR_ALL\n");
559            mavlink_msg_mission_clear_all_decode(&message,&(current_messages.clear_all));
560            current_messages.time_stamps.clear_all = get_time_usec();
561            this_timestamps.clear_all = current_messages.time_stamps.clear_all;
562            // clear the queue
563            clearMissionPlan();
564            MavlinkItem mavItem;
565            mavItem.id = message.msgid;
566            missionCommands.push(mavItem);
567            //printf("[DEBUG] sysid = %d compid = %d\n", message.sysid, message.compid);
568            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
569            break;
570        }
571        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
572        {
573            printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM_REACHED\n");
574            mavlink_msg_mission_item_reached_decode(&message, &(current_messages.mission_reached));
575            current_messages.time_stamps.mission_reached = get_time_usec();
576            this_timestamps.mission_reached = current_messages.time_stamps.mission_reached;
577            seqr=current_messages.mission_reached.seq;
578            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
579            break;
580        }
581        case MAVLINK_MSG_ID_MISSION_COUNT:
582        {
583            printf("[RECV] MAVLINK_MSG_ID_MISSION_COUNT\n");
584            mavlink_msg_mission_count_decode(&message, &(current_messages.mission_count));
585            current_messages.time_stamps.mission_count = get_time_usec();
586            this_timestamps.mission_count = current_messages.time_stamps.mission_count;
587            Waycount=current_messages.mission_count.count;
588            setMissionCount(current_messages.mission_count.count);
589            compt=0;
590            Wayseq=-1;
591            break;
592        }
593        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
594        {
595            printf("[RECV] MAVLINK_MSG_ID_MISSION_REQUEST_LIST\n");
596            mavlink_msg_mission_request_list_decode(&message, &(current_messages.mission_request_list));
597            current_messages.time_stamps.mission_request_list = get_time_usec();
598            this_timestamps.mission_request_list = current_messages.time_stamps.mission_request_list;
599            // send the mission count
600            sendMissionCount(target.getSysID(), target.getCompID(), getMissionCount());
601            break;
602        }
603        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
604        {
605            printf("[RECV] MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST\n");
606            mavlink_msg_mission_write_partial_list_decode(&message, &(current_messages).mission_write_partial_list);
607            current_messages.time_stamps.mission_write_partial_list = get_time_usec();
608            this_timestamps.mission_write_partial_list = current_messages.time_stamps.mission_write_partial_list;
609            //
610            // TODO process the partial list message
611            //
612            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
613            break;
614        }
615        case MAVLINK_MSG_ID_TIMESYNC:
616        {
617            //  printf("MAVLINK_MSG_ID_TIMESYNC\n");
618            mavlink_msg_timesync_decode(&message, &(current_messages.timesync));
619            current_messages.time_stamps.timesync = get_time_usec();
620            this_timestamps.timesync = current_messages.time_stamps.timesync;
621            Xtimec=current_messages.timesync.tc1;
622            Xtimes=current_messages.timesync.ts1;
623            Xtimesync=true;
624            break;
625        }
626        case MAVLINK_MSG_ID_SYSTEM_TIME:
627        {
628            printf("[RECV] MAVLINK_MSG_ID_SYSTEM_TIME\n");
629            mavlink_msg_system_time_decode(&message, &(current_messages.system_time));
630            current_messages.time_stamps.system_time = get_time_usec();
631            this_timestamps.system_time = current_messages.time_stamps.system_time;
632            break;
633        }
634        case MAVLINK_MSG_ID_MISSION_ITEM:
635        {
636            printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM\n");
637            mavlink_msg_mission_item_decode(&message, &(current_messages.mission_item));
638            current_messages.time_stamps.mission_item = get_time_usec();
639            this_timestamps.mission_item = current_messages.time_stamps.mission_item;
640
641            // * Mission items queue:
642            // * 16   MAV_CMD_NAV_WAYPOINT            (hold time, acceptance radius, yaw angle, lat, long, altitude)
643            // * 21   MAV_CMD_NAV_LAND                (abort alt, yaw angle, lat, long, altitude)
644            // * 22   MAV_CMD_NAV_TAKEOFF             (yaw angle, lat, long, altitude)
645            // * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
646            // * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
647
648            //waypoint=true;
649            //setMissionCount(getMissionCount()+1);
650            //Wayseq=current_messages.mission_item.seq;
651            //frame=current_messages.mission_item.frame;
652            command=current_messages.mission_item.command;
653
654            switch(command) {
655            case MAV_CMD_NAV_TAKEOFF:
656            {
657                MavlinkItem mavItem;
658                mavItem.id = command;
659                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
660                mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message);
661                mavItem.latitude = mavlink_msg_mission_item_get_x(&message);
662                mavItem.longitude = mavlink_msg_mission_item_get_y(&message);
663                mavItem.altitude = mavlink_msg_mission_item_get_z(&message);
664                // adding commands to the mission plan
665                missionPlan.push(mavItem);
666                //missionCommands.push(mavItem);
667                printf("[RECV] MISSION_ITEM: TAKEOFF\n");
668                break;
669            }
670            case MAV_CMD_NAV_LAND:
671            {
672                MavlinkItem mavItem;
673                mavItem.id = command;
674                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
675                mavItem.abort_altitude = mavlink_msg_mission_item_get_param1(&message);
676                mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message);
677                mavItem.latitude = mavlink_msg_mission_item_get_x(&message);
678                mavItem.longitude = mavlink_msg_mission_item_get_y(&message);
679                mavItem.altitude = mavlink_msg_mission_item_get_z(&message);
680                // adding commands to the mission plan
681                missionPlan.push(mavItem);
682                printf("[RECV] MISSION_ITEM: LAND\n");
683                break;
684            }
685            case MAV_CMD_DO_JUMP:
686            {
687                MavlinkItem mavItem;
688                mavItem.id = command;
689                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
690                mavItem.jump_sequence = mavlink_msg_mission_item_get_param1(&message);
691                mavItem.jump_repeat_count = mavlink_msg_mission_item_get_param2(&message);
692                // adding commands to the mission plan
693                missionPlan.push(mavItem);
694                printf("[RECV] MISSION_ITEM: MAV_CMD_DO_JUMP seq %d repeat %d\n", mavItem.jump_sequence, mavItem.jump_repeat_count);
695                break;
696            }
697            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
698            {
699                MavlinkItem mavItem;
700                mavItem.id = command;
701                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
702                // adding commands to the mission plan
703                missionPlan.push(mavItem);
704                printf("[RECV] MISSION_ITEM: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
705                break;
706            }
707            case MAV_CMD_NAV_WAYPOINT:
708            {
709                MavlinkItem mavItem;
710                mavItem.id = command;
711                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
712                mavItem.hold_time = mavlink_msg_mission_item_get_param1(&message);
713                mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message);
714                mavItem.latitude = mavlink_msg_mission_item_get_x(&message);
715                mavItem.longitude = mavlink_msg_mission_item_get_y(&message);
716                mavItem.altitude = mavlink_msg_mission_item_get_z(&message);
717                // adding commands to the mission plan
718                missionPlan.push(mavItem);
719                printf("[RECV] MISSION_ITEM: WAYPOINT seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", mavItem.sequence, mavItem.latitude, mavItem.longitude, mavItem.altitude);
720                break;
721            }
722            }
723            // respond with ACK immediately
724            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
725            break;
726        }
727        case MAVLINK_MSG_ID_MISSION_ACK:
728        {
729            printf("[RECV] MAVLINK_MSG_ID_MISSION_ACK\n");
730            mavlink_msg_mission_ack_decode(&message, &(current_messages.mission_ack));
731            // check if it is the right mission ack
732            current_messages.time_stamps.mission_ack = get_time_usec();
733            this_timestamps.mission_ack = current_messages.time_stamps.mission_ack;
734            setMissionAck();
735            //typeack=current_messages.mission_ack.type;
736            break;
737        }
738        case MAVLINK_MSG_ID_COMMAND_ACK:
739        {
740            printf("[RECV] MAVLINK_MSG_ID_COMMAND_ACK\n");
741            mavlink_msg_command_ack_decode(&message, &(current_messages.command_ack));
742            // check if it is the right command ack
743            current_messages.time_stamps.command_ack = get_time_usec();
744            this_timestamps.command_ack = current_messages.time_stamps.command_ack;
745            setCommandAck();
746            break;
747        }
748        case MAVLINK_MSG_ID_MISSION_REQUEST:
749        {
750            //    printf("MAVLINK_MSG_ID_MISSION_REQUEST\n");
751            mavlink_msg_mission_request_decode(&message, &(current_messages.mission_request));
752            seq=current_messages.mission_request.seq;
753            current_messages.time_stamps.mission_request = get_time_usec();
754            this_timestamps.mission_request = current_messages.time_stamps.mission_request;
755            request=true;
756            break;
757        }
758        case MAVLINK_MSG_ID_HOME_POSITION:
759        {
760            printf("[RECV] MAVLINK_MSG_ID_HOME_POSITION\n");
761            mavlink_msg_home_position_decode(&message, &(current_messages.home_position));
762            current_messages.time_stamps.home_position = get_time_usec();
763            this_timestamps.home_position = current_messages.time_stamps.home_position;
764            home_position_set=true;
765            break;
766        }
767        case MAVLINK_MSG_ID_HEARTBEAT:
768        {
769            printf("[RECV] MAVLINK_MSG_ID_HEARTBEAT\n");
770            mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
771            current_messages.time_stamps.heartbeat = get_time_usec();
772            this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
773            state=current_messages.heartbeat.system_status;
774            break;
775        }
776
777        case MAVLINK_MSG_ID_SYS_STATUS:
778        {
779            printf("[RECV] MAVLINK_MSG_ID_SYS_STATUS\n");
780            mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
781            current_messages.time_stamps.sys_status = get_time_usec();
782            this_timestamps.sys_status = current_messages.time_stamps.sys_status;
783            //USB_port->write_message(message);
784            break;
785        }
786
787        case MAVLINK_MSG_ID_BATTERY_STATUS:
788        {
789            //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
790            mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
791            current_messages.time_stamps.battery_status = get_time_usec();
792            this_timestamps.battery_status = current_messages.time_stamps.battery_status;
793            //USB_port->write_message(message);
794            break;
795        }
796
797        case MAVLINK_MSG_ID_RADIO_STATUS:
798        {
799            //printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
800            mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
801            current_messages.time_stamps.radio_status = get_time_usec();
802            this_timestamps.radio_status = current_messages.time_stamps.radio_status;
803            break;
804        }
805
806        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
807        {
808            printf("[RECV] MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
809            mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
810            current_messages.time_stamps.local_position_ned = get_time_usec();
811            this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
812            // insert it into the command queue
813            MavlinkItem mavItem;
814            mavItem.id = message.msgid;
815            mavItem.time_boot_ms = mavlink_msg_local_position_ned_get_time_boot_ms(&message);
816            mavItem.x = mavlink_msg_local_position_ned_get_x(&message);
817            mavItem.y = mavlink_msg_local_position_ned_get_y(&message);
818            mavItem.z = mavlink_msg_local_position_ned_get_z(&message);
819            mavItem.vx = mavlink_msg_local_position_ned_get_vx(&message);
820            mavItem.vy = mavlink_msg_local_position_ned_get_vy(&message);
821            mavItem.vz = mavlink_msg_local_position_ned_get_vz(&message);
822            missionCommands.push(mavItem);
823            recvSetpointX = mavItem.x;
824            recvSetpointY = mavItem.y;
825            break;
826        }
827
828        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
829        {
830            printf("[RECV] MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
831            mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
832            current_messages.time_stamps.global_position_int = get_time_usec();
833            this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
834            // insert it into the command queue
835            MavlinkItem mavItem;
836            mavItem.id = message.msgid;
837            mavItem.time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(&message);
838            mavItem.latitude = (float)mavlink_msg_global_position_int_get_lat(&message);
839            mavItem.longitude = (float)mavlink_msg_global_position_int_get_lon(&message);
840            mavItem.altitude = (float)mavlink_msg_global_position_int_get_alt(&message);
841            mavItem.relative_alt = (float)mavlink_msg_global_position_int_get_relative_alt(&message);
842            mavItem.vx = (float)mavlink_msg_global_position_int_get_vx(&message);
843            mavItem.vy = (float)mavlink_msg_global_position_int_get_vy(&message);
844            mavItem.vz = (float)mavlink_msg_global_position_int_get_vz(&message);
845            mavItem.yaw_angle = (float)mavlink_msg_global_position_int_get_hdg(&message); // heading
846            missionCommands.push(mavItem);
847            break;
848        }
849
850        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
851        {
852            mavlink_msg_set_position_target_local_ned_decode(&message, &(current_messages.set_position_target_local_ned));
853            current_messages.time_stamps.set_position_target_local_ned = get_time_usec();
854            this_timestamps.set_position_target_local_ned = current_messages.time_stamps.set_position_target_local_ned;
855            // insert it into the command queue
856            MavlinkItem mavItem;
857            mavItem.id = message.msgid;
858            mavItem.time_boot_ms = mavlink_msg_set_position_target_local_ned_get_time_boot_ms(&message);
859            mavItem.target_system = mavlink_msg_set_position_target_local_ned_get_target_system(&message);
860            mavItem.target_component = mavlink_msg_set_position_target_local_ned_get_target_component(&message);
861            mavItem.coordinate_frame = mavlink_msg_set_position_target_local_ned_get_coordinate_frame(&message);
862            mavItem.type_mask = mavlink_msg_set_position_target_local_ned_get_type_mask(&message);
863            mavItem.x = mavlink_msg_set_position_target_local_ned_get_x(&message);
864            mavItem.y = mavlink_msg_set_position_target_local_ned_get_y(&message);
865            mavItem.z = mavlink_msg_set_position_target_local_ned_get_z(&message);
866            mavItem.vx = mavlink_msg_set_position_target_local_ned_get_vx(&message);
867            mavItem.vy = mavlink_msg_set_position_target_local_ned_get_vy(&message);
868            mavItem.vz = mavlink_msg_set_position_target_local_ned_get_vz(&message);
869            mavItem.afx = mavlink_msg_set_position_target_local_ned_get_afx(&message);
870            mavItem.afy = mavlink_msg_set_position_target_local_ned_get_afy(&message);
871            mavItem.afz = mavlink_msg_set_position_target_local_ned_get_afz(&message);
872            mavItem.yaw = mavlink_msg_set_position_target_local_ned_get_yaw(&message);
873            mavItem.yaw_rate = mavlink_msg_set_position_target_local_ned_get_yaw_rate(&message);
874            missionCommands.push(mavItem);
875            printf("[RECV] MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED x = %.2f y = %.2f\n", mavItem.x, mavItem.y);
876            break;
877        }
878
879        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
880        {
881            printf("[RECV] MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
882            mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
883            current_messages.time_stamps.position_target_local_ned = get_time_usec();
884            this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
885            // insert it into the command queue
886            MavlinkItem mavItem;
887            mavItem.id = message.msgid;
888            mavItem.time_boot_ms = mavlink_msg_position_target_local_ned_get_time_boot_ms(&message);
889            mavItem.coordinate_frame = mavlink_msg_position_target_local_ned_get_coordinate_frame(&message);
890            mavItem.type_mask = mavlink_msg_position_target_local_ned_get_type_mask(&message);
891            mavItem.x = mavlink_msg_position_target_local_ned_get_x(&message);
892            mavItem.y = mavlink_msg_position_target_local_ned_get_y(&message);
893            mavItem.z = mavlink_msg_position_target_local_ned_get_z(&message);
894            mavItem.vx = mavlink_msg_position_target_local_ned_get_vx(&message);
895            mavItem.vy = mavlink_msg_position_target_local_ned_get_vy(&message);
896            mavItem.vz = mavlink_msg_position_target_local_ned_get_vz(&message);
897            mavItem.afx = mavlink_msg_position_target_local_ned_get_afx(&message);
898            mavItem.afy = mavlink_msg_position_target_local_ned_get_afy(&message);
899            mavItem.afz = mavlink_msg_position_target_local_ned_get_afz(&message);
900            mavItem.yaw = mavlink_msg_position_target_local_ned_get_yaw(&message);
901            mavItem.yaw_rate = mavlink_msg_position_target_local_ned_get_yaw_rate(&message);
902            missionCommands.push(mavItem);
903            break;
904        }
905
906        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
907        {
908            printf("[RECV] MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT\n");
909            mavlink_msg_set_position_target_global_int_decode(&message, &(current_messages.set_position_target_global_int));
910            current_messages.time_stamps.set_position_target_global_int = get_time_usec();
911            this_timestamps.set_position_target_global_int = current_messages.time_stamps.set_position_target_global_int;
912            // insert it into the command queue
913            MavlinkItem mavItem;
914            mavItem.id = message.msgid;
915            mavItem.time_boot_ms = mavlink_msg_set_position_target_global_int_get_time_boot_ms(&message);
916            mavItem.target_system = mavlink_msg_set_position_target_global_int_get_target_system(&message);
917            mavItem.target_component = mavlink_msg_set_position_target_global_int_get_target_component(&message);
918            mavItem.coordinate_frame = mavlink_msg_set_position_target_global_int_get_coordinate_frame(&message);
919            mavItem.type_mask = mavlink_msg_set_position_target_global_int_get_type_mask(&message);
920            mavItem.lat_int = mavlink_msg_set_position_target_global_int_get_lat_int(&message);
921            mavItem.lon_int = mavlink_msg_set_position_target_global_int_get_lon_int(&message);
922            mavItem.altitude = mavlink_msg_set_position_target_global_int_get_alt(&message);
923            mavItem.vx = mavlink_msg_set_position_target_global_int_get_vx(&message);
924            mavItem.vy = mavlink_msg_set_position_target_global_int_get_vy(&message);
925            mavItem.vz = mavlink_msg_set_position_target_global_int_get_vz(&message);
926            mavItem.afx = mavlink_msg_set_position_target_global_int_get_afx(&message);
927            mavItem.afy = mavlink_msg_set_position_target_global_int_get_afy(&message);
928            mavItem.afz = mavlink_msg_set_position_target_global_int_get_afz(&message);
929            mavItem.yaw = mavlink_msg_set_position_target_global_int_get_yaw(&message);
930            mavItem.yaw_rate = mavlink_msg_set_position_target_global_int_get_yaw_rate(&message);
931            missionCommands.push(mavItem);
932            break;
933        }
934
935        case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
936        {
937            printf("[RECV] MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
938            mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
939            current_messages.time_stamps.position_target_global_int = get_time_usec();
940            this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
941            // insert it into the command queue
942            MavlinkItem mavItem;
943            mavItem.id = message.msgid;
944            mavItem.time_boot_ms = mavlink_msg_position_target_global_int_get_time_boot_ms(&message);
945            mavItem.coordinate_frame = mavlink_msg_position_target_global_int_get_coordinate_frame(&message);
946            mavItem.type_mask = mavlink_msg_position_target_global_int_get_type_mask(&message);
947            mavItem.lat_int = mavlink_msg_position_target_global_int_get_lat_int(&message);
948            mavItem.lon_int = mavlink_msg_position_target_global_int_get_lon_int(&message);
949            mavItem.altitude = mavlink_msg_position_target_global_int_get_alt(&message);
950            mavItem.vx = mavlink_msg_position_target_global_int_get_vx(&message);
951            mavItem.vy = mavlink_msg_position_target_global_int_get_vy(&message);
952            mavItem.vz = mavlink_msg_position_target_global_int_get_vz(&message);
953            mavItem.afx = mavlink_msg_position_target_global_int_get_afx(&message);
954            mavItem.afy = mavlink_msg_position_target_global_int_get_afy(&message);
955            mavItem.afz = mavlink_msg_position_target_global_int_get_afz(&message);
956            mavItem.yaw = mavlink_msg_position_target_global_int_get_yaw(&message);
957            mavItem.yaw_rate = mavlink_msg_position_target_global_int_get_yaw_rate(&message);
958            missionCommands.push(mavItem);
959            break;
960        }
961
962        case MAVLINK_MSG_ID_HIGHRES_IMU:
963        {
964            //printf("[RECV] MAVLINK_MSG_ID_HIGHRES_IMU\n");
965            mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
966            current_messages.time_stamps.highres_imu = get_time_usec();
967            this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
968            break;
969        }
970
971        case MAVLINK_MSG_ID_ATTITUDE:
972        {
973            //printf("MAVLINK_MSG_ID_ATTITUDE\n");
974            mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
975            current_messages.time_stamps.attitude = get_time_usec();
976            this_timestamps.attitude = current_messages.time_stamps.attitude;
977            break;
978        }
979
980        case MAVLINK_MSG_ID_COMMAND_LONG:
981        {
982            //printf("[RECV] MAVLINK_MSG_ID_COMMAND_LONG\n");
983            mavlink_msg_command_long_decode(&message, &(current_messages.command_long));
984            current_messages.time_stamps.command_long = get_time_usec();
985            this_timestamps.command_long = current_messages.time_stamps.command_long;
986            // process the received command
987            printf("Received command %d\n", current_messages.command_long.command);
988            switch(current_messages.command_long.command) {
989            case MAV_CMD_SET_MESSAGE_INTERVAL:
990            {
991                int64_t desired_interval = (int64_t)current_messages.command_long.param2;
992                printf("[RECV] COMMAND_LONG: MAV_CMD_SET_MESSAGE_INTERVAL %d %ld\n", (int)current_messages.command_long.param1, (int64_t)current_messages.command_long.param2);
993                // adding to the command queue
994                MavlinkItem mavItem;
995                mavItem.id = mavlink_msg_command_long_get_command(&message);
996                mavItem.callback_message = (uint16_t)mavlink_msg_command_long_get_param1(&message);
997                (desired_interval == -1)?(mavItem.callback_flag = false):(mavItem.callback_flag = true);
998                switch((int)current_messages.command_long.param1) {
999                case MAVLINK_MSG_ID_HEARTBEAT:
1000                {
1001                    if(current_messages.command_long.param2 == -1) {
1002                        stopHeartbeat();
1003                    } else if(current_messages.command_long.param2 == 0) {
1004                        startHeartbeat(HEARTBEAT_DEFAULT_PERIOD);
1005                    } else {
1006                        startHeartbeat(desired_interval);
1007                    }
1008                    break;
1009                }
1010                case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
1011                {
1012                    if(current_messages.command_long.param2 == -1) {
1013                        sendingPosition = false;
1014                    } else if(current_messages.command_long.param2 == 0) {
1015                        sendingPositionInterval = POSITION_DEFAULT_PERIOD;
1016                        sendingPosition = true;
1017                    } else {
1018                        sendingPositionInterval = desired_interval;
1019                        sendingPosition = true;
1020                    }
1021                    mavItem.callback_period = sendingPositionInterval;
1022                    break;
1023                }
1024                case MAVLINK_MSG_ID_ATTITUDE:
1025                {
1026                    if(current_messages.command_long.param2 == -1) {
1027                        sendingAttitude = false;
1028                    } else if(current_messages.command_long.param2 == 0) {
1029                        sendingAttitudeInterval = ATTITUDE_DEFAULT_PERIOD;
1030                        sendingAttitude = true;
1031                    } else {
1032                        sendingAttitudeInterval = desired_interval;
1033                        sendingAttitude = true;
1034                    }
1035                    mavItem.callback_period = sendingAttitudeInterval;
1036                    break;
1037                }
1038                case MAVLINK_MSG_ID_SYS_STATUS:
1039                {
1040                    if(current_messages.command_long.param2 == -1) {
1041                        sendingSystemStatus = false;
1042                    } else if(current_messages.command_long.param2 == 0) {
1043                        sendingSystemStatusInterval = STATUS_DEFAULT_PERIOD;
1044                        sendingSystemStatus = true;
1045                    } else {
1046                        sendingSystemStatusInterval = desired_interval;
1047                        sendingSystemStatus = true;
1048                    }
1049                    mavItem.callback_period = sendingSystemStatusInterval;
1050                    break;
1051                }
1052                default:
1053                    break;
1054                }
1055                // finally add the command to the queue
1056                missionCommands.push(mavItem);
1057                break;
1058            }
1059                // * Commands queue:
1060                // * 176  MAV_CMD_DO_SET_MODE             (mode MAV_MODE)
1061                // * 179  MAV_CMD_DO_SET_HOME             (use current, lat, long, altitude)
1062                // * 193  MAV_CMD_DO_PAUSE_CONTINUE       (holdContinue: 0=hold, 1=continue)
1063                // * 300  MAV_CMD_MISSION_START           (first item, last item)
1064                // * 410  MAV_CMD_GET_HOME_POS            (empty)
1065                // * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
1066
1067            case MAV_CMD_MISSION_START:
1068            {
1069                MavlinkItem mavItem;
1070                mavItem.id = mavlink_msg_command_long_get_command(&message);
1071                mavItem.first_item = (uint16_t)mavlink_msg_command_long_get_param1(&message);
1072                mavItem.last_item = (uint16_t)mavlink_msg_command_long_get_param2(&message);
1073                missionCommands.push(mavItem);
1074                printf("[RECV] COMMAND_LONG: MAV_CMD_MISSION_START firstItem = %d, lastItem = %d\n", mavItem.first_item, mavItem.last_item);
1075                missionStarted();
1076                break;
1077            }
1078            case MAV_CMD_DO_SET_HOME:
1079            {
1080                MavlinkItem mavItem;
1081                mavItem.id = mavlink_msg_command_long_get_command(&message);
1082                mavItem.use_current = (uint8_t)mavlink_msg_command_long_get_param1(&message);
1083                mavItem.latitude = mavlink_msg_command_long_get_param5(&message);
1084                mavItem.longitude = mavlink_msg_command_long_get_param6(&message);
1085                mavItem.altitude = mavlink_msg_command_long_get_param7(&message);
1086                missionCommands.push(mavItem);
1087                printf("[RECV] COMMAND_LONG: MAV_CMD_DO_SET_HOME use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", mavItem.use_current, mavItem.latitude, mavItem.longitude, mavItem.altitude);
1088                break;
1089            }
1090            case MAV_CMD_GET_HOME_POSITION:
1091            {
1092                MavlinkItem mavItem;
1093                mavItem.id = mavlink_msg_command_long_get_command(&message);
1094                missionCommands.push(mavItem);
1095                // TODO respond with a home position
1096                printf("[RECV] COMMAND_LONG: MAV_CMD_GET_HOME_POSITION\n");
1097                break;
1098            }
1099            case MAV_CMD_DO_SET_MODE:
1100            {
1101                MavlinkItem mavItem;
1102                mavItem.id = mavlink_msg_command_long_get_command(&message);
1103                mavItem.mode = mavlink_msg_command_long_get_param1(&message);
1104                missionCommands.push(mavItem);
1105                printf("[RECV] COMMAND_LONG: MAV_CMD_DO_SET_MODE %d\n", mavItem.mode);
1106                break;
1107            }
1108            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
1109            {
1110                MavlinkItem mavItem;
1111                mavItem.id = mavlink_msg_command_long_get_command(&message);
1112                missionCommands.push(mavItem);
1113                printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
1114                break;
1115            }
1116            case MAV_CMD_NAV_TAKEOFF:
1117            {
1118                MavlinkItem mavItem;
1119                mavItem.id = mavlink_msg_command_long_get_command(&message);
1120                mavItem.min_pitch = mavlink_msg_command_long_get_param1(&message);
1121                mavItem.yaw_angle = mavlink_msg_command_long_get_param4(&message);
1122                mavItem.latitude = mavlink_msg_command_long_get_param5(&message);
1123                mavItem.longitude = mavlink_msg_command_long_get_param6(&message);
1124                mavItem.altitude = mavlink_msg_command_long_get_param7(&message);
1125                // adding commands to the command queue
1126                missionCommands.push(mavItem);
1127                printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_TAKEOFF\n");
1128                break;
1129            }
1130            case MAV_CMD_NAV_LAND:
1131            {
1132                MavlinkItem mavItem;
1133                mavItem.id = mavlink_msg_command_long_get_command(&message);
1134                mavItem.abort_altitude = mavlink_msg_command_long_get_param1(&message);
1135                mavItem.desired_yaw = mavlink_msg_command_long_get_param4(&message);
1136                mavItem.latitude = mavlink_msg_command_long_get_param5(&message);
1137                mavItem.longitude = mavlink_msg_command_long_get_param6(&message);
1138                mavItem.altitude = mavlink_msg_command_long_get_param7(&message);
1139                // adding commands to the command queue
1140                missionCommands.push(mavItem);
1141                printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_LAND\n");
1142                break;
1143            }
1144            case MAV_CMD_DO_PAUSE_CONTINUE:
1145            {
1146                MavlinkItem mavItem;
1147                mavItem.id = mavlink_msg_command_long_get_command(&message);
1148                mavItem.pause_continue = (uint16_t)mavlink_msg_command_long_get_param1(&message);
1149                missionCommands.push(mavItem);
1150                printf("[RECV] COMMAND_LONG: MAV_CMD_DO_PAUSE_CONTINUE pauseContinue = %d\n", mavItem.pause_continue);
1151            }
1152            case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
1153            {
1154                // shutdown signal received
1155                shutdown_flag = true;
1156                printf("[RECV] COMMAND_LONG: MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN\n");
1157                break;
1158            }
1159            case MAV_CMD_DO_REPOSITION:
1160            {
1161                // TODO reposition
1162                printf("[TODO] DO_REPOSITION\n");
1163                break;
1164            }
1165            case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
1166            {
1167                // TODO request autopilot capabilities
1168                break;
1169            }
1170            default:
1171                break;
1172            }
1173            // respond with ACK immediately
1174            sendCommandAck(current_messages.command_long.command, MAV_CMD_ACK_OK);
1175            break;
1176        }
1177        default:
1178        {
1179            printf("Warning, did not handle message id %i\n",message.msgid);
1180            break;
1181        }
1182        }
1183    } else {
1184        printf("ERROR: CRC check failed!\n");
1185    }
1186}
1187
1188uint64_t MavlinkUDP::get_time_usec() {
1189    struct timeval tv;
1190    gettimeofday(&tv, NULL);
1191    return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
1192}
1193
1194void MavlinkUDP::debug(string debug_msg) {
1195    if(debug_messages) cout << "[DEBUG] " << debug_msg << endl;
1196}
1197
1198
1199int MavlinkUDP::waitCommandAck(uint64_t timeout) {
1200    uint64_t start_time = get_time_usec();
1201    uint64_t end_time = start_time + timeout;
1202    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
1203    while(get_time_usec() < end_time)
1204    {
1205        if(getCommandAck()){
1206            //printf("Command ACK received!\n");
1207            resetCommandAck();
1208            return 0;
1209        }
1210        usleep(1000);
1211    }
1212    printf("[ERROR] Command ACK timeout! %d\n", getCommandAck());
1213    return -1;
1214}
1215
1216int MavlinkUDP::waitMissionAck(uint64_t timeout) {
1217    uint64_t start_time = get_time_usec();
1218    uint64_t end_time = start_time + timeout;
1219    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
1220    while(get_time_usec() < end_time)
1221    {
1222        if(getMissionAck()){
1223            //printf("Mission ACK received!\n");
1224            resetMissionAck();
1225            return 0;
1226        }
1227        usleep(1000);
1228    }
1229    printf("[ERROR] Mission ACK timeout!\n");
1230    return -1;
1231}
1232
1233int MavlinkUDP::waitMissionCount(uint64_t timeout) {
1234    uint64_t start_time = get_time_usec();
1235    uint64_t end_time = start_time + timeout;
1236    int mission_count;
1237    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
1238    while(get_time_usec() < end_time)
1239    {
1240        // assignment in if!
1241        if((mission_count = getMissionCount())){
1242            //printf("Mission ACK received!\n");
1243            resetMissionCount();
1244            return mission_count;
1245        }
1246        usleep(1000);
1247    }
1248    printf("[ERROR] Mission ACK timeout!\n");
1249    return -1;
1250}
1251
1252MavlinkComponent::MavlinkComponent(std::string addr) : addrIP(addr) {
1253    // craete the system ID out of the IP address
1254    int byte1, byte2, byte3, byte4;
1255    char dot = '.';
1256    std::istringstream s(addrIP);
1257    s >> byte1 >> dot >> byte2 >> dot >> byte3 >> dot >> byte4;
1258    sysid = byte4;
1259    compid = 0;
1260    printf("Mavlink component created:\tSystem ID %d\tComponent ID %d\n", sysid, compid);
1261}
Note: See TracBrowser for help on using the repository browser.