source: flair-src/tags/latest/tools/Controller/Mavlink/src/MavlinkUDP.cpp

Last change on this file was 88, checked in by Sanahuja Guillaume, 8 years ago

m
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.