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

Last change on this file since 80 was 78, checked in by Thomas Fuhrmann, 8 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.