| 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 |
|
---|
| 25 | using namespace std;
|
---|
| 26 |
|
---|
| 27 | MavlinkUDP::MavlinkUDP(const std::string& addr, int port) : m_port(port), m_addr(addr), me(addr), target(GCS_ADDR) {
|
---|
| 28 | // initialize attributes
|
---|
| 29 | write_count = 0;
|
---|
| 30 | home_position_set=false;
|
---|
| 31 | ack=false;
|
---|
| 32 | request=false;
|
---|
| 33 | seq=-1;
|
---|
| 34 | seqr=-1;
|
---|
| 35 | seqold=-1;
|
---|
| 36 | waypoint=false;
|
---|
| 37 | Waycount=0;
|
---|
| 38 | Wayseq=-1;
|
---|
| 39 | message_interval = 1000; // ms
|
---|
| 40 | state=-1;
|
---|
| 41 | control_status = 0; // whether the autopilot is in offboard control mode
|
---|
| 42 | arming_status = 0; // whether the autopilot is in arming control mode
|
---|
| 43 | debug_messages = true;
|
---|
| 44 |
|
---|
| 45 | resetCommandAck();
|
---|
| 46 | resetMissionAck();
|
---|
| 47 |
|
---|
| 48 | // period function flags
|
---|
| 49 | sendingAttitude = false;
|
---|
| 50 | sendingPosition = false;
|
---|
| 51 | sendingSystemStatus = false;
|
---|
| 52 | sendingAttitudeInterval = -1;
|
---|
| 53 | sendingPositionInterval = -1;
|
---|
| 54 | sendingSystemStatusInterval = -1;
|
---|
| 55 |
|
---|
| 56 | // mission items
|
---|
| 57 | missionFirst = 0;
|
---|
| 58 | missionLast = 0;
|
---|
| 59 | mission_items_loop = false;
|
---|
| 60 | missionActive = false; // if the mission is launched
|
---|
| 61 |
|
---|
| 62 | shutdown_flag = false;
|
---|
| 63 |
|
---|
| 64 | // TODO read the system ID from the file based on the IP address
|
---|
| 65 | system_id = 0; // system id
|
---|
| 66 | autopilot_id = 0; // autopilot component id
|
---|
| 67 | component_id = 0; // companion computer component id
|
---|
| 68 |
|
---|
| 69 | // mavlink_command_long_t com = { 0 };
|
---|
| 70 | compt=0;
|
---|
| 71 | current_messages.sysid = system_id;
|
---|
| 72 | current_messages.compid = autopilot_id;
|
---|
| 73 | Xtimes=0;
|
---|
| 74 | Xtimesync=false;
|
---|
| 75 |
|
---|
| 76 | something_to_send = false; // check if something needs to be sent
|
---|
| 77 | stop_recv = false;
|
---|
| 78 | stop_send = false;
|
---|
| 79 | startThreads(); // start both sending and receiving thread
|
---|
| 80 | }
|
---|
| 81 |
|
---|
| 82 | MavlinkUDP::~MavlinkUDP() {
|
---|
| 83 | try {
|
---|
| 84 | stopThreads();
|
---|
| 85 | } catch (int error) {
|
---|
| 86 | fprintf(stderr,"MavlinkUDP: Warning! Could not stop threads!\n");
|
---|
| 87 | }
|
---|
| 88 | }
|
---|
| 89 |
|
---|
| 90 | // starting send and receive threads
|
---|
| 91 | void MavlinkUDP::startThreads() {
|
---|
| 92 | // start receive thread
|
---|
| 93 | recv_th = std::thread(&MavlinkUDP::recv_thread, this);
|
---|
| 94 | printf("MavlinkUDP: Receiver thread created.\n");
|
---|
| 95 | // start sending thread
|
---|
| 96 | send_th = std::thread(&MavlinkUDP::send_thread, this);
|
---|
| 97 | printf("MavlinkUDP: Sender thread created.\n");
|
---|
| 98 | }
|
---|
| 99 |
|
---|
| 100 | void MavlinkUDP::stopThreads() {
|
---|
| 101 | stop_recv = true;
|
---|
| 102 | stop_send = true;
|
---|
| 103 | usleep(100);
|
---|
| 104 | if(recv_th.joinable()) recv_th.join();
|
---|
| 105 | if(send_th.joinable()) send_th.join();
|
---|
| 106 | printf("MavlinkUDP: Threads stopped.\n");
|
---|
| 107 | // close the sockets
|
---|
| 108 | close(m_socket_in);
|
---|
| 109 | close(m_socket_out);
|
---|
| 110 | printf("MavlinkUDP: Sockets closed.\n");
|
---|
| 111 | }
|
---|
| 112 |
|
---|
| 113 | // quit handler
|
---|
| 114 | void MavlinkUDP::handleQuit(int sig) {
|
---|
| 115 | try {
|
---|
| 116 | stopThreads();
|
---|
| 117 | } catch (int error) {
|
---|
| 118 | fprintf(stderr,"MavlinkUDP: Warning, could not stop communication block!\n");
|
---|
| 119 | }
|
---|
| 120 | }
|
---|
| 121 |
|
---|
| 122 | inline void MavlinkUDP::clearBuffer(uint8_t *buffer, int len) {
|
---|
| 123 | memset(buffer, 0, len);
|
---|
| 124 | }
|
---|
| 125 |
|
---|
| 126 | void MavlinkUDP::clearData() {
|
---|
| 127 | clearBuffer(buff_in, BUFF_IN_LEN);
|
---|
| 128 | }
|
---|
| 129 |
|
---|
| 130 | void MavlinkUDP::recv_thread() {
|
---|
| 131 | // UDP socket setup
|
---|
| 132 | socklen_t clilen=sizeof(client_in);
|
---|
| 133 | if((m_socket_in=socket(AF_INET,SOCK_DGRAM,0))<0)
|
---|
| 134 | {
|
---|
| 135 | throw SocketRuntimeError("[ERRR] RECV socket error!\n");
|
---|
| 136 | }
|
---|
| 137 | memset((char *)&myaddr_in, 0, sizeof(myaddr_in));
|
---|
| 138 | myaddr_in.sin_family=AF_INET;
|
---|
| 139 | // receiving messages from all addresses
|
---|
| 140 | myaddr_in.sin_addr.s_addr=htonl(INADDR_ANY);
|
---|
| 141 | myaddr_in.sin_port=htons(m_port);
|
---|
| 142 | if(bind(m_socket_in,(struct sockaddr *)&myaddr_in,sizeof(myaddr_in))<0)
|
---|
| 143 | {
|
---|
| 144 | throw SocketRuntimeError("[ERRR] UDP in bind error!\n");
|
---|
| 145 | }
|
---|
| 146 | //std::cout << "MavlinkUDP: RECV socket bind OK.\n";
|
---|
| 147 | std::cout << "MavlinkUDP: RECV thread launched.\n";
|
---|
| 148 | clearBuffer(buff_in, BUFF_IN_LEN);
|
---|
| 149 | // receive data
|
---|
| 150 | while(!stop_recv) {
|
---|
| 151 | recsize = recvfrom(m_socket_in, (void *)buff_in, BUFF_IN_LEN, 0, (struct sockaddr *)&client_in, &clilen);
|
---|
| 152 | if(recsize > 0) {
|
---|
| 153 | // parse the received data
|
---|
| 154 | //print_message(sockUDP.buff_in, sockUDP.recsize);
|
---|
| 155 | decode_message(buff_in, recsize);
|
---|
| 156 | clearBuffer(buff_in, BUFF_IN_LEN);
|
---|
| 157 | }
|
---|
| 158 | //usleep(10000);
|
---|
| 159 | std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_IN));
|
---|
| 160 | }
|
---|
| 161 | }
|
---|
| 162 |
|
---|
| 163 | void MavlinkUDP::send_thread() {
|
---|
| 164 | // socket setup
|
---|
| 165 | int broadcast = 1;
|
---|
| 166 | // socket for broadcast
|
---|
| 167 | if((m_socket_out=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0) {
|
---|
| 168 | throw SocketRuntimeError("[ERRR] udp out socket error\n");
|
---|
| 169 | }
|
---|
| 170 | if((setsockopt(m_socket_out,SOL_SOCKET,SO_BROADCAST,&broadcast,sizeof broadcast)) == -1) {
|
---|
| 171 | throw SocketRuntimeError("[ERRR] udp out setsockopt error\n");
|
---|
| 172 | }
|
---|
| 173 | cout << "MavlinkUDP: SEND thread launched.\n";
|
---|
| 174 | memset((char *) &myaddr_out, 0, sizeof(myaddr_out));
|
---|
| 175 | myaddr_out.sin_family=AF_INET;
|
---|
| 176 | myaddr_out.sin_port=htons(m_port);
|
---|
| 177 | myaddr_out.sin_addr.s_addr=inet_addr(m_addr.c_str());
|
---|
| 178 | clearBuffer(buff_out, BUFF_OUT_LEN);
|
---|
| 179 | // send data to the socket
|
---|
| 180 | while(!stop_send) {
|
---|
| 181 | if(something_to_send) {
|
---|
| 182 | // send quickly and reset the flag
|
---|
| 183 | sendto(m_socket_out, buff_out, length_to_send, 0, (struct sockaddr *) &myaddr_out, sizeof(myaddr_out));
|
---|
| 184 | clearBuffer(buff_out,BUFF_OUT_LEN);
|
---|
| 185 | something_to_send = false;
|
---|
| 186 | }
|
---|
| 187 | //clearBuffer(buff_out, BUFF_OUT_LEN);
|
---|
| 188 | std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_OUT));
|
---|
| 189 | //usleep(10000);
|
---|
| 190 | }
|
---|
| 191 | }
|
---|
| 192 |
|
---|
| 193 | // read mission configuration file
|
---|
| 194 | // TODO use the Qt XML library
|
---|
| 195 | //
|
---|
| 196 | //
|
---|
| 197 |
|
---|
| 198 | void MavlinkUDP::updateSetpoint(mavlink_set_position_target_local_ned_t setpoint) {
|
---|
| 199 | current_setpoint = setpoint;
|
---|
| 200 | }
|
---|
| 201 |
|
---|
| 202 | //
|
---|
| 203 | // MESSAGES
|
---|
| 204 | //
|
---|
| 205 |
|
---|
| 206 | // send Heartbeat message
|
---|
| 207 | void MavlinkUDP::sendHeartbeat() {
|
---|
| 208 | mavlink_message_t message;
|
---|
| 209 | mavlink_msg_heartbeat_pack(me.getSysID(), me.getCompID(), &message, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_AUTO_ARMED, 0, MAV_STATE_ACTIVE);
|
---|
| 210 | length_to_send = mavlink_msg_to_send_buffer(buff_out, &message);
|
---|
| 211 | something_to_send = true;
|
---|
| 212 | printf("[SENT] Heartbeat\n");
|
---|
| 213 | }
|
---|
| 214 |
|
---|
| 215 | // send SystemStatus message
|
---|
| 216 | void MavlinkUDP::sendSystemStatus(uint32_t onboardSensorsPresent, uint32_t onboardSensorsEnabled, uint32_t onboardSensorsHealth, uint16_t load, uint16_t voltage, int16_t current, int8_t batteryRemaining, uint16_t dropRateComm, uint16_t errorsComm, uint16_t errors1, uint16_t errors2, uint16_t errors3, uint16_t errors4) {
|
---|
| 217 | mavlink_message_t message;
|
---|
| 218 | mavlink_msg_sys_status_pack(me.getSysID(), me.getCompID(), &message, onboardSensorsPresent, onboardSensorsEnabled, onboardSensorsHealth, load, voltage, current, batteryRemaining, dropRateComm, errorsComm, errors1, errors2, errors3, errors4);
|
---|
| 219 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 220 | something_to_send = true;
|
---|
| 221 | printf("[SENT] System status\n");
|
---|
| 222 | }
|
---|
| 223 |
|
---|
| 224 | // send BatteryStatus message
|
---|
| 225 | void MavlinkUDP::sendBatteryStatus(uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, uint16_t *voltages, int16_t current, int32_t currentConsumed, int32_t energyConsumed, int8_t batteryRemaining) {
|
---|
| 226 | mavlink_message_t message;
|
---|
| 227 | mavlink_msg_battery_status_pack(me.getSysID(), me.getCompID(), &message, id, battery_function, type, temperature, voltages, current, currentConsumed, energyConsumed, batteryRemaining);
|
---|
| 228 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 229 | something_to_send = true;
|
---|
| 230 | }
|
---|
| 231 |
|
---|
| 232 | // send SystemTime message
|
---|
| 233 | void MavlinkUDP::sendSystemTime() {
|
---|
| 234 | mavlink_system_time_t sys_time;
|
---|
| 235 | sys_time.time_unix_usec = get_time_usec();
|
---|
| 236 | mavlink_message_t message;
|
---|
| 237 | mavlink_msg_system_time_pack(me.getSysID(), me.getCompID(), &message, sys_time.time_unix_usec, sys_time.time_boot_ms);
|
---|
| 238 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 239 | something_to_send = true;
|
---|
| 240 | printf("[SENT] System time\n");
|
---|
| 241 | }
|
---|
| 242 |
|
---|
| 243 | // send LocalPostiionNED message
|
---|
| 244 | void MavlinkUDP::sendLocalPositionNED(float x, float y, float z, float vx, float vy, float vz) {
|
---|
| 245 | mavlink_message_t message;
|
---|
| 246 | mavlink_msg_local_position_ned_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), x, y, z, vx, vy, vz);
|
---|
| 247 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 248 | something_to_send = true;
|
---|
| 249 | }
|
---|
| 250 |
|
---|
| 251 | // send Attitude message
|
---|
| 252 | void MavlinkUDP::sendAttitude(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) {
|
---|
| 253 | mavlink_message_t message;
|
---|
| 254 | mavlink_msg_attitude_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
|
---|
| 255 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 256 | something_to_send = true;
|
---|
| 257 | }
|
---|
| 258 |
|
---|
| 259 | // send MissionAck message
|
---|
| 260 | void MavlinkUDP::sendMissionAck(uint8_t targetSystem, uint8_t targetComponent, uint8_t type) {
|
---|
| 261 | mavlink_message_t message;
|
---|
| 262 | mavlink_msg_mission_ack_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, type);
|
---|
| 263 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 264 | something_to_send = true;
|
---|
| 265 | printf("[SENT] Mission ACK\n");
|
---|
| 266 | }
|
---|
| 267 |
|
---|
| 268 | // send commandAck message
|
---|
| 269 | void MavlinkUDP::sendCommandAck(uint16_t command, uint8_t result) {
|
---|
| 270 | mavlink_message_t message;
|
---|
| 271 | mavlink_msg_command_ack_pack(me.getSysID(), me.getCompID(), &message, command, result);
|
---|
| 272 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 273 | something_to_send = true;
|
---|
| 274 | printf("[SENT] Command ACK: %d\n", command);
|
---|
| 275 | }
|
---|
| 276 |
|
---|
| 277 | // send autopilot version
|
---|
| 278 | void MavlinkUDP::sendAutopilotVersion(uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, uint8_t *flight_custom_version, uint8_t *middleware_custom_version, uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) {
|
---|
| 279 | mavlink_message_t message;
|
---|
| 280 | // TODO get these values from autopilot
|
---|
| 281 | mavlink_msg_autopilot_version_pack(me.getSysID(), me.getCompID(), &message, capabilities, flight_sw_version, middleware_sw_version, os_sw_version, board_version, flight_custom_version, middleware_custom_version, os_custom_version, vendor_id, product_id, uid);
|
---|
| 282 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 283 | something_to_send = true;
|
---|
| 284 | printf("[SENT] Autopilot vesrion\n");
|
---|
| 285 | }
|
---|
| 286 |
|
---|
| 287 | // send MissionCount message
|
---|
| 288 | void MavlinkUDP::sendMissionCount(uint8_t targetSystem, uint8_t targetComponent, uint16_t count) {
|
---|
| 289 | mavlink_message_t message;
|
---|
| 290 | mavlink_msg_mission_count_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, count);
|
---|
| 291 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 292 | something_to_send = true;
|
---|
| 293 | }
|
---|
| 294 |
|
---|
| 295 | // send CommandLong message
|
---|
| 296 | void MavlinkUDP::sendCommandLong(uint8_t targetSystem, uint8_t targetComponent, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) {
|
---|
| 297 | mavlink_message_t message;
|
---|
| 298 | // uint8_t confirmation;
|
---|
| 299 | // float param1, param2, param3, param4, param5, param6, param7;
|
---|
| 300 | mavlink_msg_command_long_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, command, confirmation, param1, param2, param3, param4, param5, param6, param7);
|
---|
| 301 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 302 | something_to_send = true;
|
---|
| 303 | printf("[SENT] Command long: %d\n", command);
|
---|
| 304 | }
|
---|
| 305 |
|
---|
| 306 | // send MissionWritePartialList message
|
---|
| 307 | void MavlinkUDP::sendMissionWritePartialList(uint8_t targetSystem, uint8_t targetComponent, uint16_t startIndex, uint16_t endIndex) {
|
---|
| 308 | mavlink_message_t message;
|
---|
| 309 | mavlink_msg_mission_write_partial_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, startIndex, endIndex);
|
---|
| 310 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 311 | something_to_send = true;
|
---|
| 312 | }
|
---|
| 313 |
|
---|
| 314 | // send MissionItem message
|
---|
| 315 | void MavlinkUDP::sendMissionItem(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) {
|
---|
| 316 | mavlink_message_t message;
|
---|
| 317 | mavlink_msg_mission_item_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
|
---|
| 318 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 319 | something_to_send = true;
|
---|
| 320 | }
|
---|
| 321 |
|
---|
| 322 | // send MissionRequestList message
|
---|
| 323 | void MavlinkUDP::sendMissionRequestList(uint8_t targetSystem, uint8_t targetComponent) {
|
---|
| 324 | mavlink_message_t message;
|
---|
| 325 | mavlink_msg_mission_request_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
|
---|
| 326 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 327 | something_to_send = true;
|
---|
| 328 | }
|
---|
| 329 |
|
---|
| 330 | // send MissionItemReached message
|
---|
| 331 | void MavlinkUDP::sendMissionItemReached(uint16_t seq) {
|
---|
| 332 | mavlink_message_t message;
|
---|
| 333 | mavlink_msg_mission_item_reached_pack(me.getSysID(), me.getCompID(), &message, seq);
|
---|
| 334 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 335 | something_to_send = true;
|
---|
| 336 | }
|
---|
| 337 |
|
---|
| 338 | // send MissionSetCurrent message
|
---|
| 339 | void MavlinkUDP::sendMissionSetCurrent(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq) {
|
---|
| 340 | mavlink_message_t message;
|
---|
| 341 | mavlink_msg_mission_set_current_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq);
|
---|
| 342 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 343 | something_to_send = true;
|
---|
| 344 | }
|
---|
| 345 |
|
---|
| 346 | // send MissionClearAll message
|
---|
| 347 | void MavlinkUDP::sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent) {
|
---|
| 348 | mavlink_message_t message;
|
---|
| 349 | mavlink_msg_mission_clear_all_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
|
---|
| 350 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 351 | something_to_send = true;
|
---|
| 352 | printf("[SENT] Mission clear all\n");
|
---|
| 353 | }
|
---|
| 354 |
|
---|
| 355 | // send SetPositionTargetLocalNED message
|
---|
| 356 | void MavlinkUDP::sendSetPositionTargetLocalNED(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) {
|
---|
| 357 | mavlink_message_t message;
|
---|
| 358 | mavlink_msg_set_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
|
---|
| 359 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 360 | something_to_send = true;
|
---|
| 361 | }
|
---|
| 362 |
|
---|
| 363 | // send SetPositionTargetGlobalInt message
|
---|
| 364 | void MavlinkUDP::sendSetPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) {
|
---|
| 365 | mavlink_message_t message;
|
---|
| 366 | mavlink_msg_set_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
|
---|
| 367 | length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
|
---|
| 368 | something_to_send = true;
|
---|
| 369 | }
|
---|
| 370 |
|
---|
| 371 | //
|
---|
| 372 | // COMMANDS
|
---|
| 373 | //
|
---|
| 374 |
|
---|
| 375 | // command Waypoint
|
---|
| 376 | void MavlinkUDP::cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {
|
---|
| 377 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_WAYPOINT, 0, holdTime, proximityRadius, passRadius, desiredYaw, latitude, longitude, altitude);
|
---|
| 378 | }
|
---|
| 379 |
|
---|
| 380 | // command SetMessageInterval
|
---|
| 381 | void MavlinkUDP::cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec) {
|
---|
| 382 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_SET_MESSAGE_INTERVAL, 0, (float)messageID, (float)interval_usec, 0, 0, 0, 0, 0);
|
---|
| 383 | }
|
---|
| 384 |
|
---|
| 385 | // command Land
|
---|
| 386 | void MavlinkUDP::cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
|
---|
| 387 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_LAND, 0, abortAlt, 0, 0, desiredYaw, latitude, longitude, altitude);
|
---|
| 388 | }
|
---|
| 389 |
|
---|
| 390 | // command LandStart
|
---|
| 391 | void MavlinkUDP::cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude) {
|
---|
| 392 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_LAND_START, 0, 0, 0, 0, 0, latitude, longitude, 0);
|
---|
| 393 | }
|
---|
| 394 |
|
---|
| 395 | // command TakeOff
|
---|
| 396 | void MavlinkUDP::cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
|
---|
| 397 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_TAKEOFF, 0, desiredPitch, 0, 0, magnetometerYaw, latitude, longitude, altitude);
|
---|
| 398 | }
|
---|
| 399 |
|
---|
| 400 | // command DoSetMode
|
---|
| 401 | void MavlinkUDP::cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode) {
|
---|
| 402 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_MODE, 0, mavMode, 0, 0, 0, 0, 0, 0);
|
---|
| 403 | }
|
---|
| 404 |
|
---|
| 405 | // command DoSetHome
|
---|
| 406 | void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) {
|
---|
| 407 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_HOME, 0, (float)useCurrent, 0, 0, 0, 0, 0, 0);
|
---|
| 408 | }
|
---|
| 409 |
|
---|
| 410 | // command MissionStart
|
---|
| 411 | void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) {
|
---|
| 412 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_MISSION_START, 0, (float)firstItem, (float)lastItem, 0, 0, 0, 0, 0);
|
---|
| 413 | }
|
---|
| 414 |
|
---|
| 415 | // command DoSetParameter
|
---|
| 416 | void MavlinkUDP::cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue) {
|
---|
| 417 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_PARAMETER, 0, (float)paramNumber, paramValue, 0, 0, 0, 0, 0);
|
---|
| 418 | }
|
---|
| 419 |
|
---|
| 420 | // command RequestAutopilotCapabilities
|
---|
| 421 | void MavlinkUDP::cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent) {
|
---|
| 422 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, 0, 1, 0, 0, 0, 0, 0, 0);
|
---|
| 423 | }
|
---|
| 424 |
|
---|
| 425 | // command ReturnToLaunch
|
---|
| 426 | void MavlinkUDP::cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
|
---|
| 427 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0);
|
---|
| 428 | }
|
---|
| 429 |
|
---|
| 430 | // command Shutdown
|
---|
| 431 | void MavlinkUDP::cmdRebootShutdown(uint8_t targetSystem, uint8_t targetComponent, uint8_t autopilot, uint8_t onboardComputer) {
|
---|
| 432 | // for autopilot and onboardComputer, see TMAV_REBOOT_SHUTDOWN_PARAM
|
---|
| 433 | sendCommandLong(targetSystem, targetComponent, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, autopilot, onboardComputer, 0, 0, 0, 0, 0);
|
---|
| 434 | }
|
---|
| 435 |
|
---|
| 436 | // check the received message CRC, return true if OK
|
---|
| 437 | bool MavlinkUDP::check_mavlink_crc(u_int8_t *buff_in, ssize_t recsize, u_int8_t msgid) {
|
---|
| 438 | u_int16_t *crc_accum = new u_int16_t(X25_INIT_CRC);
|
---|
| 439 | u_int16_t recv_crc;
|
---|
| 440 | for (int i = 1; i < (recsize-2); ++i)
|
---|
| 441 | crc_accumulate(buff_in[i],crc_accum);
|
---|
| 442 | crc_accumulate(hds_mavlink_message_crcs[msgid],crc_accum);
|
---|
| 443 | recv_crc = buff_in[recsize-1]<<8 ^ buff_in[recsize-2];
|
---|
| 444 | //cout << "CRC(recv): " << hex << setw(4) << recv_crc << endl;
|
---|
| 445 | //cout << "CRC(calc): " << hex << setw(4) << *crc_accum << endl;
|
---|
| 446 | // if the CRCs are the same, the subtraction result should be 0:
|
---|
| 447 | recv_crc -= *crc_accum;
|
---|
| 448 | delete crc_accum;
|
---|
| 449 | if(!recv_crc) return true;
|
---|
| 450 | return false;
|
---|
| 451 | }
|
---|
| 452 |
|
---|
| 453 | // parse the received message
|
---|
| 454 | void MavlinkUDP::print_message(u_int8_t *buff_in, ssize_t recsize) {
|
---|
| 455 | uint8_t temp;
|
---|
| 456 | mavlink_message_t msg;
|
---|
| 457 | mavlink_status_t status;
|
---|
| 458 | printf("\n[PRINT MESSAGE]\n");
|
---|
| 459 | printf("Timestamp: %ld\n", get_time_usec());
|
---|
| 460 | printf("Bytes received: %ld\n", recsize);
|
---|
| 461 | printf("Datagram: ");
|
---|
| 462 | for(int i=0; i<recsize; ++i) {
|
---|
| 463 | temp = buff_in[i];
|
---|
| 464 | //cout << base(10) << temp << " ";
|
---|
| 465 | printf("%02x ", temp);
|
---|
| 466 | if(mavlink_parse_char(MAVLINK_COMM_0, temp, &msg, &status)) {
|
---|
| 467 | printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
|
---|
| 468 | //printf("Iteration: %d\n", i);
|
---|
| 469 | }
|
---|
| 470 | }
|
---|
| 471 | printf("\n");
|
---|
| 472 |
|
---|
| 473 | }
|
---|
| 474 |
|
---|
| 475 | void MavlinkUDP::decode_message(u_int8_t *buff_in, ssize_t msg_size) {
|
---|
| 476 | mavlink_message_t message;
|
---|
| 477 | mavlink_status_t status;
|
---|
| 478 | Time_Stamps this_timestamps;
|
---|
| 479 | // parse message
|
---|
| 480 | for(int i=0; i<msg_size; ++i) mavlink_parse_char(MAVLINK_COMM_0, buff_in[i], &message, &status);
|
---|
| 481 | // handle message ID
|
---|
| 482 | current_messages.sysid = message.sysid;
|
---|
| 483 | current_messages.compid = message.compid;
|
---|
| 484 | // printf("[DECODE]\n");
|
---|
| 485 | // printf("System ID: %d\n", message.sysid);
|
---|
| 486 | // printf("Component ID: %d\n", message.compid);
|
---|
| 487 | // printf("Message ID: %d\n", message.msgid);
|
---|
| 488 | // printf("Length: %d\n", message.len);
|
---|
| 489 |
|
---|
| 490 | if(check_mavlink_crc(buff_in, msg_size, message.msgid))
|
---|
| 491 | {
|
---|
| 492 | switch (message.msgid)
|
---|
| 493 | {
|
---|
| 494 | case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
|
---|
| 495 | {
|
---|
| 496 | printf("[RECV] MAVLINK_MSG_ID_MISSION_CLEAR_ALL\n");
|
---|
| 497 | mavlink_msg_mission_clear_all_decode(&message,&(current_messages.clear_all));
|
---|
| 498 | current_messages.time_stamps.clear_all = get_time_usec();
|
---|
| 499 | this_timestamps.clear_all = current_messages.time_stamps.clear_all;
|
---|
| 500 | // clear the queue
|
---|
| 501 | clearMissionPlan();
|
---|
| 502 | //
|
---|
| 503 | // TODO signal the app that the vector is cleared
|
---|
| 504 | //
|
---|
| 505 | sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
|
---|
| 506 | break;
|
---|
| 507 | }
|
---|
| 508 | case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
|
---|
| 509 | {
|
---|
| 510 | printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM_REACHED\n");
|
---|
| 511 | mavlink_msg_mission_item_reached_decode(&message, &(current_messages.mission_reached));
|
---|
| 512 | current_messages.time_stamps.mission_reached = get_time_usec();
|
---|
| 513 | this_timestamps.mission_reached = current_messages.time_stamps.mission_reached;
|
---|
| 514 | seqr=current_messages.mission_reached.seq;
|
---|
| 515 | sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
|
---|
| 516 | break;
|
---|
| 517 | }
|
---|
| 518 | case MAVLINK_MSG_ID_MISSION_COUNT:
|
---|
| 519 | {
|
---|
| 520 | printf("[RECV] MAVLINK_MSG_ID_MISSION_COUNT\n");
|
---|
| 521 | mavlink_msg_mission_count_decode(&message, &(current_messages.mission_count));
|
---|
| 522 | current_messages.time_stamps.mission_count = get_time_usec();
|
---|
| 523 | this_timestamps.mission_count = current_messages.time_stamps.mission_count;
|
---|
| 524 | Waycount=current_messages.mission_count.count;
|
---|
| 525 | setMissionCount(current_messages.mission_count.count);
|
---|
| 526 | compt=0;
|
---|
| 527 | Wayseq=-1;
|
---|
| 528 | break;
|
---|
| 529 | }
|
---|
| 530 | case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
|
---|
| 531 | {
|
---|
| 532 | printf("[RECV] MAVLINK_MSG_ID_MISSION_REQUEST_LIST\n");
|
---|
| 533 | mavlink_msg_mission_request_list_decode(&message, &(current_messages.mission_request_list));
|
---|
| 534 | current_messages.time_stamps.mission_request_list = get_time_usec();
|
---|
| 535 | this_timestamps.mission_request_list = current_messages.time_stamps.mission_request_list;
|
---|
| 536 | // send the mission count
|
---|
| 537 | sendMissionCount(target.getSysID(), target.getCompID(), getMissionCount());
|
---|
| 538 | break;
|
---|
| 539 | }
|
---|
| 540 | case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
|
---|
| 541 | {
|
---|
| 542 | printf("[RECV] MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST\n");
|
---|
| 543 | mavlink_msg_mission_write_partial_list_decode(&message, &(current_messages).mission_write_partial_list);
|
---|
| 544 | current_messages.time_stamps.mission_write_partial_list = get_time_usec();
|
---|
| 545 | this_timestamps.mission_write_partial_list = current_messages.time_stamps.mission_write_partial_list;
|
---|
| 546 | //
|
---|
| 547 | // TODO process the partial list message
|
---|
| 548 | //
|
---|
| 549 | sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
|
---|
| 550 | break;
|
---|
| 551 | }
|
---|
| 552 | case MAVLINK_MSG_ID_TIMESYNC:
|
---|
| 553 | {
|
---|
| 554 | // printf("MAVLINK_MSG_ID_TIMESYNC\n");
|
---|
| 555 | mavlink_msg_timesync_decode(&message, &(current_messages.timesync));
|
---|
| 556 | current_messages.time_stamps.timesync = get_time_usec();
|
---|
| 557 | this_timestamps.timesync = current_messages.time_stamps.timesync;
|
---|
| 558 | Xtimec=current_messages.timesync.tc1;
|
---|
| 559 | Xtimes=current_messages.timesync.ts1;
|
---|
| 560 | Xtimesync=true;
|
---|
| 561 | break;
|
---|
| 562 | }
|
---|
| 563 | case MAVLINK_MSG_ID_SYSTEM_TIME:
|
---|
| 564 | {
|
---|
| 565 | printf("[RECV] MAVLINK_MSG_ID_SYSTEM_TIME\n");
|
---|
| 566 | mavlink_msg_system_time_decode(&message, &(current_messages.system_time));
|
---|
| 567 | current_messages.time_stamps.system_time = get_time_usec();
|
---|
| 568 | this_timestamps.system_time = current_messages.time_stamps.system_time;
|
---|
| 569 | break;
|
---|
| 570 | }
|
---|
| 571 | case MAVLINK_MSG_ID_MISSION_ITEM:
|
---|
| 572 | {
|
---|
| 573 | printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM\n");
|
---|
| 574 | mavlink_msg_mission_item_decode(&message, &(current_messages.mission_item));
|
---|
| 575 | current_messages.time_stamps.mission_item = get_time_usec();
|
---|
| 576 | this_timestamps.mission_item = current_messages.time_stamps.mission_item;
|
---|
| 577 |
|
---|
| 578 | // * Mission items queue:
|
---|
| 579 | // * 16 MAV_CMD_NAV_WAYPOINT (hold time, acceptance radius, yaw angle, lat, long, altitude)
|
---|
| 580 | // * 21 MAV_CMD_NAV_LAND (abort alt, yaw angle, lat, long, altitude)
|
---|
| 581 | // * 22 MAV_CMD_NAV_TAKEOFF (yaw angle, lat, long, altitude)
|
---|
| 582 | // * 177 MAV_CMD_DO_JUMP (sequence, repeat count)
|
---|
| 583 | // * 20 MAV_CMD_NAV_RETURN_TO_LAUNCH (empty)
|
---|
| 584 |
|
---|
| 585 | //waypoint=true;
|
---|
| 586 | //setMissionCount(getMissionCount()+1);
|
---|
| 587 | //Wayseq=current_messages.mission_item.seq;
|
---|
| 588 | //frame=current_messages.mission_item.frame;
|
---|
| 589 | command=current_messages.mission_item.command;
|
---|
| 590 |
|
---|
| 591 | switch(command) {
|
---|
| 592 | case MAV_CMD_NAV_TAKEOFF:
|
---|
| 593 | {
|
---|
| 594 | printf("[RECV] MISSION ITEM: TAKEOFF\n");
|
---|
| 595 | MissionItem item;
|
---|
| 596 | item.itemID = command;
|
---|
| 597 | item.sequence = mavlink_msg_mission_item_get_seq(&message);
|
---|
| 598 | item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
|
---|
| 599 | item.latitude = mavlink_msg_mission_item_get_x(&message);
|
---|
| 600 | item.longitude = mavlink_msg_mission_item_get_y(&message);
|
---|
| 601 | item.altitude = mavlink_msg_mission_item_get_z(&message);
|
---|
| 602 | missionPlan.push(item);
|
---|
| 603 | break;
|
---|
| 604 | }
|
---|
| 605 | case MAV_CMD_NAV_LAND:
|
---|
| 606 | {
|
---|
| 607 | printf("[RECV] MISSION ITEM: LAND\n");
|
---|
| 608 | MissionItem item;
|
---|
| 609 | item.itemID = command;
|
---|
| 610 | item.sequence = mavlink_msg_mission_item_get_seq(&message);
|
---|
| 611 | item.abortAltitude = mavlink_msg_mission_item_get_param1(&message);
|
---|
| 612 | item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
|
---|
| 613 | item.latitude = mavlink_msg_mission_item_get_x(&message);
|
---|
| 614 | item.longitude = mavlink_msg_mission_item_get_y(&message);
|
---|
| 615 | item.altitude = mavlink_msg_mission_item_get_z(&message);
|
---|
| 616 | missionPlan.push(item);
|
---|
| 617 | break;
|
---|
| 618 | }
|
---|
| 619 | case MAV_CMD_NAV_WAYPOINT:
|
---|
| 620 | {
|
---|
| 621 | MissionItem item;
|
---|
| 622 | item.itemID = command;
|
---|
| 623 | item.sequence = mavlink_msg_mission_item_get_seq(&message);
|
---|
| 624 | //item.type = (uint8_t)mavlink_msg_mission_item_get_param3(&message);
|
---|
| 625 | item.holdTime = mavlink_msg_mission_item_get_param1(&message);
|
---|
| 626 | item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
|
---|
| 627 | item.latitude = mavlink_msg_mission_item_get_x(&message);
|
---|
| 628 | item.longitude = mavlink_msg_mission_item_get_y(&message);
|
---|
| 629 | item.altitude = mavlink_msg_mission_item_get_z(&message);
|
---|
| 630 | missionPlan.push(item);
|
---|
| 631 | //printf("[RECV] WAYPOINT: seq = %d, x = %.2f, y = %.2f, type = %d\n", mavlink_msg_mission_item_get_seq(&message), recvWaypoint.m_x, recvWaypoint.m_y, (int)recvWaypoint.m_type);
|
---|
| 632 | printf("[RECV] WAYPOINT: seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", item.sequence, item.latitude, item.longitude, item.altitude);
|
---|
| 633 | break;
|
---|
| 634 | }
|
---|
| 635 | }
|
---|
| 636 | // respond with ACK immediately
|
---|
| 637 | sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
|
---|
| 638 | break;
|
---|
| 639 | }
|
---|
| 640 | case MAVLINK_MSG_ID_MISSION_ACK:
|
---|
| 641 | {
|
---|
| 642 | printf("[RECV] MAVLINK_MSG_ID_MISSION_ACK\n");
|
---|
| 643 | mavlink_msg_mission_ack_decode(&message, &(current_messages.mission_ack));
|
---|
| 644 | // check if it is the right mission ack
|
---|
| 645 | current_messages.time_stamps.mission_ack = get_time_usec();
|
---|
| 646 | this_timestamps.mission_ack = current_messages.time_stamps.mission_ack;
|
---|
| 647 | setMissionAck();
|
---|
| 648 | //typeack=current_messages.mission_ack.type;
|
---|
| 649 | break;
|
---|
| 650 | }
|
---|
| 651 | case MAVLINK_MSG_ID_COMMAND_ACK:
|
---|
| 652 | {
|
---|
| 653 | printf("[RECV] MAVLINK_MSG_ID_COMMAND_ACK\n");
|
---|
| 654 | mavlink_msg_command_ack_decode(&message, &(current_messages.command_ack));
|
---|
| 655 | // check if it is the right command ack
|
---|
| 656 | current_messages.time_stamps.command_ack = get_time_usec();
|
---|
| 657 | this_timestamps.command_ack = current_messages.time_stamps.command_ack;
|
---|
| 658 | setCommandAck();
|
---|
| 659 | break;
|
---|
| 660 | }
|
---|
| 661 | case MAVLINK_MSG_ID_MISSION_REQUEST:
|
---|
| 662 | {
|
---|
| 663 | // printf("MAVLINK_MSG_ID_MISSION_REQUEST\n");
|
---|
| 664 | mavlink_msg_mission_request_decode(&message, &(current_messages.mission_request));
|
---|
| 665 | seq=current_messages.mission_request.seq;
|
---|
| 666 | current_messages.time_stamps.mission_request = get_time_usec();
|
---|
| 667 | this_timestamps.mission_request = current_messages.time_stamps.mission_request;
|
---|
| 668 | request=true;
|
---|
| 669 | break;
|
---|
| 670 | }
|
---|
| 671 | case MAVLINK_MSG_ID_HOME_POSITION:
|
---|
| 672 | {
|
---|
| 673 | printf("[RECV] MAVLINK_MSG_ID_HOME_POSITION\n");
|
---|
| 674 | mavlink_msg_home_position_decode(&message, &(current_messages.home_position));
|
---|
| 675 | current_messages.time_stamps.home_position = get_time_usec();
|
---|
| 676 | this_timestamps.home_position = current_messages.time_stamps.home_position;
|
---|
| 677 | home_position_set=true;
|
---|
| 678 | break;
|
---|
| 679 | }
|
---|
| 680 | case MAVLINK_MSG_ID_HEARTBEAT:
|
---|
| 681 | {
|
---|
| 682 | printf("[RECV] MAVLINK_MSG_ID_HEARTBEAT\n");
|
---|
| 683 | mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
|
---|
| 684 | current_messages.time_stamps.heartbeat = get_time_usec();
|
---|
| 685 | this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
|
---|
| 686 | state=current_messages.heartbeat.system_status;
|
---|
| 687 | break;
|
---|
| 688 | }
|
---|
| 689 |
|
---|
| 690 | case MAVLINK_MSG_ID_SYS_STATUS:
|
---|
| 691 | {
|
---|
| 692 | printf("[RECV] MAVLINK_MSG_ID_SYS_STATUS\n");
|
---|
| 693 | mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
|
---|
| 694 | current_messages.time_stamps.sys_status = get_time_usec();
|
---|
| 695 | this_timestamps.sys_status = current_messages.time_stamps.sys_status;
|
---|
| 696 | //USB_port->write_message(message);
|
---|
| 697 | break;
|
---|
| 698 | }
|
---|
| 699 |
|
---|
| 700 | case MAVLINK_MSG_ID_BATTERY_STATUS:
|
---|
| 701 | {
|
---|
| 702 | //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
|
---|
| 703 | mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
|
---|
| 704 | current_messages.time_stamps.battery_status = get_time_usec();
|
---|
| 705 | this_timestamps.battery_status = current_messages.time_stamps.battery_status;
|
---|
| 706 | //USB_port->write_message(message);
|
---|
| 707 | break;
|
---|
| 708 | }
|
---|
| 709 |
|
---|
| 710 | case MAVLINK_MSG_ID_RADIO_STATUS:
|
---|
| 711 | {
|
---|
| 712 | //printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
|
---|
| 713 | mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
|
---|
| 714 | current_messages.time_stamps.radio_status = get_time_usec();
|
---|
| 715 | this_timestamps.radio_status = current_messages.time_stamps.radio_status;
|
---|
| 716 | break;
|
---|
| 717 | }
|
---|
| 718 |
|
---|
| 719 | case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
|
---|
| 720 | {
|
---|
| 721 | printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
|
---|
| 722 | mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
|
---|
| 723 | current_messages.time_stamps.local_position_ned = get_time_usec();
|
---|
| 724 | this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
|
---|
| 725 | break;
|
---|
| 726 | }
|
---|
| 727 |
|
---|
| 728 | case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
|
---|
| 729 | {
|
---|
| 730 | printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
|
---|
| 731 | mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
|
---|
| 732 | current_messages.time_stamps.global_position_int = get_time_usec();
|
---|
| 733 | this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
|
---|
| 734 | //USB_port->write_message(message);
|
---|
| 735 | break;
|
---|
| 736 | }
|
---|
| 737 |
|
---|
| 738 | case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
|
---|
| 739 | {
|
---|
| 740 | printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
|
---|
| 741 | mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
|
---|
| 742 | current_messages.time_stamps.position_target_local_ned = get_time_usec();
|
---|
| 743 | this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
|
---|
| 744 | break;
|
---|
| 745 | }
|
---|
| 746 |
|
---|
| 747 | case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
|
---|
| 748 | {
|
---|
| 749 | printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
|
---|
| 750 | mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
|
---|
| 751 | current_messages.time_stamps.position_target_global_int = get_time_usec();
|
---|
| 752 | this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
|
---|
| 753 | break;
|
---|
| 754 | }
|
---|
| 755 |
|
---|
| 756 | case MAVLINK_MSG_ID_HIGHRES_IMU:
|
---|
| 757 | {
|
---|
| 758 | //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
|
---|
| 759 | mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
|
---|
| 760 | current_messages.time_stamps.highres_imu = get_time_usec();
|
---|
| 761 | this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
|
---|
| 762 | break;
|
---|
| 763 | }
|
---|
| 764 |
|
---|
| 765 | case MAVLINK_MSG_ID_ATTITUDE:
|
---|
| 766 | {
|
---|
| 767 | //printf("MAVLINK_MSG_ID_ATTITUDE\n");
|
---|
| 768 | mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
|
---|
| 769 | current_messages.time_stamps.attitude = get_time_usec();
|
---|
| 770 | this_timestamps.attitude = current_messages.time_stamps.attitude;
|
---|
| 771 | break;
|
---|
| 772 | }
|
---|
| 773 |
|
---|
| 774 | case MAVLINK_MSG_ID_COMMAND_LONG:
|
---|
| 775 | {
|
---|
| 776 | //printf("[RECV] MAVLINK_MSG_ID_COMMAND_LONG\n");
|
---|
| 777 | mavlink_msg_command_long_decode(&message, &(current_messages.command_long));
|
---|
| 778 | current_messages.time_stamps.command_long = get_time_usec();
|
---|
| 779 | this_timestamps.command_long = current_messages.time_stamps.command_long;
|
---|
| 780 | // process the received command
|
---|
| 781 | printf("Received command %d\n", current_messages.command_long.command);
|
---|
| 782 | switch(current_messages.command_long.command) {
|
---|
| 783 | case MAV_CMD_DO_SET_PARAMETER:
|
---|
| 784 | switch((int)current_messages.command_long.param1) {
|
---|
| 785 | case MAV_LOOP_MISSION_ITEMS:
|
---|
| 786 | if((int)current_messages.command_long.param2) {
|
---|
| 787 | itemsLoop();
|
---|
| 788 | } else {
|
---|
| 789 | itemsNoLoop();
|
---|
| 790 | }
|
---|
| 791 |
|
---|
| 792 | break;
|
---|
| 793 | default:
|
---|
| 794 | break;
|
---|
| 795 | }
|
---|
| 796 |
|
---|
| 797 | break;
|
---|
| 798 | case MAV_CMD_SET_MESSAGE_INTERVAL:
|
---|
| 799 | cout << "[DEBUG] callback interval value: " << (int64_t)current_messages.command_long.param2 << endl;
|
---|
| 800 | int64_t desired_interval;
|
---|
| 801 | desired_interval = (int64_t)current_messages.command_long.param2;
|
---|
| 802 | switch((int)current_messages.command_long.param1) {
|
---|
| 803 | case MAVLINK_MSG_ID_HEARTBEAT:
|
---|
| 804 | if(current_messages.command_long.param2 == -1) {
|
---|
| 805 | // stop sending position
|
---|
| 806 | sendingHeartbeat = false;
|
---|
| 807 | } else {
|
---|
| 808 | // start sending position
|
---|
| 809 | sendingHeartbeatInterval = desired_interval;
|
---|
| 810 | sendingHeartbeat = true;
|
---|
| 811 | }
|
---|
| 812 | break;
|
---|
| 813 | case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
|
---|
| 814 | if(current_messages.command_long.param2 == -1) {
|
---|
| 815 | // stop sending position
|
---|
| 816 | sendingPosition = false;
|
---|
| 817 | } else {
|
---|
| 818 | // start sending position
|
---|
| 819 | sendingPositionInterval = desired_interval;
|
---|
| 820 | sendingPosition = true;
|
---|
| 821 | }
|
---|
| 822 | break;
|
---|
| 823 | case MAVLINK_MSG_ID_ATTITUDE:
|
---|
| 824 | if(current_messages.command_long.param2 == -1) {
|
---|
| 825 | // stop sending attitude
|
---|
| 826 | sendingAttitude = false;
|
---|
| 827 | } else {
|
---|
| 828 | // start sending attitude
|
---|
| 829 | sendingAttitudeInterval = desired_interval;
|
---|
| 830 | sendingAttitude = true;
|
---|
| 831 | }
|
---|
| 832 | break;
|
---|
| 833 | case MAVLINK_MSG_ID_SYS_STATUS:
|
---|
| 834 | if(current_messages.command_long.param2 == -1) {
|
---|
| 835 | // stop sending status
|
---|
| 836 | sendingSystemStatus = false;
|
---|
| 837 | } else {
|
---|
| 838 | // start sending status
|
---|
| 839 | sendingSystemStatusInterval = desired_interval;
|
---|
| 840 | sendingSystemStatus = true;
|
---|
| 841 | }
|
---|
| 842 | break;
|
---|
| 843 | default:
|
---|
| 844 | break;
|
---|
| 845 | }
|
---|
| 846 |
|
---|
| 847 | break;
|
---|
| 848 | case MAV_CMD_MISSION_START:
|
---|
| 849 | {
|
---|
| 850 | MissionCommand missCom;
|
---|
| 851 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 852 | missCom.firstItem = (uint16_t)mavlink_msg_command_long_get_param1(&message);
|
---|
| 853 | missCom.lastItem = (uint16_t)mavlink_msg_command_long_get_param2(&message);
|
---|
| 854 | missionCommands.push(missCom);
|
---|
| 855 | printf("[RECV] MAV_CMD_MISSION_START: firstItem = %d, lastItem = %d\n", missCom.firstItem, missCom.lastItem);
|
---|
| 856 | missionStarted();
|
---|
| 857 | break;
|
---|
| 858 | }
|
---|
| 859 | case MAV_CMD_DO_LAND_START:
|
---|
| 860 | // TODO do land start
|
---|
| 861 | break;
|
---|
| 862 | case MAV_CMD_DO_SET_HOME:
|
---|
| 863 | {
|
---|
| 864 | MissionCommand missCom;
|
---|
| 865 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 866 | missCom.useCurrent = (uint8_t)mavlink_msg_command_long_get_param1(&message);
|
---|
| 867 | missCom.latitude = mavlink_msg_command_long_get_param5(&message);
|
---|
| 868 | missCom.longitude = mavlink_msg_command_long_get_param6(&message);
|
---|
| 869 | missCom.altitude = mavlink_msg_command_long_get_param7(&message);
|
---|
| 870 | missionCommands.push(missCom);
|
---|
| 871 | printf("[RECV] SET HOME: use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", missCom.useCurrent, missCom.latitude, missCom.longitude, missCom.altitude);
|
---|
| 872 | break;
|
---|
| 873 | }
|
---|
| 874 | case MAV_CMD_GET_HOME_POSITION:
|
---|
| 875 | {
|
---|
| 876 | MissionCommand missCom;
|
---|
| 877 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 878 | missionCommands.push(missCom);
|
---|
| 879 | printf("[RECV] GET HOME POSITION\n");
|
---|
| 880 | break;
|
---|
| 881 | }
|
---|
| 882 | case MAV_CMD_DO_SET_MODE:
|
---|
| 883 | {
|
---|
| 884 | MissionCommand missCom;
|
---|
| 885 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 886 | missCom.mode = mavlink_msg_command_long_get_param1(&message);
|
---|
| 887 | missionCommands.push(missCom);
|
---|
| 888 | printf("[RECV] SET MODE: %d\n", missCom.mode);
|
---|
| 889 | break;
|
---|
| 890 | }
|
---|
| 891 | case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
|
---|
| 892 | // TODO request autopilot capabilities
|
---|
| 893 | break;
|
---|
| 894 | case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
---|
| 895 | {
|
---|
| 896 | MissionItem missItem;
|
---|
| 897 | missItem.itemID = mavlink_msg_command_long_get_command(&message);
|
---|
| 898 | missionPlan.push(missItem);
|
---|
| 899 | MissionCommand missCom;
|
---|
| 900 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 901 | missionCommands.push(missCom);
|
---|
| 902 | printf("[RECV] Return to launch.\n");
|
---|
| 903 | break;
|
---|
| 904 | }
|
---|
| 905 | case MAV_CMD_DO_REPOSITION:
|
---|
| 906 | // TODO reposition
|
---|
| 907 | printf("[TODO] Do reposition.\n");
|
---|
| 908 | break;
|
---|
| 909 | case MAV_CMD_NAV_TAKEOFF:
|
---|
| 910 | // TODO takeoff
|
---|
| 911 | printf("[TODO] Execute takeoff.\n");
|
---|
| 912 | break;
|
---|
| 913 | case MAV_CMD_NAV_LAND:
|
---|
| 914 | // TODO emergency landing
|
---|
| 915 | printf("[TODO] Execute emergency landing.\n");
|
---|
| 916 | break;
|
---|
| 917 | case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
---|
| 918 | // shutdown signal received
|
---|
| 919 | shutdown_flag = true;
|
---|
| 920 | break;
|
---|
| 921 | case MAV_CMD_DO_JUMP:
|
---|
| 922 | {
|
---|
| 923 | // jump to a desired command in mission list, repeat a specified number of times
|
---|
| 924 | MissionItem missItem;
|
---|
| 925 | missItem.itemID = mavlink_msg_command_long_get_command(&message);
|
---|
| 926 | missItem.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message);
|
---|
| 927 | missItem.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message);
|
---|
| 928 | missionPlan.push(missItem);
|
---|
| 929 | MissionCommand missCom;
|
---|
| 930 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 931 | missCom.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message);
|
---|
| 932 | missCom.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message);
|
---|
| 933 | missionCommands.push(missCom);
|
---|
| 934 | printf("[RECV] MAV_CMD_DO_JUMP: seq = %d, repeat = %d\n", missItem.sequence, missItem.repeatCount);
|
---|
| 935 | }
|
---|
| 936 | case MAV_CMD_DO_PAUSE_CONTINUE:
|
---|
| 937 | {
|
---|
| 938 | MissionCommand missCom;
|
---|
| 939 | missCom.commandId = mavlink_msg_command_long_get_command(&message);
|
---|
| 940 | missCom.pauseContinue = (uint16_t)mavlink_msg_command_long_get_param1(&message);
|
---|
| 941 | missionCommands.push(missCom);
|
---|
| 942 | printf("[RECV] MAV_CMD_DO_PAUSE_CONTINUE: pauseContinue = %d\n", missCom.pauseContinue);
|
---|
| 943 | }
|
---|
| 944 | default:
|
---|
| 945 | break;
|
---|
| 946 | }
|
---|
| 947 | // respond with ACK immediately
|
---|
| 948 | sendCommandAck(current_messages.command_long.command, MAV_CMD_ACK_OK);
|
---|
| 949 | break;
|
---|
| 950 | }
|
---|
| 951 | default:
|
---|
| 952 | {
|
---|
| 953 | printf("Warning, did not handle message id %i\n",message.msgid);
|
---|
| 954 | break;
|
---|
| 955 | }
|
---|
| 956 | }
|
---|
| 957 | } else {
|
---|
| 958 | printf("ERROR: CRC check failed!\n");
|
---|
| 959 | }
|
---|
| 960 | }
|
---|
| 961 |
|
---|
| 962 | uint64_t MavlinkUDP::get_time_usec() {
|
---|
| 963 | struct timeval tv;
|
---|
| 964 | gettimeofday(&tv, NULL);
|
---|
| 965 | return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
|
---|
| 966 | }
|
---|
| 967 |
|
---|
| 968 | void MavlinkUDP::debug(string debug_msg) {
|
---|
| 969 | if(debug_messages) cout << "[DEBUG] " << debug_msg << endl;
|
---|
| 970 | }
|
---|
| 971 |
|
---|
| 972 |
|
---|
| 973 | int MavlinkUDP::waitCommandAck(uint64_t timeout) {
|
---|
| 974 | uint64_t start_time = get_time_usec();
|
---|
| 975 | uint64_t end_time = start_time + timeout;
|
---|
| 976 | //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
|
---|
| 977 | while(get_time_usec() < end_time)
|
---|
| 978 | {
|
---|
| 979 | if(getCommandAck()){
|
---|
| 980 | //printf("Command ACK received!\n");
|
---|
| 981 | resetCommandAck();
|
---|
| 982 | return 0;
|
---|
| 983 | }
|
---|
| 984 | usleep(1000);
|
---|
| 985 | }
|
---|
| 986 | printf("[ERROR] Command ACK timeout! %d\n", getCommandAck());
|
---|
| 987 | return -1;
|
---|
| 988 | }
|
---|
| 989 |
|
---|
| 990 | int MavlinkUDP::waitMissionAck(uint64_t timeout) {
|
---|
| 991 | uint64_t start_time = get_time_usec();
|
---|
| 992 | uint64_t end_time = start_time + timeout;
|
---|
| 993 | //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
|
---|
| 994 | while(get_time_usec() < end_time)
|
---|
| 995 | {
|
---|
| 996 | if(getMissionAck()){
|
---|
| 997 | //printf("Mission ACK received!\n");
|
---|
| 998 | resetMissionAck();
|
---|
| 999 | return 0;
|
---|
| 1000 | }
|
---|
| 1001 | usleep(1000);
|
---|
| 1002 | }
|
---|
| 1003 | printf("[ERROR] Mission ACK timeout!\n");
|
---|
| 1004 | return -1;
|
---|
| 1005 | }
|
---|
| 1006 |
|
---|
| 1007 | int MavlinkUDP::waitMissionCount(uint64_t timeout) {
|
---|
| 1008 | uint64_t start_time = get_time_usec();
|
---|
| 1009 | uint64_t end_time = start_time + timeout;
|
---|
| 1010 | int mission_count;
|
---|
| 1011 | //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
|
---|
| 1012 | while(get_time_usec() < end_time)
|
---|
| 1013 | {
|
---|
| 1014 | // assignment in if!
|
---|
| 1015 | if((mission_count = getMissionCount())){
|
---|
| 1016 | //printf("Mission ACK received!\n");
|
---|
| 1017 | resetMissionCount();
|
---|
| 1018 | return mission_count;
|
---|
| 1019 | }
|
---|
| 1020 | usleep(1000);
|
---|
| 1021 | }
|
---|
| 1022 | printf("[ERROR] Mission ACK timeout!\n");
|
---|
| 1023 | return -1;
|
---|
| 1024 | }
|
---|
| 1025 |
|
---|
| 1026 | MavlinkComponent::MavlinkComponent(std::string addr) : addrIP(addr) {
|
---|
| 1027 | // craete the system ID out of the IP address
|
---|
| 1028 | int byte1, byte2, byte3, byte4;
|
---|
| 1029 | char dot = '.';
|
---|
| 1030 | std::istringstream s(addrIP);
|
---|
| 1031 | s >> byte1 >> dot >> byte2 >> dot >> byte3 >> dot >> byte4;
|
---|
| 1032 | sysid = byte4;
|
---|
| 1033 | compid = 0;
|
---|
| 1034 | printf("Mavlink component created:\tSystem ID %d\tComponent ID %d\n", sysid, compid);
|
---|
| 1035 | }
|
---|