source: flair-src/trunk/tools/Controller/Mavlink/src/MavlinkUDP.h @ 88

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

m

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