source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h @ 82

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

Add the mavlink udp com class

File size: 16.5 KB
RevLine 
[78]1//  created:    20/04/2016
2//  updated:    20/09/2016
3//  filename:   MavlinkUDP.h
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#ifndef MAVLINKUDP_H
22#define MAVLINKUDP_H
23
24#include <thread>
25#include <iostream>
26#include <queue>
27#include <arpa/inet.h>
28#include <cstring>
29#include <unistd.h>
30#include <sys/time.h>
31#include "include/common/mavlink.h"
32
33// TODO these constants should be read from a file
34#define LOCALHOST "127.0.0.1"
35#define MAVLINK_UDP_PORT 5555
36#define GCS_ADDR "192.168.6.150"
37#define UAV2 "192.168.6.2"
38#define UAV1 "192.168.6.1"
39#define PC_LATITUDE "192.168.6.100"
40
41#define MAX_NEIGH 50
42
43#define THREAD_MS_IN 10
44#define THREAD_MS_OUT 10
45
46#define BUFF_IN_LEN 80
47#define BUFF_OUT_LEN 80
48
49#define ACK_TIMEOUT 100000 // us
50
51#ifndef MAV_CUSTOM_PARAM
52#define MAV_CUSTOM_PARAM
53
54enum TMAVCustomParams {
55    MAV_LOOP_MISSION_ITEMS=42,
56    MAV_CUSTOM_PARAM1=43,
57    MAV_CUSTOM_PARAM2=44
58};
59
60#endif
61
62#ifndef REBOOT_SHUTDOWN_PARAMS
63#define REBOOT_SHUTDOWN_PARAMS
64
65enum TRebootShutdownParam {
66    NO_EFFECT=0,
67    REBOOT=1,
68    SHUTDOWN=2,
69    REBOOT_KEEP_IN_BOOTLOADER=3
70};
71
72#endif
73
74struct Time_Stamps
75{
76    Time_Stamps() {
77        reset_timestamps();
78    }
79    uint64_t mission_ack;
80    uint64_t command_ack;
81    uint64_t command_long;
82    uint64_t timesync;
83    uint64_t mission_count;
84    uint64_t mission_request;
85    uint64_t mission_reached;
86    uint64_t mission_item;
87    uint64_t mission_request_list;
88    uint64_t mission_write_partial_list;
89    uint64_t heartbeat;
90    uint64_t home_position;
91    uint64_t sys_status;
92    uint64_t battery_status;
93    uint64_t radio_status;
94    uint64_t local_position_ned;
95    uint64_t global_position_int;
96    uint64_t position_target_local_ned;
97    uint64_t position_target_global_int;
98    uint64_t highres_imu;
99    uint64_t attitude;
100    uint64_t system_time;
101    uint64_t clear_all;
102
103    void reset_timestamps() {
104        mission_request=0;
105        mission_request_list=0;
106        mission_write_partial_list=0;
107        mission_reached=0;
108        command_long=0;
109        timesync=0;
110        mission_count=0;
111        mission_item=0;
112        mission_ack=0;
113        home_position =0;
114        heartbeat = 0;
115        sys_status = 0;
116        battery_status = 0;
117        radio_status = 0;
118        local_position_ned = 0;
119        global_position_int = 0;
120        position_target_local_ned = 0;
121        position_target_global_int = 0;
122        highres_imu = 0;
123        attitude = 0;
124        system_time = 0;
125        clear_all = 0;
126    }
127};
128
129// Struct containing information on the MAV we are currently connected to
130struct Mavlink_Messages {
131    int sysid;
132    int compid;
133    // system time
134    mavlink_system_time_t system_time;
135    // mission_request
136    mavlink_mission_request_t mission_request;
137    // mission request list
138    mavlink_mission_request_list_t mission_request_list;
139    // mission write partial list
140    mavlink_mission_write_partial_list_t mission_write_partial_list;
141    // mission_reached
142    mavlink_mission_item_reached_t mission_reached;
143    // mission_clear_all
144    mavlink_mission_clear_all_t clear_all;
145    // command long
146    mavlink_command_long_t command_long;
147    // command_ack
148    mavlink_command_ack_t command_ack;
149    // timesync
150    mavlink_timesync_t timesync;
151    // mission_count
152    mavlink_mission_count_t mission_count;
153    // mission_ack
154    mavlink_mission_ack_t mission_ack;
155    // mission_item
156    mavlink_mission_item_t mission_item;
157    // Homeposition
158    mavlink_home_position_t home_position;
159    // Heartbeat
160    mavlink_heartbeat_t heartbeat;
161    // System Status
162    mavlink_sys_status_t sys_status;
163    // Battery Status
164    mavlink_battery_status_t battery_status;
165    // Radio Status
166    mavlink_radio_status_t radio_status;
167    // Local Position
168    mavlink_local_position_ned_t local_position_ned;
169    // Global Position
170    mavlink_global_position_int_t global_position_int;
171    // Local Position Target
172    mavlink_position_target_local_ned_t position_target_local_ned;
173    // Global Position Target
174    mavlink_position_target_global_int_t position_target_global_int;
175    // HiRes IMU
176    mavlink_highres_imu_t highres_imu;
177    // Attitude
178    mavlink_attitude_t attitude;
179    // System Parameters?
180    // To add
181    // Time Stamps
182    Time_Stamps time_stamps;
183    void reset_timestamps() {
184        time_stamps.reset_timestamps();
185    }
186};
187
188// * Mission items queue:
189// * 16   MAV_CMD_NAV_WAYPOINT            (hold time, acceptance radius, yaw angle, lat, long, altitude)
190// * 21   MAV_CMD_NAV_LAND                (abort alt, yaw angle, lat, long, altitude)
191// * 22   MAV_CMD_NAV_TAKEOFF             (yaw angle, lat, long, altitude)
192// * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
193// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
194
195struct MissionItem {
196    uint16_t itemID;
197    uint8_t type;
198    uint64_t holdTime;
199    float radius;
200    float yawAngle;
201    float latitude;
202    float longitude;
203    float altitude;
204    float x, y, z;
205    float abortAltitude;
206    uint16_t sequence;
207    uint16_t repeatCount;
208    MissionItem() {
209        // struct construtor resets all the values
210        itemID = sequence = repeatCount = 0;
211        holdTime = 0;
212        radius = yawAngle = latitude = longitude = altitude = abortAltitude = x = y = z = 0;
213    }
214};
215
216// * Commands queue:
217// * 176  MAV_CMD_DO_SET_MODE             (mode MAV_MODE)
218// * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
219// * 179  MAV_CMD_DO_SET_HOME             (use current, lat, long, altitude)
220// * 193  MAV_CMD_DO_PAUSE_CONTINUE       (holdContinue: 0=hold, 1=continue)
221// * 300  MAV_CMD_MISSION_START           (first item, last item)
222// * 410  MAV_CMD_GET_HOME_POS            (empty)
223// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
224
225struct MissionCommand {
226    uint16_t commandId;
227    // DO_JUMP
228    uint16_t sequence;
229    uint16_t repeatCount;
230    // DO_SET_HOME
231    uint8_t useCurrent;
232    float latitude, longitude, altitude;
233    float x, y, z;
234    // DO_PAUSE_CONTINUE
235    uint8_t pauseContinue;
236    // MISSION_START
237    uint16_t firstItem, lastItem;
238    // DO_SET_MODE
239    uint8_t mode;
240    MissionCommand() {
241        // reset all the attributes
242        pauseContinue = useCurrent = 0;
243        commandId = sequence = repeatCount = 0;
244        latitude = longitude = altitude = x = y = z = 0;
245    }
246};
247
248class SocketRuntimeError {
249private:
250    std::string info;
251public:
252    SocketRuntimeError(const std::string& str) : info(str) {}
253    std::string getInfo() const { return info; }
254};
255
256class MavlinkRuntimeError {
257private:
258    std::string info;
259public:
260    MavlinkRuntimeError(const std::string& str) : info(str) {}
261    std::string getInfo() const { return info; }
262};
263
264class MavlinkComponent
265{
266public:
267    MavlinkComponent(std::string addr);
268    //MavComAgent();
269    ~MavlinkComponent() {}
270    uint8_t getSysID() const { return sysid; }
271    uint8_t getCompID() const { return compid; }
272    std::string getAddrIP() { return addrIP; }
273private:
274    uint8_t sysid; // system ID
275    uint8_t compid; // component ID
276    std::string addrIP; // IP address
277};
278
279class MavlinkUDP {
280public:
281    MavlinkUDP(const std::string& addr, int port);
282    ~MavlinkUDP();
283
284    // begin SockUDP
285    // threads start and stop
286    void startThreads();
287    void stopThreads();
288    void handleQuit( int sig );
289    int getSocket() const { return m_socket_in; }
290    int getPort() const { return m_port; }
291    std::string getAddr() const { return m_addr; }
292    bool something_to_send;
293    uint16_t length_to_send;
294    u_int8_t buff_out[BUFF_OUT_LEN];
295    u_int8_t buff_in[BUFF_IN_LEN];
296    ssize_t recsize;
297    socklen_t fromlen;
298    void clearData();
299    // end SockUDP
300
301
302    char control_status;
303    char arming_status;
304    uint64_t write_count; // number of messages sent
305    uint64_t Xtimec;
306    uint64_t Xtimes;
307
308    int system_id;
309    int autopilot_id;
310    int component_id;
311    bool home_position_set;
312    bool ack;
313    bool Xtimesync;
314    int state;
315    int typeack;
316    bool request;
317    bool waypoint;
318    int frame;
319    int command;
320    int seq;
321    int seqr;
322    int seqold;
323    int count;
324    int Waycount;
325    int Wayseq;
326    int compt;
327    uint16_t message_interval;
328    bool debug_messages;
329
330    bool sendingPosition;
331    uint16_t sendingPositionInterval;
332    bool sendingAttitude;
333    uint16_t sendingAttitudeInterval;
334    bool sendingSystemStatus;
335    uint16_t sendingSystemStatusInterval;
336    bool sendingHeartbeat;
337    uint16_t sendingHeartbeatInterval;
338
339    // queue of mission items (plan de vol)
340    std::queue<MissionItem> missionPlan;
341    //void getMissionPlan(std::vector<Waypoint>& x) { x =  missionPlan; }
342    const std::queue<MissionItem>& getMissionPlan() const { return missionPlan; }
343    void clearMissionPlan() { std::queue<MissionItem> empty; std::swap(missionPlan, empty); }
344    uint16_t currentMissionItem;
345    uint16_t missionFirst;
346    uint16_t missionLast;
347
348    // queue of received navigation commands
349    std::queue<MissionCommand> missionCommands;
350    const std::queue<MissionCommand>& getMissionCommands() const { return missionCommands; }
351    void clearMissionCommands() { std::queue<MissionCommand> empty; std::swap(missionCommands, empty); }
352
353    bool mission_items_loop;
354    void itemsLoop() { mission_items_loop = true; }
355    void itemsNoLoop() { mission_items_loop = false; }
356    bool getItemsLoop() const { return mission_items_loop; }
357
358    bool missionActive;
359    bool getMissionStatus() const { return missionActive; }
360    void missionStarted() { missionActive = true; }
361    void missionStopped() { missionActive = false; }
362
363    mavlink_command_long_t com;
364    Mavlink_Messages current_messages;
365    mavlink_set_position_target_local_ned_t initial_position;
366    Time_Stamps last_timestamps;
367
368    MavlinkComponent me, target; // destination system
369
370    bool check_mavlink_crc(u_int8_t *buff_in, ssize_t recsize, u_int8_t msgid);
371    void decode_message(u_int8_t *buff_in, ssize_t size);
372    void print_message(u_int8_t *buff_in, ssize_t recsize);
373
374    // messages
375    void sendHeartbeat();
376    void 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); // callback
377    void 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); // callback
378    void sendSystemTime();
379    void sendLocalPositionNED(float x, float y, float z, float vx, float vy, float vz); // callback
380    void sendAttitude(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); // callback
381    void updateSetpoint(mavlink_set_position_target_local_ned_t setpoint);
382    void sendMissionAck(uint8_t targetSystem, uint8_t targetComponent, uint8_t type);
383    void sendCommandAck(uint16_t command, uint8_t result);
384    void 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);
385    void sendMissionCount(uint8_t targetSystem, uint8_t targetComponent, uint16_t count);
386    void 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);
387    void sendMissionWritePartialList(uint8_t targetSystem, uint8_t targetComponent, uint16_t startIndex, uint16_t endIndex);
388    void 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);
389    void sendMissionRequestList(uint8_t targetSystem, uint8_t targetComponent);
390    void sendMissionItemReached(uint16_t seq);
391    void sendMissionSetCurrent(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq);
392    void sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent);
393    void sendSetPositionTargetLocalNED(uint32_t timeBootMs, 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);
394    void 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);
395
396    // mavlink commands
397    void cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec);
398    void cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);
399    void cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location
400    void cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude); // mission command to perform landing
401    void cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude);
402    void cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode);
403    void cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent);
404    void cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem);
405    void cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue);
406    void cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent);
407    void cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent);
408    //void cmdVideoStartCapture(uint8_t targetSystem, uint8_t targetComponent);
409    //void cmdVideoStopCapture(uint8_t targetSystem, uint8_t targetComponent);
410    void cmdRebootShutdown(uint8_t targetSystem, uint8_t targetComponent, uint8_t autopilot, uint8_t onboardComputer);
411
412    bool getMissionAck() const { return mission_ack; }
413    bool getCommandAck() const { return command_ack; }
414    int getMissionCount() const { return mission_count; }
415    void setMissionAck() { mission_ack = true; }
416    void setCommandAck() { command_ack = true; }
417    void setMissionCount(int count) { mission_count = count; }
418    void resetMissionAck() { mission_ack = false; }
419    void resetCommandAck() { command_ack = false; }
420    void resetMissionCount() { mission_count = 0; }
421
422    bool ShutdownReceived() { return shutdown_flag; }
423
424    uint64_t get_time_usec();
425
426    int waitCommandAck(uint64_t timeout);
427    int waitMissionAck(uint64_t timeout);
428    int waitMissionCount(uint64_t timeout);
429
430private:
431
432    // begin SockUDP
433    int m_socket_in;
434    int m_socket_out;
435    uint16_t m_port;
436    std::string m_addr;
437    struct addrinfo* m_addrinfo;
438    struct sockaddr_in myaddr_in, myaddr_out, client_in;
439    std::thread send_th;
440    std::thread recv_th;
441    void recv_thread(); // receiver thread
442    void send_thread();// sender thread
443    //int timed_recv(char *msg, size_t max_size, int max_wait_ms);
444    bool stop_recv;
445    bool stop_send;
446    //std::queue<std::string> queue_out;
447    //Queue<uint8_t> queue_out;
448    // end SockUDP
449
450    inline void clearBuffer(uint8_t *buffer, int len);
451
452    mavlink_set_position_target_local_ned_t current_setpoint;
453
454    int bytes_sent;
455    // for local_position_ned
456    float position[6] = {};
457
458    // FIXME doesn't work with mavlink 2.0
459    mavlink_message_t msg;
460    u_int16_t msg_len;
461    u_int8_t hds_mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
462
463    bool command_ack;
464    bool mission_ack;
465    int mission_count;
466
467    bool shutdown_flag;
468
469    // TODO change to mission item class
470    void debug(std::string debug_msg);
471};
472
473#endif // MAVLINKUDP_H
Note: See TracBrowser for help on using the repository browser.