PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_main.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mavlink_main.cpp
36  * MAVLink 1.0 protocol implementation.
37  *
38  * @author Lorenz Meier <lm@inf.ethz.ch>
39  * @author Julian Oes <julian@oes.ch>
40  * @author Anton Babushkin <anton.babushkin@me.com>
41  */
42 
43 #include <termios.h>
44 
45 #ifdef CONFIG_NET
46 #include <arpa/inet.h>
47 #include <netinet/in.h>
48 #include <netutils/netlib.h>
49 #endif
50 
51 #include <lib/ecl/geo/geo.h>
52 #include <lib/mathlib/mathlib.h>
53 #include <lib/version/version.h>
55 
56 #include "mavlink_receiver.h"
57 #include "mavlink_main.h"
58 
59 // Guard against MAVLink misconfiguration
60 #ifndef MAVLINK_CRC_EXTRA
61 #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
62 #endif
63 
64 // Guard against flow control misconfiguration
65 #if defined (CRTSCTS) && defined (__PX4_NUTTX) && (CRTSCTS != (CRTS_IFLOW | CCTS_OFLOW))
66 #error The non-standard CRTSCTS define is incorrect. Fix this in the OS or replace with (CRTS_IFLOW | CCTS_OFLOW)
67 #endif
68 
69 #ifdef CONFIG_NET
70 #define MAVLINK_NET_ADDED_STACK 350
71 #else
72 #define MAVLINK_NET_ADDED_STACK 0
73 #endif
74 
75 #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it.
76 #define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s
77 #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate
78 
79 static Mavlink *_mavlink_instances = nullptr;
80 
81 /**
82  * Mavlink app start / stop handling function.
83  *
84  * @ingroup apps
85  */
86 extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
87 
88 void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
89 {
90  Mavlink *m = Mavlink::get_instance(chan);
91 
92  if (m != nullptr) {
93  m->send_bytes(ch, length);
94 #ifdef MAVLINK_PRINT_PACKETS
95 
96  for (unsigned i = 0; i < length; i++) {
97  printf("%02x", (unsigned char)ch[i]);
98  }
99 
100 #endif
101  }
102 }
103 
104 void mavlink_start_uart_send(mavlink_channel_t chan, int length)
105 {
106  Mavlink *m = Mavlink::get_instance(chan);
107 
108  if (m != nullptr) {
109  (void)m->begin_send();
110 #ifdef MAVLINK_PRINT_PACKETS
111  printf("START PACKET (%u): ", (unsigned)chan);
112 #endif
113  }
114 }
115 
116 void mavlink_end_uart_send(mavlink_channel_t chan, int length)
117 {
118  Mavlink *m = Mavlink::get_instance(chan);
119 
120  if (m != nullptr) {
121  (void)m->send_packet();
122 #ifdef MAVLINK_PRINT_PACKETS
123  printf("\n");
124 #endif
125  }
126 }
127 
128 /*
129  * Internal function to give access to the channel status for each channel
130  */
131 mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
132 {
133  Mavlink *m = Mavlink::get_instance(channel);
134 
135  if (m != nullptr) {
136  return m->get_status();
137 
138  } else {
139  return nullptr;
140  }
141 }
142 
143 /*
144  * Internal function to give access to the channel buffer for each channel
145  */
146 mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
147 {
148  Mavlink *m = Mavlink::get_instance(channel);
149 
150  if (m != nullptr) {
151  return m->get_buffer();
152 
153  } else {
154  return nullptr;
155  }
156 }
157 
158 static void usage();
159 
161 
162 bool Mavlink::_boot_complete = false;
163 
165  ModuleParams(nullptr)
166 {
167  // initialise parameter cache
169 
170  // save the current system- and component ID because we don't allow them to change during operation
171  int sys_id = _param_mav_sys_id.get();
172 
173  if (sys_id > 0 && sys_id < 255) {
174  mavlink_system.sysid = sys_id;
175  }
176 
177  int comp_id = _param_mav_comp_id.get();
178 
179  if (comp_id > 0 && comp_id < 255) {
180  mavlink_system.compid = comp_id;
181  }
182 
183  if (_first_start_time == 0) {
185  }
186 }
187 
189 {
190  if (_task_running) {
191  /* task wakes up every 10ms or so at the longest */
192  _task_should_exit = true;
193 
194  /* wait for a second for the task to quit at our request */
195  unsigned i = 0;
196 
197  do {
198  /* wait 20ms */
199  px4_usleep(20000);
200 
201  /* if we have given up, kill it */
202  if (++i > 50) {
203  //TODO store main task handle in Mavlink instance to allow killing task
204  //task_delete(_mavlink_task);
205  break;
206  }
207  } while (_task_running);
208  }
209 
210  perf_free(_loop_perf);
212 }
213 
214 void
216 {
217  updateParams();
218 
219  int32_t proto = _param_mav_proto_ver.get();
220 
221  if (_protocol_version_switch != proto) {
222  _protocol_version_switch = proto;
223  set_proto_version(proto);
224  }
225 
226  if (_param_mav_type.get() < 0 || _param_mav_type.get() >= MAV_TYPE_ENUM_END) {
227  _param_mav_type.set(0);
228  _param_mav_type.commit_no_notification();
229  PX4_ERR("MAV_TYPE parameter invalid, resetting to 0.");
230  }
231 }
232 
233 void
235 {
236  /* set channel according to instance id */
237  switch (_instance_id) {
238  case 0:
239  _channel = MAVLINK_COMM_0;
240  break;
241 
242  case 1:
243  _channel = MAVLINK_COMM_1;
244  break;
245 
246  case 2:
247  _channel = MAVLINK_COMM_2;
248  break;
249 
250  case 3:
251  _channel = MAVLINK_COMM_3;
252  break;
253 #ifdef MAVLINK_COMM_4
254 
255  case 4:
256  _channel = MAVLINK_COMM_4;
257  break;
258 #endif
259 #ifdef MAVLINK_COMM_5
260 
261  case 5:
262  _channel = MAVLINK_COMM_5;
263  break;
264 #endif
265 #ifdef MAVLINK_COMM_6
266 
267  case 6:
268  _channel = MAVLINK_COMM_6;
269  break;
270 #endif
271 
272  default:
273  PX4_WARN("instance ID is out of range");
274  px4_task_exit(1);
275  break;
276  }
277 }
278 
279 void
281 {
283 }
284 
285 void
286 Mavlink::set_proto_version(unsigned version)
287 {
288  if ((version == 1 || version == 0) &&
290  get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
291  _protocol_version = 1;
292 
293  } else if (version == 2 &&
295  get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
296  _protocol_version = 2;
297  }
298 }
299 
300 int
302 {
303  size_t inst_index = 0;
304  Mavlink *inst;
305 
306  LL_FOREACH(::_mavlink_instances, inst) {
307  inst_index++;
308  }
309 
310  return inst_index;
311 }
312 
313 Mavlink *
315 {
316  Mavlink *inst;
317  LL_FOREACH(::_mavlink_instances, inst) {
318  if (instance == inst->get_instance_id()) {
319  return inst;
320  }
321  }
322 
323  return nullptr;
324 }
325 
326 Mavlink *
328 {
329  Mavlink *inst;
330 
331  LL_FOREACH(::_mavlink_instances, inst) {
332  if (strcmp(inst->_device_name, device_name) == 0) {
333  return inst;
334  }
335  }
336 
337  return nullptr;
338 }
339 
340 #ifdef MAVLINK_UDP
341 Mavlink *
342 Mavlink::get_instance_for_network_port(unsigned long port)
343 {
344  Mavlink *inst;
345 
346  LL_FOREACH(::_mavlink_instances, inst) {
347  if (inst->_network_port == port) {
348  return inst;
349  }
350  }
351 
352  return nullptr;
353 }
354 #endif // MAVLINK_UDP
355 
356 int
358 {
359  /* start deleting from the end */
360  Mavlink *inst_to_del = nullptr;
361  Mavlink *next_inst = ::_mavlink_instances;
362 
363  unsigned iterations = 0;
364 
365  PX4_INFO("waiting for instances to stop");
366 
367  while (next_inst != nullptr) {
368  inst_to_del = next_inst;
369  next_inst = inst_to_del->next;
370 
371  /* set flag to stop thread and wait for all threads to finish */
372  inst_to_del->_task_should_exit = true;
373 
374  while (inst_to_del->_task_running) {
375  printf(".");
376  fflush(stdout);
377  px4_usleep(10000);
378  iterations++;
379 
380  if (iterations > 1000) {
381  PX4_ERR("Couldn't stop all mavlink instances.");
382  return PX4_ERROR;
383  }
384  }
385 
386  }
387 
388  //we know all threads have exited, so it's safe to manipulate the linked list and delete objects.
389  while (_mavlink_instances) {
390  inst_to_del = _mavlink_instances;
391  LL_DELETE(_mavlink_instances, inst_to_del);
392  delete inst_to_del;
393  }
394 
395  printf("\n");
396  PX4_INFO("all instances stopped");
397  return OK;
398 }
399 
400 int
401 Mavlink::get_status_all_instances(bool show_streams_status)
402 {
404 
405  unsigned iterations = 0;
406 
407  while (inst != nullptr) {
408 
409  printf("\ninstance #%u:\n", iterations);
410 
411  if (show_streams_status) {
412  inst->display_status_streams();
413 
414  } else {
415  inst->display_status();
416  }
417 
418  /* move on */
419  inst = inst->next;
420  iterations++;
421  }
422 
423  /* return an error if there are no instances */
424  return (iterations == 0);
425 }
426 
427 bool
429 {
431 
432  while (inst != nullptr) {
433 
434  /* don't compare with itself and with non serial instances*/
435  if ((inst != self) && (inst->get_protocol() == Protocol::SERIAL) && !strcmp(device_name, inst->_device_name)) {
436  return true;
437  }
438 
439  inst = inst->next;
440  }
441 
442  return false;
443 }
444 
445 void
446 Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
447 {
448  Mavlink *inst;
449  LL_FOREACH(_mavlink_instances, inst) {
450  if (inst != self) {
451  const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid);
452 
453  int target_system_id = 0;
454  int target_component_id = 0;
455 
456  // might be nullptr if message is unknown
457  if (meta) {
458  // Extract target system and target component if set
459  if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
460  target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs];
461  }
462 
463  if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
464  target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs];
465  }
466  }
467 
468  // We forward messages targetted at the same system, or addressed to all systems, or
469  // if not target system is set.
470  const bool target_system_id_ok =
471  (target_system_id == 0 || target_system_id == self->get_system_id());
472 
473  // We forward messages that are targetting another component, or are addressed to all
474  // components, or if the target component is not set.
475  const bool target_component_id_ok =
476  (target_component_id == 0 || target_component_id != self->get_component_id());
477 
478  // We don't forward heartbeats unless it's specifically enabled.
479  const bool heartbeat_check_ok =
480  (msg->msgid != MAVLINK_MSG_ID_HEARTBEAT || self->forward_heartbeats_enabled());
481 
482  if (target_system_id_ok && target_component_id_ok && heartbeat_check_ok) {
483 
484  inst->pass_message(msg);
485  }
486  }
487  }
488 }
489 
490 int
491 Mavlink::get_uart_fd(unsigned index)
492 {
493  Mavlink *inst = get_instance(index);
494 
495  if (inst) {
496  return inst->get_uart_fd();
497  }
498 
499  return -1;
500 }
501 
502 int
503 Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool force_flow_control)
504 {
505 #ifndef B460800
506 #define B460800 460800
507 #endif
508 
509 #ifndef B500000
510 #define B500000 500000
511 #endif
512 
513 #ifndef B921600
514 #define B921600 921600
515 #endif
516 
517 #ifndef B1000000
518 #define B1000000 1000000
519 #endif
520 
521  /* process baud rate */
522  int speed;
523 
524  switch (baud) {
525  case 0: speed = B0; break;
526 
527  case 50: speed = B50; break;
528 
529  case 75: speed = B75; break;
530 
531  case 110: speed = B110; break;
532 
533  case 134: speed = B134; break;
534 
535  case 150: speed = B150; break;
536 
537  case 200: speed = B200; break;
538 
539  case 300: speed = B300; break;
540 
541  case 600: speed = B600; break;
542 
543  case 1200: speed = B1200; break;
544 
545  case 1800: speed = B1800; break;
546 
547  case 2400: speed = B2400; break;
548 
549  case 4800: speed = B4800; break;
550 
551  case 9600: speed = B9600; break;
552 
553  case 19200: speed = B19200; break;
554 
555  case 38400: speed = B38400; break;
556 
557  case 57600: speed = B57600; break;
558 
559  case 115200: speed = B115200; break;
560 
561  case 230400: speed = B230400; break;
562 
563  case 460800: speed = B460800; break;
564 
565  case 500000: speed = B500000; break;
566 
567  case 921600: speed = B921600; break;
568 
569  case 1000000: speed = B1000000; break;
570 
571 #ifdef B1500000
572 
573  case 1500000: speed = B1500000; break;
574 #endif
575 
576 #ifdef B2000000
577 
578  case 2000000: speed = B2000000; break;
579 #endif
580 
581 #ifdef B3000000
582 
583  case 3000000: speed = B3000000; break;
584 #endif
585 
586  default:
587  PX4_ERR("Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n",
588  baud);
589  return -EINVAL;
590  }
591 
592  /* back off 1800 ms to avoid running into the USB setup timing */
593  while (_is_usb_uart &&
594  hrt_absolute_time() < 1800U * 1000U) {
595  px4_usleep(50000);
596  }
597 
598  /* open uart */
599  _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
600 
601  /* if this is a config link, stay here and wait for it to open */
602  if (_uart_fd < 0 && _is_usb_uart) {
603 
604  uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
605 
606  /* get the system arming state and abort on arming */
607  while (_uart_fd < 0) {
608 
609  /* abort if an arming topic is published and system is armed */
610  armed_sub.update();
611 
612  /* the system is now providing arming status feedback.
613  * instead of timing out, we resort to abort bringing
614  * up the terminal.
615  */
616  if (armed_sub.get().armed) {
617  /* this is not an error, but we are done */
618  return -1;
619  }
620 
621  int errcode = errno;
622  /* ENOTCONN means that the USB device is not yet connected */
623  px4_usleep(errcode == ENOTCONN ? 1000000 : 100000);
624  _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
625  }
626  }
627 
628  if (_uart_fd < 0) {
629  return _uart_fd;
630  }
631 
632  /* Try to set baud rate */
633  struct termios uart_config;
634  int termios_state;
635 
636  /* Initialize the uart config */
637  if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) {
638  PX4_ERR("ERR GET CONF %s: %d\n", uart_name, termios_state);
639  ::close(_uart_fd);
640  return -1;
641  }
642 
643  /* Clear ONLCR flag (which appends a CR for every LF) */
644  uart_config.c_oflag &= ~ONLCR;
645 
646  if (!_is_usb_uart) {
647 
648  /* Set baud rate */
649  if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
650  PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
651  ::close(_uart_fd);
652  return -1;
653  }
654 
655  } else {
656 
657  /* USB has no baudrate, but use a magic number for 'fast' */
658  _baudrate = 2000000;
659 
660  set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
661  }
662 
663 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
664  /* Put in raw mode */
665  cfmakeraw(&uart_config);
666 #endif
667 
668  if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
669  PX4_WARN("ERR SET CONF %s\n", uart_name);
670  ::close(_uart_fd);
671  return -1;
672  }
673 
674  /*
675  * Setup hardware flow control. If the port has no RTS pin this call will fail,
676  * which is not an issue, but requires a separate call so we can fail silently.
677  */
678 
679  /* setup output flow control */
680  if (enable_flow_control(force_flow_control ? FLOW_CONTROL_ON : FLOW_CONTROL_AUTO)) {
681  PX4_WARN("hardware flow control not supported");
682  }
683 
684  return _uart_fd;
685 }
686 
687 int
689 {
690  // We can't do this on USB - skip
691  if (_is_usb_uart) {
693  return OK;
694  }
695 
696  struct termios uart_config;
697 
698  int ret = tcgetattr(_uart_fd, &uart_config);
699 
700  if (mode) {
701  uart_config.c_cflag |= CRTSCTS;
702 
703  } else {
704  uart_config.c_cflag &= ~CRTSCTS;
705 
706  }
707 
708  ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
709 
710  if (!ret) {
712  }
713 
714  return ret;
715 }
716 
717 int
718 Mavlink::set_hil_enabled(bool hil_enabled)
719 {
720  int ret = OK;
721 
722  /* enable HIL (only on links with sufficient bandwidth) */
723  if (hil_enabled && !_hil_enabled && _datarate > 5000) {
724  _hil_enabled = true;
725  ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f);
726 
727  if (_param_sys_hitl.get() == 2) { // Simulation in Hardware enabled ?
728  configure_stream("GROUND_TRUTH", 25.0f); // HIL_STATE_QUATERNION to display the SIH
729 
730  } else {
731  configure_stream("GROUND_TRUTH", 0.0f);
732  }
733  }
734 
735  /* disable HIL */
736  if (!hil_enabled && _hil_enabled) {
737  _hil_enabled = false;
738  ret = configure_stream("HIL_ACTUATOR_CONTROLS", 0.0f);
739 
740  configure_stream("GROUND_TRUTH", 0.0f);
741  }
742 
743  return ret;
744 }
745 
746 unsigned
748 {
749  /*
750  * Check if the OS buffer is full and disable HW
751  * flow control if it continues to be full
752  */
753  int buf_free = 0;
754 
755 #if defined(MAVLINK_UDP)
756 
757  // if we are using network sockets, return max length of one packet
758  if (get_protocol() == Protocol::UDP) {
759  return 1500;
760 
761  } else
762 #endif // MAVLINK_UDP
763  {
764  // No FIONSPACE on Linux todo:use SIOCOUTQ and queue size to emulate FIONSPACE
765 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
766  //Linux cp210x does not support TIOCOUTQ
767  buf_free = 256;
768 #else
769  (void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free);
770 #endif
771 
773  /* Disable hardware flow control in FLOW_CONTROL_AUTO mode:
774  * if no successful write since a defined time
775  * and if the last try was not the last successful write
776  */
777  if (_last_write_try_time != 0 &&
780 
782  }
783  }
784  }
785 
786  return buf_free;
787 }
788 
789 int
791 {
792  int ret = -1;
793 
794 #if defined(MAVLINK_UDP)
795 
796  /* Only send packets if there is something in the buffer. */
797  if (_network_buf_len == 0) {
798  pthread_mutex_unlock(&_send_mutex);
799  return 0;
800  }
801 
802  if (get_protocol() == Protocol::UDP) {
803 
804 #ifdef CONFIG_NET
805 
806  if (_src_addr_initialized) {
807 #endif
808  ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
809  (struct sockaddr *)&_src_addr, sizeof(_src_addr));
810 #ifdef CONFIG_NET
811  }
812 
813 #endif
814 
815  /* resend message via broadcast if no valid connection exists */
816  if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
817  (!get_client_source_initialized()
818  || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
819 
820  if (!_broadcast_address_found) {
821  find_broadcast_address();
822  }
823 
824  if (_broadcast_address_found && _network_buf_len > 0) {
825 
826  int bret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
827  (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
828 
829  if (bret <= 0) {
830  if (!_broadcast_failed_warned) {
831  PX4_ERR("sending broadcast failed, errno: %d: %s", errno, strerror(errno));
832  _broadcast_failed_warned = true;
833  }
834 
835  } else {
836  _broadcast_failed_warned = false;
837  }
838  }
839  }
840 
841  }
842 
843  _network_buf_len = 0;
844 
845 #endif // MAVLINK_UDP
846 
847  pthread_mutex_unlock(&_send_mutex);
848  return ret;
849 }
850 
851 void
852 Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
853 {
855 
856  if (_mavlink_start_time == 0) {
858  }
859 
860  if (get_protocol() == Protocol::SERIAL) {
861  /* check if there is space in the buffer, let it overflow else */
862  unsigned buf_free = get_free_tx_buf();
863 
864  if (buf_free < packet_len) {
865  /* not enough space in buffer to send */
866  count_txerrbytes(packet_len);
867  return;
868  }
869  }
870 
871  size_t ret = -1;
872 
873  /* send message to UART */
874  if (get_protocol() == Protocol::SERIAL) {
875  ret = ::write(_uart_fd, buf, packet_len);
876  }
877 
878 #if defined(MAVLINK_UDP)
879 
880  else {
881  if (_network_buf_len + packet_len < sizeof(_network_buf) / sizeof(_network_buf[0])) {
882  memcpy(&_network_buf[_network_buf_len], buf, packet_len);
883  _network_buf_len += packet_len;
884 
885  ret = packet_len;
886  }
887  }
888 
889 #endif // MAVLINK_UDP
890 
891  if (ret != (size_t) packet_len) {
892  count_txerrbytes(packet_len);
893 
894  } else {
896  count_txbytes(packet_len);
897  }
898 }
899 
900 #ifdef MAVLINK_UDP
901 void
902 Mavlink::find_broadcast_address()
903 {
904 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
905  struct ifconf ifconf;
906  int ret;
907 
908 #if defined(__APPLE__) && defined(__MACH__) || defined(__CYGWIN__)
909  // On Mac, we can't determine the required buffer
910  // size in advance, so we just use what tends to work.
911  ifconf.ifc_len = 1024;
912 #else
913  // On Linux, we can determine the required size of the
914  // buffer first by providing NULL to ifc_req.
915  ifconf.ifc_req = nullptr;
916  ifconf.ifc_len = 0;
917 
918  ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
919 
920  if (ret != 0) {
921  PX4_WARN("getting required buffer size failed");
922  return;
923  }
924 
925 #endif
926 
927  PX4_DEBUG("need to allocate %d bytes", ifconf.ifc_len);
928 
929  // Allocate buffer.
930  ifconf.ifc_req = (struct ifreq *)(new uint8_t[ifconf.ifc_len]);
931 
932  if (ifconf.ifc_req == nullptr) {
933  PX4_ERR("Could not allocate ifconf buffer");
934  return;
935  }
936 
937  memset(ifconf.ifc_req, 0, ifconf.ifc_len);
938 
939  ret = ioctl(_socket_fd, SIOCGIFCONF, &ifconf);
940 
941  if (ret != 0) {
942  PX4_ERR("getting network config failed");
943  delete[] ifconf.ifc_req;
944  return;
945  }
946 
947  int offset = 0;
948  // Later used to point to next network interface in buffer.
949  struct ifreq *cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
950 
951  // The ugly `for` construct is used because it allows to use
952  // `continue` and `break`.
953  for (;
954  offset < ifconf.ifc_len;
955 #if defined(__APPLE__) && defined(__MACH__)
956  // On Mac, to get to next entry in buffer, jump by the size of
957  // the interface name size plus whatever is greater, either the
958  // sizeof sockaddr or ifr_addr.sa_len.
959  offset += IF_NAMESIZE
960  + (sizeof(struct sockaddr) > cur_ifreq->ifr_addr.sa_len ?
961  sizeof(struct sockaddr) : cur_ifreq->ifr_addr.sa_len)
962 #else
963  // On Linux, it's much easier to traverse the buffer, every entry
964  // has the constant length.
965  offset += sizeof(struct ifreq)
966 #endif
967  ) {
968  // Point to next network interface in buffer.
969  cur_ifreq = (struct ifreq *) & (((uint8_t *)ifconf.ifc_req)[offset]);
970 
971  PX4_DEBUG("looking at %s", cur_ifreq->ifr_name);
972 
973  // ignore loopback network
974  if (strcmp(cur_ifreq->ifr_name, "lo") == 0 ||
975  strcmp(cur_ifreq->ifr_name, "lo0") == 0 ||
976  strcmp(cur_ifreq->ifr_name, "lo1") == 0 ||
977  strcmp(cur_ifreq->ifr_name, "lo2") == 0) {
978  PX4_DEBUG("skipping loopback");
979  continue;
980  }
981 
982  struct in_addr &sin_addr = ((struct sockaddr_in *)&cur_ifreq->ifr_addr)->sin_addr;
983 
984  // Accept network interfaces to local network only. This means it's an IP starting with:
985  // 192./172./10.
986  // Also see https://tools.ietf.org/html/rfc1918#section-3
987 
988  uint8_t first_byte = sin_addr.s_addr & 0xFF;
989 
990  if (first_byte != 192 && first_byte != 172 && first_byte != 10) {
991  continue;
992  }
993 
994  if (!_broadcast_address_found) {
995  const struct in_addr netmask_addr = query_netmask_addr(_socket_fd, *cur_ifreq);
996  const struct in_addr broadcast_addr = compute_broadcast_addr(sin_addr, netmask_addr);
997 
998  if (_interface_name && strstr(cur_ifreq->ifr_name, _interface_name) == nullptr) { continue; }
999 
1000  PX4_INFO("using network interface %s, IP: %s", cur_ifreq->ifr_name, inet_ntoa(sin_addr));
1001  PX4_INFO("with netmask: %s", inet_ntoa(netmask_addr));
1002  PX4_INFO("and broadcast IP: %s", inet_ntoa(broadcast_addr));
1003 
1004  _bcast_addr.sin_family = AF_INET;
1005  _bcast_addr.sin_addr = broadcast_addr;
1006 
1007  _broadcast_address_found = true;
1008 
1009  } else {
1010  PX4_DEBUG("ignoring additional network interface %s, IP: %s",
1011  cur_ifreq->ifr_name, inet_ntoa(sin_addr));
1012  }
1013  }
1014 
1015 #elif defined (CONFIG_NET) && defined (__PX4_NUTTX)
1016  int ret;
1017 
1018  PX4_INFO("using network interface");
1019 
1020  struct in_addr eth_addr;
1021  struct in_addr bc_addr;
1022  struct in_addr netmask_addr;
1023  ret = netlib_get_ipv4addr("eth0", &eth_addr);
1024 
1025  if (ret != 0) {
1026  PX4_ERR("getting network config failed");
1027  return;
1028  }
1029 
1030  ret = netlib_get_ipv4netmask("eth0", &netmask_addr);
1031 
1032  if (ret != 0) {
1033  PX4_ERR("getting network config failed");
1034  return;
1035  }
1036 
1037  PX4_INFO("ipv4addr IP: %s", inet_ntoa(eth_addr));
1038  PX4_INFO("netmask_addr IP: %s", inet_ntoa(netmask_addr));
1039 
1040  bc_addr.s_addr = eth_addr.s_addr | ~(netmask_addr.s_addr);
1041 
1042  if (!_broadcast_address_found) {
1043  PX4_INFO("using network interface %s, IP: %s", "eth0", inet_ntoa(eth_addr));
1044 
1045  //struct in_addr &bc_addr = ((struct sockaddr_in *)&bc_ifreq.ifr_broadaddr)->sin_addr;
1046  PX4_INFO("with broadcast IP: %s", inet_ntoa(bc_addr));
1047 
1048  _bcast_addr.sin_family = AF_INET;
1049  _bcast_addr.sin_addr = bc_addr;
1050 
1051  _broadcast_address_found = true;
1052  }
1053 
1054 #endif
1055 
1056 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
1057 
1058  if (_broadcast_address_found) {
1059  _bcast_addr.sin_port = htons(_remote_port);
1060 
1061  int broadcast_opt = 1;
1062 
1063  if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
1064  PX4_WARN("setting broadcast permission failed");
1065  }
1066 
1067  _broadcast_address_not_found_warned = false;
1068 
1069  } else {
1070  if (!_broadcast_address_not_found_warned) {
1071  PX4_WARN("no broadcasting address found");
1072  _broadcast_address_not_found_warned = true;
1073  }
1074  }
1075 
1076 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
1077  delete[] ifconf.ifc_req;
1078 #endif
1079 
1080 #endif
1081 }
1082 #endif // MAVLINK_UDP
1083 
1084 #ifdef __PX4_POSIX
1085 const in_addr
1086 Mavlink::query_netmask_addr(const int socket_fd, const ifreq &ifreq)
1087 {
1088  struct ifreq netmask_ifreq;
1089  memset(&netmask_ifreq, 0, sizeof(netmask_ifreq));
1090  strncpy(netmask_ifreq.ifr_name, ifreq.ifr_name, IF_NAMESIZE);
1091  ioctl(socket_fd, SIOCGIFNETMASK, &netmask_ifreq);
1092 
1093  return ((struct sockaddr_in *)&netmask_ifreq.ifr_addr)->sin_addr;
1094 }
1095 
1096 const in_addr
1097 Mavlink::compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr)
1098 {
1099  struct in_addr broadcast_addr;
1100  broadcast_addr.s_addr = ~netmask_addr.s_addr | host_addr.s_addr;
1101 
1102  return broadcast_addr;
1103 }
1104 #endif
1105 
1106 #ifdef MAVLINK_UDP
1107 void
1108 Mavlink::init_udp()
1109 {
1110  PX4_DEBUG("Setting up UDP with port %d", _network_port);
1111 
1112  _myaddr.sin_family = AF_INET;
1113  _myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
1114  _myaddr.sin_port = htons(_network_port);
1115 
1116  if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
1117  PX4_WARN("create socket failed: %s", strerror(errno));
1118  return;
1119  }
1120 
1121  if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
1122  PX4_WARN("bind failed: %s", strerror(errno));
1123  return;
1124  }
1125 
1126  /* set default target address, but not for onboard mode (will be set on first received packet) */
1127  if (!_src_addr_initialized) {
1128  _src_addr.sin_family = AF_INET;
1129  inet_aton("127.0.0.1", &_src_addr.sin_addr);
1130  }
1131 
1132  _src_addr.sin_port = htons(_remote_port);
1133 }
1134 #endif // MAVLINK_UDP
1135 
1136 void
1137 Mavlink::handle_message(const mavlink_message_t *msg)
1138 {
1139  /*
1140  * NOTE: this is called from the receiver thread
1141  */
1142 
1143  if (get_forwarding_on()) {
1144  /* forward any messages to other mavlink instances */
1145  Mavlink::forward_message(msg, this);
1146  }
1147 }
1148 
1149 void
1151 {
1152  mavlink_log_info(&_mavlink_log_pub, "%s", string);
1153 }
1154 
1155 void
1157 {
1158  mavlink_log_critical(&_mavlink_log_pub, "%s", string);
1159 }
1160 
1161 void
1163 {
1164  mavlink_log_emergency(&_mavlink_log_pub, "%s", string);
1165 }
1166 
1167 void
1169 {
1170  struct vehicle_status_s status;
1171 
1172  MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status));
1173 
1174  if (status_sub->update(&status)) {
1175  mavlink_autopilot_version_t msg = {};
1176 
1177  msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
1178  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
1179  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT;
1180  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
1181  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
1182  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
1183  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
1184  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
1185  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
1186  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
1187  msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
1188  msg.flight_sw_version = px4_firmware_version();
1189  msg.middleware_sw_version = px4_firmware_version();
1190  msg.os_sw_version = px4_os_version();
1191  msg.board_version = px4_board_version();
1192  /* use only first 5 bytes of git hash for firmware version */
1193  const uint64_t fw_git_version_binary = px4_firmware_version_binary() & 0xFFFFFFFFFF000000;
1194  const uint64_t fw_vendor_version = px4_firmware_vendor_version() >> 8;
1195  constexpr size_t fw_vendor_version_length = 3;
1196  memcpy(&msg.flight_custom_version, &fw_git_version_binary, sizeof(msg.flight_custom_version));
1197  memcpy(&msg.flight_custom_version, &fw_vendor_version, fw_vendor_version_length);
1198  memcpy(&msg.middleware_custom_version, &fw_git_version_binary, sizeof(msg.middleware_custom_version));
1199  uint64_t os_git_version_binary = px4_os_version_binary();
1200  memcpy(&msg.os_custom_version, &os_git_version_binary, sizeof(msg.os_custom_version));
1201 #ifdef CONFIG_CDCACM_VENDORID
1202  msg.vendor_id = CONFIG_CDCACM_VENDORID;
1203 #else
1204  msg.vendor_id = 0;
1205 #endif
1206 #ifdef CONFIG_CDCACM_PRODUCTID
1207  msg.product_id = CONFIG_CDCACM_PRODUCTID;
1208 #else
1209  msg.product_id = 0;
1210 #endif
1211  uuid_uint32_t uid;
1212  board_get_uuid32(uid);
1213  msg.uid = (((uint64_t)uid[PX4_CPU_UUID_WORD32_UNIQUE_M]) << 32) | uid[PX4_CPU_UUID_WORD32_UNIQUE_H];
1214 
1215 #ifndef BOARD_HAS_NO_UUID
1216  px4_guid_t px4_guid;
1217  board_get_px4_guid(px4_guid);
1218  static_assert(sizeof(px4_guid_t) == sizeof(msg.uid2), "GUID byte length mismatch");
1219  memcpy(&msg.uid2, &px4_guid, sizeof(msg.uid2));
1220 #endif /* BOARD_HAS_NO_UUID */
1221 
1222 #ifdef CONFIG_ARCH_BOARD_PX4_SITL
1223  // To avoid that multiple SITL instances have the same UUID, we add the mavlink
1224  // system ID. We subtract 1, so that the first UUID remains unchanged given the
1225  // default system ID is 1.
1226  //
1227  // Note that the UUID show in `ver` will still be the same for all instances.
1228  msg.uid += mavlink_system.sysid - 1;
1229  msg.uid2[0] += mavlink_system.sysid - 1;
1230 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */
1231  mavlink_msg_autopilot_version_send_struct(get_channel(), &msg);
1232  }
1233 }
1234 
1235 void
1237 {
1238  mavlink_protocol_version_t msg = {};
1239 
1240  msg.version = _protocol_version * 100;
1241  msg.min_version = 100;
1242  msg.max_version = 200;
1243  uint64_t mavlink_lib_git_version_binary = px4_mavlink_lib_version_binary();
1244  // TODO add when available
1245  //memcpy(&msg.spec_version_hash, &mavlink_spec_git_version_binary, sizeof(msg.spec_version_hash));
1246  memcpy(&msg.library_version_hash, &mavlink_lib_git_version_binary, sizeof(msg.library_version_hash));
1247 
1248  // Switch to MAVLink 2
1249  int curr_proto_ver = _protocol_version;
1250  set_proto_version(2);
1251  // Send response - if it passes through the link its fine to use MAVLink 2
1252  mavlink_msg_protocol_version_send_struct(get_channel(), &msg);
1253  // Reset to previous value
1254  set_proto_version(curr_proto_ver);
1255 }
1256 
1258 Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing)
1259 {
1260  if (!disable_sharing) {
1261  /* check if already subscribed to this topic */
1262  for (MavlinkOrbSubscription *sub : _subscriptions) {
1263  if (sub->get_topic() == topic && sub->get_instance() == instance) {
1264  /* already subscribed */
1265  return sub;
1266  }
1267  }
1268  }
1269 
1270  /* add new subscription */
1271  MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
1272 
1273  _subscriptions.add(sub_new);
1274 
1275  return sub_new;
1276 }
1277 
1278 int
1279 Mavlink::configure_stream(const char *stream_name, const float rate)
1280 {
1281  PX4_DEBUG("configure_stream(%s, %.3f)", stream_name, (double)rate);
1282 
1283  /* calculate interval in us, -1 means unlimited stream, 0 means disabled */
1284  int interval = 0;
1285 
1286  if (rate > 0.000001f) {
1287  interval = (1000000.0f / rate);
1288 
1289  } else if (rate < 0.0f) {
1290  interval = -1;
1291  }
1292 
1293  for (const auto &stream : _streams) {
1294  if (strcmp(stream_name, stream->get_name()) == 0) {
1295  if (interval != 0) {
1296  /* set new interval */
1297  stream->set_interval(interval);
1298 
1299  } else {
1300  /* delete stream */
1301  _streams.deleteNode(stream);
1302  return OK; // must finish with loop after node is deleted
1303  }
1304 
1305  return OK;
1306  }
1307  }
1308 
1309  if (interval == 0) {
1310  /* stream was not active and is requested to be disabled, do nothing */
1311  return OK;
1312  }
1313 
1314  // search for stream with specified name in supported streams list
1315  // create new instance if found
1316  MavlinkStream *stream = create_mavlink_stream(stream_name, this);
1317 
1318  if (stream != nullptr) {
1319  stream->set_interval(interval);
1320  _streams.add(stream);
1321 
1322  return OK;
1323  }
1324 
1325  /* if we reach here, the stream list does not contain the stream */
1326  PX4_WARN("stream %s not found", stream_name);
1327 
1328  return PX4_ERROR;
1329 }
1330 
1331 void
1332 Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
1333 {
1334  /* orb subscription must be done from the main thread,
1335  * set _subscribe_to_stream and _subscribe_to_stream_rate fields
1336  * which polled in mavlink main loop */
1337  if (!_task_should_exit) {
1338  /* wait for previous subscription completion */
1339  while (_subscribe_to_stream != nullptr) {
1340  px4_usleep(MAIN_LOOP_DELAY / 2);
1341  }
1342 
1343  /* copy stream name */
1344  unsigned n = strlen(stream_name) + 1;
1345  char *s = new char[n];
1346  strcpy(s, stream_name);
1347 
1348  /* set subscription task */
1351 
1352  /* wait for subscription */
1353  do {
1354  px4_usleep(MAIN_LOOP_DELAY / 2);
1355  } while (_subscribe_to_stream != nullptr);
1356 
1357  delete[] s;
1358  }
1359 }
1360 
1361 int
1363 {
1364  _message_buffer.size = size;
1367  _message_buffer.data = (char *)malloc(_message_buffer.size);
1368 
1369  int ret;
1370 
1371  if (_message_buffer.data == nullptr) {
1372  ret = PX4_ERROR;
1373  _message_buffer.size = 0;
1374 
1375  } else {
1376  ret = OK;
1377  }
1378 
1379  return ret;
1380 }
1381 
1382 void
1384 {
1385  _message_buffer.size = 0;
1388  free(_message_buffer.data);
1389 }
1390 
1391 int
1393 {
1395 
1396  if (n < 0) {
1397  n += _message_buffer.size;
1398  }
1399 
1400  return n;
1401 }
1402 
1403 bool
1404 Mavlink::message_buffer_write(const void *ptr, int size)
1405 {
1406  // bytes available to write
1407  int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
1408 
1409  if (available < 0) {
1410  available += _message_buffer.size;
1411  }
1412 
1413  if (size > available) {
1414  // buffer overflow
1415  return false;
1416  }
1417 
1418  char *c = (char *) ptr;
1419  int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer
1420 
1421  if (n < size) {
1422  // message goes over end of the buffer
1423  memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
1425 
1426  } else {
1427  n = 0;
1428  }
1429 
1430  // now: n = bytes already written
1431  int p = size - n; // number of bytes to write
1432  memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
1434  return true;
1435 }
1436 
1437 int
1438 Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
1439 {
1440  // bytes available to read
1442 
1443  if (available == 0) {
1444  return 0; // buffer is empty
1445  }
1446 
1447  int n = 0;
1448 
1449  if (available > 0) {
1450  // read pointer is before write pointer, all available bytes can be read
1451  n = available;
1452  *is_part = false;
1453 
1454  } else {
1455  // read pointer is after write pointer, read bytes from read_ptr to end of the buffer
1457  *is_part = _message_buffer.write_ptr > 0;
1458  }
1459 
1461  return n;
1462 }
1463 
1464 void
1465 Mavlink::pass_message(const mavlink_message_t *msg)
1466 {
1467  if (_forwarding_on) {
1468  /* size is 8 bytes plus variable payload */
1469  int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
1470  pthread_mutex_lock(&_message_buffer_mutex);
1471  message_buffer_write(msg, size);
1472  pthread_mutex_unlock(&_message_buffer_mutex);
1473  }
1474 }
1475 
1476 MavlinkShell *
1478 {
1479  if (!_mavlink_shell) {
1480  _mavlink_shell = new MavlinkShell();
1481 
1482  if (!_mavlink_shell) {
1483  PX4_ERR("Failed to allocate a shell");
1484 
1485  } else {
1486  int ret = _mavlink_shell->start();
1487 
1488  if (ret != 0) {
1489  PX4_ERR("Failed to start shell (%i)", ret);
1490  delete _mavlink_shell;
1491  _mavlink_shell = nullptr;
1492  }
1493  }
1494  }
1495 
1496  return _mavlink_shell;
1497 }
1498 
1499 void
1501 {
1502  if (_mavlink_shell) {
1503  delete _mavlink_shell;
1504  _mavlink_shell = nullptr;
1505  }
1506 }
1507 
1508 void
1510 {
1511  float const_rate = 0.0f;
1512  float rate = 0.0f;
1513 
1514  /* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
1515  for (const auto &stream : _streams) {
1516  if (stream->const_rate()) {
1517  const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
1518 
1519  } else {
1520  rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
1521  }
1522  }
1523 
1524  float mavlink_ulog_streaming_rate_inv = 1.0f;
1525 
1526  if (_mavlink_ulog) {
1527  mavlink_ulog_streaming_rate_inv = 1.0f - _mavlink_ulog->current_data_rate();
1528  }
1529 
1530  /* scale up and down as the link permits */
1531  float bandwidth_mult = (float)(_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate;
1532 
1533  /* if we do not have flow control, limit to the set data rate */
1534  if (!get_flow_control_enabled()) {
1535  bandwidth_mult = fminf(1.0f, bandwidth_mult);
1536  }
1537 
1538  float hardware_mult = 1.0f;
1539 
1540  /* scale down if we have a TX err rate suggesting link congestion */
1541  if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) {
1542  hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr);
1543 
1544  } else if (_radio_status_available) {
1545 
1546  // check for RADIO_STATUS timeout and reset
1547  if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) {
1548  PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
1549  set_telemetry_status_type(telemetry_status_s::LINK_TYPE_GENERIC);
1550 
1551  _radio_status_available = false;
1552  _radio_status_critical = false;
1553  _radio_status_mult = 1.0f;
1554  }
1555 
1556  hardware_mult *= _radio_status_mult;
1557  }
1558 
1559  /* pick the minimum from bandwidth mult and hardware mult as limit */
1560  _rate_mult = fminf(bandwidth_mult, hardware_mult);
1561 
1562  /* ensure the rate multiplier never drops below 5% so that something is always sent */
1563  _rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
1564 }
1565 
1566 void
1568 {
1569  _rstatus = radio_status;
1570  set_telemetry_status_type(telemetry_status_s::LINK_TYPE_3DR_RADIO);
1571 
1572  /* check hardware limits */
1573  _radio_status_available = true;
1575 
1576  if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
1577  /* this indicates link congestion, reduce rate by 20% */
1578  _radio_status_mult *= 0.80f;
1579 
1580  } else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
1581  /* this indicates link congestion, reduce rate by 2.5% */
1582  _radio_status_mult *= 0.975f;
1583 
1584  } else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
1585  /* this indicates spare bandwidth, increase by 2.5% */
1586  _radio_status_mult *= 1.025f;
1587  }
1588 }
1589 
1590 int
1591 Mavlink::configure_streams_to_default(const char *configure_single_stream)
1592 {
1593  int ret = 0;
1594  bool stream_configured = false;
1595 
1596  auto configure_stream_local =
1597  [&stream_configured, configure_single_stream, &ret, this](const char *stream_name, float rate) {
1598  if (!configure_single_stream || strcmp(configure_single_stream, stream_name) == 0) {
1599  int ret_local = configure_stream(stream_name, rate);
1600 
1601  if (ret_local != 0) {
1602  ret = ret_local;
1603  }
1604 
1605  stream_configured = true;
1606  }
1607  };
1608 
1609  const float unlimited_rate = -1.0f;
1610 
1611  switch (_mode) {
1612  case MAVLINK_MODE_NORMAL:
1613  configure_stream_local("ADSB_VEHICLE", unlimited_rate);
1614  configure_stream_local("ALTITUDE", 1.0f);
1615  configure_stream_local("ATTITUDE", 15.0f);
1616  configure_stream_local("ATTITUDE_TARGET", 2.0f);
1617  configure_stream_local("BATTERY_STATUS", 0.5f);
1618  configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
1619  configure_stream_local("COLLISION", unlimited_rate);
1620  configure_stream_local("DEBUG", 1.0f);
1621  configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
1622  configure_stream_local("DEBUG_VECT", 1.0f);
1623  configure_stream_local("DISTANCE_SENSOR", 0.5f);
1624  configure_stream_local("ESTIMATOR_STATUS", 0.5f);
1625  configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
1626  configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
1627  configure_stream_local("GPS2_RAW", 1.0f);
1628  configure_stream_local("GPS_RAW_INT", 1.0f);
1629  configure_stream_local("HOME_POSITION", 0.5f);
1630  configure_stream_local("LOCAL_POSITION_NED", 1.0f);
1631  configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
1632  configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f);
1633  configure_stream_local("OBSTACLE_DISTANCE", 1.0f);
1634  configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f);
1635  configure_stream_local("PING", 0.1f);
1636  configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f);
1637  configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
1638  configure_stream_local("RC_CHANNELS", 5.0f);
1639  configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
1640  configure_stream_local("SYS_STATUS", 1.0f);
1641  configure_stream_local("UTM_GLOBAL_POSITION", 0.5f);
1642  configure_stream_local("VFR_HUD", 4.0f);
1643  configure_stream_local("WIND_COV", 0.5f);
1644  break;
1645 
1646  case MAVLINK_MODE_ONBOARD:
1647  configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f);
1648  configure_stream_local("ADSB_VEHICLE", unlimited_rate);
1649  configure_stream_local("ALTITUDE", 10.0f);
1650  configure_stream_local("ATTITUDE", 100.0f);
1651  configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
1652  configure_stream_local("ATTITUDE_TARGET", 10.0f);
1653  configure_stream_local("BATTERY_STATUS", 0.5f);
1654  configure_stream_local("CAMERA_CAPTURE", 2.0f);
1655  configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
1656  configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
1657  configure_stream_local("COLLISION", unlimited_rate);
1658  configure_stream_local("DEBUG", 10.0f);
1659  configure_stream_local("DEBUG_FLOAT_ARRAY", 10.0f);
1660  configure_stream_local("DEBUG_VECT", 10.0f);
1661  configure_stream_local("DISTANCE_SENSOR", 10.0f);
1662  configure_stream_local("ESTIMATOR_STATUS", 1.0f);
1663  configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
1664  configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
1665  configure_stream_local("GPS2_RAW", unlimited_rate);
1666  configure_stream_local("GPS_RAW_INT", unlimited_rate);
1667  configure_stream_local("HIGHRES_IMU", 50.0f);
1668  configure_stream_local("HOME_POSITION", 0.5f);
1669  configure_stream_local("LOCAL_POSITION_NED", 30.0f);
1670  configure_stream_local("NAMED_VALUE_FLOAT", 10.0f);
1671  configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
1672  configure_stream_local("ODOMETRY", 30.0f);
1673  configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
1674  configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
1675  configure_stream_local("PING", 1.0f);
1676  configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
1677  configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
1678  configure_stream_local("RC_CHANNELS", 20.0f);
1679  configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
1680  configure_stream_local("SYS_STATUS", 5.0f);
1681  configure_stream_local("SYSTEM_TIME", 1.0f);
1682  configure_stream_local("TIMESYNC", 10.0f);
1683  configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
1684  configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
1685  configure_stream_local("VFR_HUD", 10.0f);
1686  configure_stream_local("WIND_COV", 10.0f);
1687  break;
1688 
1690  configure_stream_local("HIGHRES_IMU", unlimited_rate); // for VIO
1691  configure_stream_local("TIMESYNC", 10.0f);
1692 
1693  // FALLTHROUGH
1695  configure_stream_local("ADSB_VEHICLE", unlimited_rate);
1696  configure_stream_local("ALTITUDE", 10.0f);
1697  configure_stream_local("ATTITUDE", 20.0f);
1698  configure_stream_local("ATTITUDE_TARGET", 2.0f);
1699  configure_stream_local("BATTERY_STATUS", 0.5f);
1700  configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
1701  configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
1702  configure_stream_local("COLLISION", unlimited_rate);
1703  configure_stream_local("DEBUG", 1.0f);
1704  configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
1705  configure_stream_local("DEBUG_VECT", 1.0f);
1706  configure_stream_local("DISTANCE_SENSOR", 10.0f);
1707  configure_stream_local("ESTIMATOR_STATUS", 1.0f);
1708  configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
1709  configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
1710  configure_stream_local("GPS2_RAW", 1.0f);
1711  configure_stream_local("GPS_RAW_INT", 1.0f);
1712  configure_stream_local("HOME_POSITION", 0.5f);
1713  configure_stream_local("LOCAL_POSITION_NED", 30.0f);
1714  configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
1715  configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
1716  configure_stream_local("ODOMETRY", 30.0f);
1717  configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
1718  configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
1719  configure_stream_local("PING", 0.1f);
1720  configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f);
1721  configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
1722  configure_stream_local("RC_CHANNELS", 5.0f);
1723  configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
1724  configure_stream_local("SYS_STATUS", 5.0f);
1725  configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
1726  configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
1727  configure_stream_local("VFR_HUD", 4.0f);
1728  configure_stream_local("WIND_COV", 1.0f);
1729  break;
1730 
1731 
1732  case MAVLINK_MODE_OSD:
1733  configure_stream_local("ALTITUDE", 10.0f);
1734  configure_stream_local("ATTITUDE", 25.0f);
1735  configure_stream_local("ATTITUDE_TARGET", 10.0f);
1736  configure_stream_local("BATTERY_STATUS", 0.5f);
1737  configure_stream_local("ESTIMATOR_STATUS", 1.0f);
1738  configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
1739  configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
1740  configure_stream_local("GPS_RAW_INT", 1.0f);
1741  configure_stream_local("HOME_POSITION", 0.5f);
1742  configure_stream_local("RC_CHANNELS", 5.0f);
1743  configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
1744  configure_stream_local("SYS_STATUS", 5.0f);
1745  configure_stream_local("SYSTEM_TIME", 1.0f);
1746  configure_stream_local("VFR_HUD", 25.0f);
1747  configure_stream_local("WIND_COV", 2.0f);
1748  break;
1749 
1750  case MAVLINK_MODE_MAGIC:
1751 
1752  /* fallthrough */
1753  case MAVLINK_MODE_CUSTOM:
1754  //stream nothing
1755  break;
1756 
1757  case MAVLINK_MODE_CONFIG:
1758  // Enable a number of interesting streams we want via USB
1759  configure_stream_local("ACTUATOR_CONTROL_TARGET0", 30.0f);
1760  configure_stream_local("ADSB_VEHICLE", unlimited_rate);
1761  configure_stream_local("ALTITUDE", 10.0f);
1762  configure_stream_local("ATTITUDE", 50.0f);
1763  configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
1764  configure_stream_local("ATTITUDE_TARGET", 8.0f);
1765  configure_stream_local("BATTERY_STATUS", 0.5f);
1766  configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
1767  configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
1768  configure_stream_local("COLLISION", unlimited_rate);
1769  configure_stream_local("DEBUG", 50.0f);
1770  configure_stream_local("DEBUG_FLOAT_ARRAY", 50.0f);
1771  configure_stream_local("DEBUG_VECT", 50.0f);
1772  configure_stream_local("DISTANCE_SENSOR", 10.0f);
1773  configure_stream_local("ESTIMATOR_STATUS", 5.0f);
1774  configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
1775  configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
1776  configure_stream_local("GPS2_RAW", unlimited_rate);
1777  configure_stream_local("GPS_RAW_INT", unlimited_rate);
1778  configure_stream_local("HIGHRES_IMU", 50.0f);
1779  configure_stream_local("HOME_POSITION", 0.5f);
1780  configure_stream_local("LOCAL_POSITION_NED", 30.0f);
1781  configure_stream_local("MANUAL_CONTROL", 5.0f);
1782  configure_stream_local("NAMED_VALUE_FLOAT", 50.0f);
1783  configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f);
1784  configure_stream_local("ODOMETRY", 30.0f);
1785  configure_stream_local("OPTICAL_FLOW_RAD", 10.0f);
1786  configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f);
1787  configure_stream_local("PING", 1.0f);
1788  configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
1789  configure_stream_local("RC_CHANNELS", 10.0f);
1790  configure_stream_local("SCALED_IMU", 25.0f);
1791  configure_stream_local("SCALED_IMU2", 25.0f);
1792  configure_stream_local("SCALED_IMU3", 25.0f);
1793  configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
1794  configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
1795  configure_stream_local("SYS_STATUS", 1.0f);
1796  configure_stream_local("SYSTEM_TIME", 1.0f);
1797  configure_stream_local("TIMESYNC", 10.0f);
1798  configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
1799  configure_stream_local("VFR_HUD", 20.0f);
1800  configure_stream_local("WIND_COV", 10.0f);
1801  break;
1802 
1803  case MAVLINK_MODE_IRIDIUM:
1804  configure_stream_local("HIGH_LATENCY2", 0.015f);
1805  break;
1806 
1807  case MAVLINK_MODE_MINIMAL:
1808  configure_stream_local("ALTITUDE", 0.5f);
1809  configure_stream_local("ATTITUDE", 10.0f);
1810  configure_stream_local("EXTENDED_SYS_STATE", 0.1f);
1811  configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
1812  configure_stream_local("GPS_RAW_INT", 0.5f);
1813  configure_stream_local("HOME_POSITION", 0.1f);
1814  configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
1815  configure_stream_local("RC_CHANNELS", 0.5f);
1816  configure_stream_local("SYS_STATUS", 0.1f);
1817  configure_stream_local("VFR_HUD", 1.0f);
1818  break;
1819 
1820  default:
1821  ret = -1;
1822  break;
1823  }
1824 
1825  if (configure_single_stream && !stream_configured && strcmp(configure_single_stream, "HEARTBEAT") != 0) {
1826  // stream was not found, assume it is disabled by default
1827  return configure_stream(configure_single_stream, 0.0f);
1828  }
1829 
1830  return ret;
1831 }
1832 
1833 int
1834 Mavlink::task_main(int argc, char *argv[])
1835 {
1836  int ch;
1837  _baudrate = 57600;
1838  _datarate = 0;
1840  bool _force_flow_control = false;
1841 
1842  _interface_name = nullptr;
1843 
1844 #ifdef __PX4_NUTTX
1845  /* the NuttX optarg handler does not
1846  * ignore argv[0] like the POSIX handler
1847  * does, nor does it deal with non-flag
1848  * verbs well. So we remove the application
1849  * name and the verb.
1850  */
1851  argc -= 2;
1852  argv += 2;
1853 #endif
1854 
1855  /* don't exit from getopt loop to leave getopt global variables in consistent state,
1856  * set error flag instead */
1857  bool err_flag = false;
1858  int myoptind = 1;
1859  const char *myoptarg = nullptr;
1860 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
1861  char *eptr;
1862  int temp_int_arg;
1863 #endif
1864 
1865  while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fwxz", &myoptind, &myoptarg)) != EOF) {
1866  switch (ch) {
1867  case 'b':
1868  if (px4_get_parameter_value(myoptarg, _baudrate) != 0) {
1869  PX4_ERR("baudrate parsing failed");
1870  err_flag = true;
1871  }
1872 
1873  if (_baudrate < 9600 || _baudrate > 3000000) {
1874  PX4_ERR("invalid baud rate '%s'", myoptarg);
1875  err_flag = true;
1876  }
1877 
1878  break;
1879 
1880  case 'r':
1881  if (px4_get_parameter_value(myoptarg, _datarate) != 0) {
1882  PX4_ERR("datarate parsing failed");
1883  err_flag = true;
1884  }
1885 
1886  if (_datarate > MAX_DATA_RATE) {
1887  PX4_ERR("invalid data rate '%s'", myoptarg);
1888  err_flag = true;
1889  }
1890 
1891  break;
1892 
1893  case 'd':
1894  _device_name = myoptarg;
1896  break;
1897 
1898  case 'n':
1899  _interface_name = myoptarg;
1900  break;
1901 
1902 #if defined(MAVLINK_UDP)
1903 
1904  case 'u':
1905  temp_int_arg = strtoul(myoptarg, &eptr, 10);
1906 
1907  if (*eptr == '\0') {
1908  _network_port = temp_int_arg;
1909  set_protocol(Protocol::UDP);
1910 
1911  } else {
1912  PX4_ERR("invalid data udp_port '%s'", myoptarg);
1913  err_flag = true;
1914  }
1915 
1916  break;
1917 
1918  case 'o':
1919  temp_int_arg = strtoul(myoptarg, &eptr, 10);
1920 
1921  if (*eptr == '\0') {
1922  _remote_port = temp_int_arg;
1923  set_protocol(Protocol::UDP);
1924 
1925  } else {
1926  PX4_ERR("invalid remote udp_port '%s'", myoptarg);
1927  err_flag = true;
1928  }
1929 
1930  break;
1931 
1932  case 't':
1933  _src_addr.sin_family = AF_INET;
1934 
1935  if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
1936  _src_addr_initialized = true;
1937 
1938  } else {
1939  PX4_ERR("invalid partner ip '%s'", myoptarg);
1940  err_flag = true;
1941  }
1942 
1943  break;
1944 
1945 #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
1946 
1947  // multicast
1948  case 'c':
1949  _src_addr.sin_family = AF_INET;
1950 
1951  if (inet_aton(myoptarg, &_src_addr.sin_addr)) {
1952  _src_addr_initialized = true;
1953 
1954  } else {
1955  PX4_ERR("invalid partner ip '%s'", myoptarg);
1956  err_flag = true;
1957  }
1958 
1959  break;
1960 #else
1961 
1962  case 'c':
1963  PX4_ERR("Multicast option is not supported on this platform");
1964  err_flag = true;
1965  break;
1966 #endif
1967 #else
1968 
1969  case 'u':
1970  case 'o':
1971  case 't':
1972  PX4_ERR("UDP options not supported on this platform");
1973  err_flag = true;
1974  break;
1975 #endif
1976 
1977 // case 'e':
1978 // _mavlink_link_termination_allowed = true;
1979 // break;
1980 
1981  case 'm': {
1982 
1983  int mode;
1984 
1985  if (px4_get_parameter_value(myoptarg, mode) == 0) {
1986  if (mode >= 0 && mode < (int)MAVLINK_MODE_COUNT) {
1987  _mode = (MAVLINK_MODE)mode;
1988 
1989  } else {
1990  PX4_ERR("invalid mode");
1991  err_flag = true;
1992  }
1993 
1994  } else {
1995  if (strcmp(myoptarg, "custom") == 0) {
1997 
1998  } else if (strcmp(myoptarg, "camera") == 0) {
1999  // left in here for compatibility
2001 
2002  } else if (strcmp(myoptarg, "onboard") == 0) {
2004 
2005  } else if (strcmp(myoptarg, "osd") == 0) {
2007 
2008  } else if (strcmp(myoptarg, "magic") == 0) {
2010 
2011  } else if (strcmp(myoptarg, "config") == 0) {
2013 
2014  } else if (strcmp(myoptarg, "iridium") == 0) {
2016  set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM);
2017 
2018  } else if (strcmp(myoptarg, "minimal") == 0) {
2020 
2021  } else if (strcmp(myoptarg, "extvision") == 0) {
2023 
2024  } else if (strcmp(myoptarg, "extvisionmin") == 0) {
2026 
2027  } else {
2028  PX4_ERR("invalid mode");
2029  err_flag = true;
2030  }
2031  }
2032 
2033  break;
2034  }
2035 
2036  case 'f':
2037  _forwarding_on = true;
2038  break;
2039 
2040  case 'w':
2041  _wait_to_transmit = true;
2042  break;
2043 
2044  case 'x':
2045  _ftp_on = true;
2046  break;
2047 
2048  case 'z':
2049  _force_flow_control = true;
2050  break;
2051 
2052  default:
2053  err_flag = true;
2054  break;
2055  }
2056  }
2057 
2058  if (err_flag) {
2059  usage();
2060  return PX4_ERROR;
2061  }
2062 
2063  /* USB serial is indicated by /dev/ttyACM0*/
2064  if (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) {
2065  if (_datarate == 0) {
2066  _datarate = 800000;
2067  }
2068 
2069  if (_mode == MAVLINK_MODE_COUNT) {
2071  }
2072 
2073  _ftp_on = true;
2074  _is_usb_uart = true;
2075  }
2076 
2077  if (_mode == MAVLINK_MODE_COUNT) {
2079  }
2080 
2081  if (_datarate == 0) {
2082  /* convert bits to bytes and use 1/2 of bandwidth by default */
2083  _datarate = _baudrate / 20;
2084  }
2085 
2086  if (_datarate > MAX_DATA_RATE) {
2088  }
2089 
2090  if (get_protocol() == Protocol::SERIAL) {
2092  PX4_ERR("%s already running", _device_name);
2093  return PX4_ERROR;
2094  }
2095 
2096  PX4_INFO("mode: %s, data rate: %d B/s on %s @ %dB",
2098 
2099  /* flush stdout in case MAVLink is about to take it over */
2100  fflush(stdout);
2101 
2102  /* default values for arguments */
2103  _uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control);
2104 
2105  if (_uart_fd < 0 && !_is_usb_uart) {
2106  PX4_ERR("could not open %s", _device_name);
2107  return PX4_ERROR;
2108 
2109  } else if (_uart_fd < 0 && _is_usb_uart) {
2110  /* the config link is optional */
2111  return OK;
2112  }
2113 
2114  }
2115 
2116 #if defined(MAVLINK_UDP)
2117 
2118  else if (get_protocol() == Protocol::UDP) {
2119  if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
2120  PX4_ERR("port %d already occupied", _network_port);
2121  return PX4_ERROR;
2122  }
2123 
2124  PX4_INFO("mode: %s, data rate: %d B/s on udp port %hu remote port %hu",
2125  mavlink_mode_str(_mode), _datarate, _network_port, _remote_port);
2126  }
2127 
2128 #endif // MAVLINK_UDP
2129 
2130  /* initialize send mutex */
2131  pthread_mutex_init(&_send_mutex, nullptr);
2132 
2133  /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
2134  if (_forwarding_on) {
2135  /* initialize message buffer if multiplexing is on.
2136  * make space for two messages plus off-by-one space as we use the empty element
2137  * marker ring buffer approach.
2138  */
2139  if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
2140  PX4_ERR("msg buf alloc fail");
2141  return 1;
2142  }
2143 
2144  /* initialize message buffer mutex */
2145  pthread_mutex_init(&_message_buffer_mutex, nullptr);
2146  }
2147 
2148  uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
2149 
2150  MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true);
2151  MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
2152  uint64_t status_time = 0;
2153  MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack), 0, true);
2154  /* We don't want to miss the first advertise of an ACK, so we subscribe from the
2155  * beginning and not just when the topic exists. */
2156  ack_sub->subscribe_from_beginning(true);
2157  cmd_sub->subscribe_from_beginning(true);
2158 
2159  /* command ack */
2160  uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
2161 
2162  MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
2163 
2165  status_sub->update(&status_time, &status);
2166 
2167  /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
2168  _transmitting_enabled = true;
2170 
2171  if (_mode == MAVLINK_MODE_IRIDIUM) {
2173  }
2174 
2175  /* add default streams depending on mode */
2176  if (_mode != MAVLINK_MODE_IRIDIUM) {
2177 
2178  /* HEARTBEAT is constant rate stream, rate never adjusted */
2179  configure_stream("HEARTBEAT", 1.0f);
2180 
2181  /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
2182  configure_stream("STATUSTEXT", 20.0f);
2183 
2184  /* COMMAND_LONG stream: use unlimited rate to send all commands */
2185  configure_stream("COMMAND_LONG");
2186 
2187  }
2188 
2189  if (configure_streams_to_default() != 0) {
2190  PX4_ERR("configure_streams_to_default() failed");
2191  }
2192 
2193  /* set main loop delay depending on data rate to minimize CPU overhead */
2195 
2196  /* hard limit to 1000 Hz at max */
2199  }
2200 
2201  /* hard limit to 100 Hz at least */
2204  }
2205 
2206  set_instance_id();
2207 
2208  set_channel();
2209 
2210  /* now the instance is fully initialized and we can bump the instance count */
2211  LL_APPEND(_mavlink_instances, this);
2212 
2213 #if defined(MAVLINK_UDP)
2214 
2215  /* init socket if necessary */
2216  if (get_protocol() == Protocol::UDP) {
2217  init_udp();
2218  }
2219 
2220 #endif // MAVLINK_UDP
2221 
2222  /* if the protocol is serial, we send the system version blindly */
2223  if (get_protocol() == Protocol::SERIAL) {
2225  }
2226 
2227  /* start the MAVLink receiver last to avoid a race */
2229 
2230  while (!_task_should_exit) {
2231  /* main loop */
2232  px4_usleep(_main_loop_delay);
2233 
2234  if (!should_transmit()) {
2236  continue;
2237  }
2238 
2240  perf_begin(_loop_perf);
2241 
2243 
2244  update_rate_mult();
2245 
2246  // check for parameter updates
2247  if (parameter_update_sub.updated()) {
2248  // clear update
2249  parameter_update_s pupdate;
2250  parameter_update_sub.copy(&pupdate);
2251 
2252  // update parameters from storage
2254 
2255 #if defined(CONFIG_NET)
2256 
2257  if (_param_mav_broadcast.get() != BROADCAST_MODE_MULTICAST) {
2258  _src_addr_initialized = false;
2259  }
2260 
2261 #endif // CONFIG_NET
2262  }
2263 
2265 
2266  if (status_sub->update(&status_time, &status)) {
2267  /* switch HIL mode if required */
2268  set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
2269 
2270  set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED);
2271 
2272  if (_mode == MAVLINK_MODE_IRIDIUM) {
2273 
2274  if (_transmitting_enabled &&
2278 
2279  _transmitting_enabled = false;
2280  mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
2281 
2283  _transmitting_enabled = true;
2284  mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
2285  }
2286  }
2287  }
2288 
2289  vehicle_command_s vehicle_cmd{};
2290 
2291  if (cmd_sub->update_if_changed(&vehicle_cmd)) {
2292  if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
2293  (_mode == MAVLINK_MODE_IRIDIUM)) {
2294  if (vehicle_cmd.param1 > 0.5f) {
2295  if (!_transmitting_enabled) {
2296  mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
2297  _device_name);
2298  }
2299 
2300  _transmitting_enabled = true;
2302 
2303  } else {
2304  if (_transmitting_enabled) {
2305  mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
2306  _device_name);
2307  }
2308 
2309  _transmitting_enabled = false;
2311  }
2312 
2313  // send positive command ack
2314  vehicle_command_ack_s command_ack{};
2315  command_ack.timestamp = vehicle_cmd.timestamp;
2316  command_ack.command = vehicle_cmd.command;
2317  command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
2318  command_ack.from_external = !vehicle_cmd.from_external;
2319  command_ack.target_system = vehicle_cmd.source_system;
2320  command_ack.target_component = vehicle_cmd.source_component;
2321 
2322  command_ack_pub.publish(command_ack);
2323  }
2324  }
2325 
2326  /* send command ACK */
2327  uint16_t current_command_ack = 0;
2328  vehicle_command_ack_s command_ack{};
2329 
2330  if (ack_sub->update_if_changed(&command_ack)) {
2331  if (!command_ack.from_external) {
2332  mavlink_command_ack_t msg;
2333  msg.result = command_ack.result;
2334  msg.command = command_ack.command;
2335  msg.progress = command_ack.result_param1;
2336  msg.result_param2 = command_ack.result_param2;
2337  msg.target_system = command_ack.target_system;
2338  msg.target_component = command_ack.target_component;
2339  current_command_ack = command_ack.command;
2340 
2341  // TODO: always transmit the acknowledge once it is only sent over the instance the command is received
2342  //bool _transmitting_enabled_temp = _transmitting_enabled;
2343  //_transmitting_enabled = true;
2344  mavlink_msg_command_ack_send_struct(get_channel(), &msg);
2345  //_transmitting_enabled = _transmitting_enabled_temp;
2346  }
2347  }
2348 
2349  mavlink_log_s mavlink_log{};
2350 
2351  if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
2352  _logbuffer.put(&mavlink_log);
2353  }
2354 
2355  /* check for shell output */
2356  if (_mavlink_shell && _mavlink_shell->available() > 0) {
2357  if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
2358  mavlink_serial_control_t msg;
2359  msg.baudrate = 0;
2360  msg.flags = SERIAL_CONTROL_FLAG_REPLY;
2361  msg.timeout = 0;
2362  msg.device = SERIAL_CONTROL_DEV_SHELL;
2363  msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
2364  mavlink_msg_serial_control_send_struct(get_channel(), &msg);
2365  }
2366  }
2367 
2368  /* check for ulog streaming messages */
2369  if (_mavlink_ulog) {
2371  _mavlink_ulog->stop();
2372  _mavlink_ulog = nullptr;
2374 
2375  } else {
2376  if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
2378  }
2379 
2380  int ret = _mavlink_ulog->handle_update(get_channel());
2381 
2382  if (ret < 0) { //abort the streaming on error
2383  if (ret != -1) {
2384  PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
2385  }
2386 
2387  _mavlink_ulog->stop();
2388  _mavlink_ulog = nullptr;
2389  }
2390  }
2391  }
2392 
2394 
2395  /* update streams */
2396  for (const auto &stream : _streams) {
2397  stream->update(t);
2398 
2399  if (!_first_heartbeat_sent) {
2400  if (_mode == MAVLINK_MODE_IRIDIUM) {
2401  if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
2402  _first_heartbeat_sent = stream->first_message_sent();
2403  }
2404 
2405  } else {
2406  if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
2407  _first_heartbeat_sent = stream->first_message_sent();
2408  }
2409  }
2410  }
2411  }
2412 
2413  /* pass messages from other UARTs */
2414  if (_forwarding_on) {
2415 
2416  bool is_part;
2417  uint8_t *read_ptr;
2418  uint8_t *write_ptr;
2419 
2420  pthread_mutex_lock(&_message_buffer_mutex);
2421  int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
2422  pthread_mutex_unlock(&_message_buffer_mutex);
2423 
2424  if (available > 0) {
2425  // Reconstruct message from buffer
2426 
2427  mavlink_message_t msg;
2428  write_ptr = (uint8_t *)&msg;
2429 
2430  // Pull a single message from the buffer
2431  size_t read_count = available;
2432 
2433  if (read_count > sizeof(mavlink_message_t)) {
2434  read_count = sizeof(mavlink_message_t);
2435  }
2436 
2437  memcpy(write_ptr, read_ptr, read_count);
2438 
2439  // We hold the mutex until after we complete the second part of the buffer. If we don't
2440  // we may end up breaking the empty slot overflow detection semantics when we mark the
2441  // possibly partial read below.
2442  pthread_mutex_lock(&_message_buffer_mutex);
2443 
2444  message_buffer_mark_read(read_count);
2445 
2446  /* write second part of buffer if there is some */
2447  if (is_part && read_count < sizeof(mavlink_message_t)) {
2448  write_ptr += read_count;
2449  available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
2450  read_count = sizeof(mavlink_message_t) - read_count;
2451  memcpy(write_ptr, read_ptr, read_count);
2452  message_buffer_mark_read(available);
2453  }
2454 
2455  pthread_mutex_unlock(&_message_buffer_mutex);
2456 
2457  resend_message(&msg);
2458  }
2459  }
2460 
2461  /* update TX/RX rates*/
2462  if (t > _bytes_timestamp + 1000000) {
2463  if (_bytes_timestamp != 0) {
2464  const float dt = (t - _bytes_timestamp) / 1000.0f;
2465 
2469 
2470  _bytes_tx = 0;
2471  _bytes_txerr = 0;
2472  _bytes_rx = 0;
2473  }
2474 
2475  _bytes_timestamp = t;
2476  }
2477 
2478  // publish status at 1 Hz, or sooner if HEARTBEAT has updated
2481  }
2482 
2483  perf_end(_loop_perf);
2484  }
2485 
2486  /* first wait for threads to complete before tearing down anything */
2487  pthread_join(_receive_thread, nullptr);
2488 
2489  delete _subscribe_to_stream;
2490  _subscribe_to_stream = nullptr;
2491 
2492  /* delete streams */
2493  _streams.clear();
2494 
2495  /* delete subscriptions */
2497 
2498  if (_uart_fd >= 0 && !_is_usb_uart) {
2499  /* close UART */
2500  ::close(_uart_fd);
2501  }
2502 
2503  if (_socket_fd >= 0) {
2504  close(_socket_fd);
2505  _socket_fd = -1;
2506  }
2507 
2508  if (_forwarding_on) {
2510  pthread_mutex_destroy(&_message_buffer_mutex);
2511  }
2512 
2513  if (_mavlink_ulog) {
2514  _mavlink_ulog->stop();
2515  _mavlink_ulog = nullptr;
2516  }
2517 
2518  PX4_INFO("exiting channel %i", (int)_channel);
2519 
2520  return OK;
2521 }
2522 
2524 {
2525  if (_subscribe_to_stream != nullptr) {
2526  if (_subscribe_to_stream_rate < -1.5f) {
2528  if (get_protocol() == Protocol::SERIAL) {
2529  PX4_DEBUG("stream %s on device %s set to default rate", _subscribe_to_stream, _device_name);
2530  }
2531 
2532 #if defined(MAVLINK_UDP)
2533 
2534  else if (get_protocol() == Protocol::UDP) {
2535  PX4_DEBUG("stream %s on UDP port %d set to default rate", _subscribe_to_stream, _network_port);
2536  }
2537 
2538 #endif // MAVLINK_UDP
2539 
2540  } else {
2541  PX4_ERR("setting stream %s to default failed", _subscribe_to_stream);
2542  }
2543 
2545  if (fabsf(_subscribe_to_stream_rate) > 0.00001f) {
2546  if (get_protocol() == Protocol::SERIAL) {
2547  PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
2548  (double)_subscribe_to_stream_rate);
2549 
2550  }
2551 
2552 #if defined(MAVLINK_UDP)
2553 
2554  else if (get_protocol() == Protocol::UDP) {
2555  PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port,
2556  (double)_subscribe_to_stream_rate);
2557  }
2558 
2559 #endif // MAVLINK_UDP
2560 
2561  } else {
2562  if (get_protocol() == Protocol::SERIAL) {
2563  PX4_DEBUG("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
2564 
2565  }
2566 
2567 #if defined(MAVLINK_UDP)
2568 
2569  else if (get_protocol() == Protocol::UDP) {
2570  PX4_DEBUG("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port);
2571  }
2572 
2573 #endif // MAVLINK_UDP
2574  }
2575 
2576  } else {
2577  if (get_protocol() == Protocol::SERIAL) {
2578  PX4_ERR("stream %s on device %s not found", _subscribe_to_stream, _device_name);
2579 
2580  }
2581 
2582 #if defined(MAVLINK_UDP)
2583 
2584  else if (get_protocol() == Protocol::UDP) {
2585  PX4_ERR("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port);
2586  }
2587 
2588 #endif // MAVLINK_UDP
2589  }
2590 
2591  _subscribe_to_stream = nullptr;
2592  }
2593 }
2594 
2596 {
2597  // many fields are populated in place
2598 
2599  _tstatus.mode = _mode;
2603  _tstatus.ftp = ftp_enabled();
2606 
2608 
2610 
2612 }
2613 
2615 {
2616  /* radio config check */
2617  if (_uart_fd >= 0 && _param_mav_radio_id.get() != 0
2618  && _tstatus.type == telemetry_status_s::LINK_TYPE_3DR_RADIO) {
2619  /* request to configure radio and radio is present */
2620  FILE *fs = fdopen(_uart_fd, "w");
2621 
2622  if (fs) {
2623  /* switch to AT command mode */
2624  px4_usleep(1200000);
2625  fprintf(fs, "+++\n");
2626  px4_usleep(1200000);
2627 
2628  if (_param_mav_radio_id.get() > 0) {
2629  /* set channel */
2630  fprintf(fs, "ATS3=%u\n", _param_mav_radio_id.get());
2631  px4_usleep(200000);
2632 
2633  } else {
2634  /* reset to factory defaults */
2635  fprintf(fs, "AT&F\n");
2636  px4_usleep(200000);
2637  }
2638 
2639  /* write config */
2640  fprintf(fs, "AT&W");
2641  px4_usleep(200000);
2642 
2643  /* reboot */
2644  fprintf(fs, "ATZ");
2645  px4_usleep(200000);
2646 
2647  // XXX NuttX suffers from a bug where
2648  // fclose() also closes the fd, not just
2649  // the file stream. Since this is a one-time
2650  // config thing, we leave the file struct
2651  // allocated.
2652 #ifndef __PX4_NUTTX
2653  fclose(fs);
2654 #endif
2655 
2656  } else {
2657  PX4_WARN("open fd %d failed", _uart_fd);
2658  }
2659 
2660  /* reset param and save */
2661  _param_mav_radio_id.set(0);
2662  _param_mav_radio_id.commit_no_notification();
2663  }
2664 }
2665 
2666 int Mavlink::start_helper(int argc, char *argv[])
2667 {
2668  /* create the instance in task context */
2669  Mavlink *instance = new Mavlink();
2670 
2671  int res;
2672 
2673  if (!instance) {
2674 
2675  /* out of memory */
2676  res = -ENOMEM;
2677  PX4_ERR("OUT OF MEM");
2678 
2679  } else {
2680  /* this will actually only return once MAVLink exits */
2681  res = instance->task_main(argc, argv);
2682  instance->_task_running = false;
2683 
2684  }
2685 
2686  return res;
2687 }
2688 
2689 int
2690 Mavlink::start(int argc, char *argv[])
2691 {
2694 
2695  // Wait for the instance count to go up one
2696  // before returning to the shell
2697  int ic = Mavlink::instance_count();
2698 
2699  if (ic == Mavlink::MAVLINK_MAX_INSTANCES) {
2700  PX4_ERR("Maximum MAVLink instance count of %d reached.",
2702  return 1;
2703  }
2704 
2705  // Instantiate thread
2706  char buf[24];
2707  sprintf(buf, "mavlink_if%d", ic);
2708 
2709  // This is where the control flow splits
2710  // between the starting task and the spawned
2711  // task - start_helper() only returns
2712  // when the started task exits.
2713  px4_task_spawn_cmd(buf,
2714  SCHED_DEFAULT,
2715  SCHED_PRIORITY_DEFAULT,
2716  2650 + MAVLINK_NET_ADDED_STACK,
2717  (px4_main_t)&Mavlink::start_helper,
2718  (char *const *)argv);
2719 
2720  // Ensure that this shell command
2721  // does not return before the instance
2722  // is fully initialized. As this is also
2723  // the only path to create a new instance,
2724  // this is effectively a lock on concurrent
2725  // instance starting. XXX do a real lock.
2726 
2727  // Sleep 500 us between each attempt
2728  const unsigned sleeptime = 500;
2729 
2730  // Wait 100 ms max for the startup.
2731  const unsigned limit = 100 * 1000 / sleeptime;
2732 
2733  unsigned count = 0;
2734 
2735  while (ic == Mavlink::instance_count() && count < limit) {
2736  px4_usleep(sleeptime);
2737  count++;
2738  }
2739 
2740  if (ic == Mavlink::instance_count()) {
2741  return PX4_ERROR;
2742 
2743  } else {
2744  return PX4_OK;
2745  }
2746 }
2747 
2748 void
2750 {
2751  if (_tstatus.heartbeat_time > 0) {
2752  printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_tstatus.heartbeat_time));
2753  }
2754 
2755  printf("\tmavlink chan: #%u\n", _channel);
2756 
2757  if (_tstatus.timestamp > 0) {
2758 
2759  printf("\ttype:\t\t");
2760 
2761  switch (_tstatus.type) {
2762  case telemetry_status_s::LINK_TYPE_3DR_RADIO:
2763  printf("3DR RADIO\n");
2764  printf("\t rssi:\t\t%d\n", _rstatus.rssi);
2765  printf("\t remote rssi:\t%u\n", _rstatus.remote_rssi);
2766  printf("\t txbuf:\t%u\n", _rstatus.txbuf);
2767  printf("\t noise:\t%d\n", _rstatus.noise);
2768  printf("\t remote noise:\t%u\n", _rstatus.remote_noise);
2769  printf("\t rx errors:\t%u\n", _rstatus.rxerrors);
2770  printf("\t fixed:\t%u\n", _rstatus.fix);
2771  break;
2772 
2773  case telemetry_status_s::LINK_TYPE_USB:
2774  printf("USB CDC\n");
2775  break;
2776 
2777  default:
2778  printf("GENERIC LINK OR RADIO\n");
2779  break;
2780  }
2781 
2782  } else {
2783  printf("\tno radio status.\n");
2784  }
2785 
2786  printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
2787  printf("\trates:\n");
2788  printf("\t tx: %.3f kB/s\n", (double)_tstatus.rate_tx);
2789  printf("\t txerr: %.3f kB/s\n", (double)_tstatus.rate_txerr);
2790  printf("\t tx rate mult: %.3f\n", (double)_rate_mult);
2791  printf("\t tx rate max: %i B/s\n", _datarate);
2792  printf("\t rx: %.3f kB/s\n", (double)_tstatus.rate_rx);
2793 
2794  if (_mavlink_ulog) {
2795  printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
2796  (double)_mavlink_ulog->maximum_data_rate() * 100.);
2797  }
2798 
2799  printf("\tFTP enabled: %s, TX enabled: %s\n",
2800  _ftp_on ? "YES" : "NO",
2801  _transmitting_enabled ? "YES" : "NO");
2802  printf("\tmode: %s\n", mavlink_mode_str(_mode));
2803  printf("\tMAVLink version: %i\n", _protocol_version);
2804 
2805  printf("\ttransport protocol: ");
2806 
2807  switch (_protocol) {
2808 #if defined(MAVLINK_UDP)
2809 
2810  case Protocol::UDP:
2811  printf("UDP (%i, remote port: %i)\n", _network_port, _remote_port);
2812 #ifdef __PX4_POSIX
2813 
2814  if (get_client_source_initialized()) {
2815  printf("\tpartner IP: %s\n", inet_ntoa(get_client_source_address().sin_addr));
2816  }
2817 
2818 #endif
2819  break;
2820 #endif // MAVLINK_UDP
2821 
2822  case Protocol::SERIAL:
2823  printf("serial (%s @%i)\n", _device_name, _baudrate);
2824  break;
2825  }
2826 
2827  if (_ping_stats.last_ping_time > 0) {
2828  printf("\tping statistics:\n");
2829  printf("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt);
2830  printf("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt);
2831  printf("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt);
2832  printf("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt);
2833  printf("\t dropped packets: %u\n", _ping_stats.dropped_packets);
2834  }
2835 }
2836 
2837 void
2839 {
2840  printf("\t%-20s%-16s %s\n", "Name", "Rate Config (current) [Hz]", "Message Size (if active) [B]");
2841 
2842  const float rate_mult = _rate_mult;
2843 
2844  for (const auto &stream : _streams) {
2845  const int interval = stream->get_interval();
2846  const unsigned size = stream->get_size();
2847  char rate_str[20];
2848 
2849  if (interval < 0) {
2850  strcpy(rate_str, "unlimited");
2851 
2852  } else {
2853  float rate = 1000000.0f / (float)interval;
2854  // Note that the actual current rate can be lower if the associated uORB topic updates at a
2855  // lower rate.
2856  float rate_current = stream->const_rate() ? rate : rate * rate_mult;
2857  snprintf(rate_str, sizeof(rate_str), "%6.2f (%.3f)", (double)rate, (double)rate_current);
2858  }
2859 
2860  printf("\t%-30s%-16s", stream->get_name(), rate_str);
2861 
2862  if (size > 0) {
2863  printf(" %3i\n", size);
2864 
2865  } else {
2866  printf("\n");
2867  }
2868  }
2869 }
2870 
2871 int
2872 Mavlink::stream_command(int argc, char *argv[])
2873 {
2874  const char *device_name = DEFAULT_DEVICE_NAME;
2875  float rate = -1.0f;
2876  const char *stream_name = nullptr;
2877 #ifdef MAVLINK_UDP
2878  char *eptr;
2879  int temp_int_arg;
2880  unsigned short network_port = 0;
2881 #endif // MAVLINK_UDP
2882  bool provided_device = false;
2883  bool provided_network_port = false;
2884 
2885  /*
2886  * Called via main with original argv
2887  * mavlink start
2888  *
2889  * Remove 2
2890  */
2891  argc -= 2;
2892  argv += 2;
2893 
2894  /* don't exit from getopt loop to leave getopt global variables in consistent state,
2895  * set error flag instead */
2896  bool err_flag = false;
2897 
2898  int i = 0;
2899 
2900  while (i < argc) {
2901 
2902  if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
2903  rate = strtod(argv[i + 1], nullptr);
2904 
2905  if (rate < 0.0f) {
2906  err_flag = true;
2907  }
2908 
2909  i++;
2910 
2911  } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
2912  provided_device = true;
2913  device_name = argv[i + 1];
2914  i++;
2915 
2916  } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
2917  stream_name = argv[i + 1];
2918  i++;
2919 
2920 #ifdef MAVLINK_UDP
2921 
2922  } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
2923  provided_network_port = true;
2924  temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
2925 
2926  if (*eptr == '\0') {
2927  network_port = temp_int_arg;
2928 
2929  } else {
2930  err_flag = true;
2931  }
2932 
2933  i++;
2934 #endif // MAVLINK_UDP
2935 
2936  } else {
2937  err_flag = true;
2938  }
2939 
2940  i++;
2941  }
2942 
2943  if (!err_flag && stream_name != nullptr) {
2944 
2945  Mavlink *inst = nullptr;
2946 
2947  if (provided_device && !provided_network_port) {
2948  inst = get_instance_for_device(device_name);
2949 
2950 #ifdef MAVLINK_UDP
2951 
2952  } else if (provided_network_port && !provided_device) {
2953  inst = get_instance_for_network_port(network_port);
2954 #endif // MAVLINK_UDP
2955 
2956  } else if (provided_device && provided_network_port) {
2957  PX4_WARN("please provide either a device name or a network port");
2958  return 1;
2959  }
2960 
2961  if (rate < 0.0f) {
2962  rate = -2.0f; // use default rate
2963  }
2964 
2965  if (inst != nullptr) {
2966  inst->configure_stream_threadsafe(stream_name, rate);
2967 
2968  } else {
2969 
2970  // If the link is not running we should complain, but not fall over
2971  // because this is so easy to get wrong and not fatal. Warning is sufficient.
2972  if (provided_device) {
2973  PX4_WARN("mavlink for device %s is not running", device_name);
2974 
2975  }
2976 
2977 #ifdef MAVLINK_UDP
2978 
2979  else {
2980  PX4_WARN("mavlink for network on port %hu is not running", network_port);
2981  }
2982 
2983 #endif // MAVLINK_UDP
2984 
2985  return 1;
2986  }
2987 
2988  } else {
2989  usage();
2990  return 1;
2991  }
2992 
2993  return OK;
2994 }
2995 
2996 void
2998 {
2999  _boot_complete = true;
3000 
3001 #if defined(MAVLINK_UDP)
3002  Mavlink *inst;
3003  LL_FOREACH(::_mavlink_instances, inst) {
3004  if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) &&
3005  !inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP) {
3006 
3007  PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)");
3008  }
3009  }
3010 #endif // MAVLINK_UDP
3011 
3012 }
3013 
3014 static void usage()
3015 {
3016 
3017  PRINT_MODULE_DESCRIPTION(
3018  R"DESCR_STR(
3019 ### Description
3020 This module implements the MAVLink protocol, which can be used on a Serial link or UDP network connection.
3021 It communicates with the system via uORB: some messages are directly handled in the module (eg. mission
3022 protocol), others are published via uORB (eg. vehicle_command).
3023 
3024 Streams are used to send periodic messages with a specific rate, such as the vehicle attitude.
3025 When starting the mavlink instance, a mode can be specified, which defines the set of enabled streams with their rates.
3026 For a running instance, streams can be configured via `mavlink stream` command.
3027 
3028 There can be multiple independent instances of the module, each connected to one serial device or network port.
3029 
3030 ### Implementation
3031 The implementation uses 2 threads, a sending and a receiving thread. The sender runs at a fixed rate and dynamically
3032 reduces the rates of the streams if the combined bandwidth is higher than the configured rate (`-r`) or the
3033 physical link becomes saturated. This can be checked with `mavlink status`, see if `rate mult` is less than 1.
3034 
3035 **Careful**: some of the data is accessed and modified from both threads, so when changing code or extend the
3036 functionality, this needs to be take into account, in order to avoid race conditions and corrupt data.
3037 
3038 ### Examples
3039 Start mavlink on ttyS1 serial with baudrate 921600 and maximum sending rate of 80kB/s:
3040 $ mavlink start -d /dev/ttyS1 -b 921600 -m onboard -r 80000
3041 
3042 Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz:
3043 $ mavlink start -u 14556 -r 1000000
3044 $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
3045 )DESCR_STR");
3046 
3047  PRINT_MODULE_USAGE_NAME("mavlink", "communication");
3048  PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start a new instance");
3049  PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS1", "<file:dev>", "Select Serial Device", true);
3050  PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate (can also be p:<param_name>)", true);
3051  PRINT_MODULE_USAGE_PARAM_INT('r', 0, 10, 10000000, "Maximum sending data rate in B/s (if 0, use baudrate / 20)", true);
3052 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
3053  PRINT_MODULE_USAGE_PARAM_INT('u', 14556, 0, 65536, "Select UDP Network Port (local)", true);
3054  PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true);
3055  PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr,
3056  "Partner IP (broadcasting can be enabled via MAV_BROADCAST param)", true);
3057 #endif
3058  PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvsision",
3059  "Mode: sets default streams and rates", true);
3060  PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "<interface_name>", "wifi/ethernet interface name", true);
3061 #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
3062  PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_BROADCAST param)", true);
3063 #endif
3064  PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true);
3065  PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true);
3066  PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true);
3067  PRINT_MODULE_USAGE_PARAM_FLAG('z', "Force flow control always on", true);
3068 
3069  PRINT_MODULE_USAGE_COMMAND_DESCR("stop-all", "Stop all instances");
3070 
3071  PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status for all instances");
3072  PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true);
3073 
3074  PRINT_MODULE_USAGE_COMMAND_DESCR("stream", "Configure the sending rate of a stream for a running instance");
3075 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
3076  PRINT_MODULE_USAGE_PARAM_INT('u', -1, 0, 65536, "Select Mavlink instance via local Network Port", true);
3077 #endif
3078  PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Select Mavlink instance via Serial Device", true);
3079  PRINT_MODULE_USAGE_PARAM_STRING('s', nullptr, nullptr, "Mavlink stream to configure", false);
3080  PRINT_MODULE_USAGE_PARAM_FLOAT('r', -1.0f, 0.0f, 2000.0f, "Rate in Hz (0 = turn off, -1 = set to default)", false);
3081 
3082  PRINT_MODULE_USAGE_COMMAND_DESCR("boot_complete",
3083  "Enable sending of messages. (Must be) called as last step in startup script.");
3084 
3085 }
3086 
3087 int mavlink_main(int argc, char *argv[])
3088 {
3089  if (argc < 2) {
3090  usage();
3091  return 1;
3092  }
3093 
3094  if (!strcmp(argv[1], "start")) {
3095  return Mavlink::start(argc, argv);
3096 
3097  } else if (!strcmp(argv[1], "stop")) {
3098  PX4_WARN("mavlink stop is deprecated, use stop-all instead");
3099  usage();
3100  return 1;
3101 
3102  } else if (!strcmp(argv[1], "stop-all")) {
3104 
3105  } else if (!strcmp(argv[1], "status")) {
3106  bool show_streams_status = argc > 2 && strcmp(argv[2], "streams") == 0;
3107  return Mavlink::get_status_all_instances(show_streams_status);
3108 
3109  } else if (!strcmp(argv[1], "stream")) {
3110  return Mavlink::stream_command(argc, argv);
3111 
3112  } else if (!strcmp(argv[1], "boot_complete")) {
3114  return 0;
3115 
3116  } else {
3117  usage();
3118  return 1;
3119  }
3120 
3121  return 0;
3122 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
#define LL_FOREACH(head, el)
Definition: utlist.h:377
__EXPORT uint64_t px4_mavlink_lib_version_binary(void)
MAVLink lib version in binary form (first part of the git tag)
uint8_t remote_noise
Definition: radio_status.h:60
static struct vehicle_status_s status
Definition: Commander.cpp:138
uint32_t px4_firmware_version(void)
get the PX4 Firmware version
Definition: version.c:149
Definition of geo / math functions to perform geodesic calculations.
uint16_t fix
Definition: radio_status.h:55
Definition: I2C.hpp:51
bool publish(const T &data)
Publish the struct.
uint64_t px4_os_version_binary(void)
Operating system version in binary form (first part of the git tag)
Definition: version.c:364
LidarLite * instance
Definition: ll40ls.cpp:65
uint64_t timestamp
Definition: radio_status.h:53
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uint32_t px4_os_version(void)
operating system version
Definition: version.c:259
void perf_count(perf_counter_t handle)
Count a performance event.
void perf_free(perf_counter_t handle)
Free a counter.
uint16_t rxerrors
Definition: radio_status.h:54
uint8_t remote_rssi
Definition: radio_status.h:57
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void add(T newNode)
Definition: List.hpp:63
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
#define LL_APPEND(head, add)
Definition: utlist.h:307
bool high_latency_data_link_lost
uint64_t px4_firmware_version_binary(void)
Firmware version in binary form (first part of the git tag)
Definition: version.c:343
void clear()
Definition: List.hpp:162
void perf_end(perf_counter_t handle)
End a performance event.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
Tools for system version detection.
const char * device_name
static void write(bootloader_app_shared_t *pshared)
Object metadata.
Definition: uORB.h:50
uint32_t px4_firmware_vendor_version(void)
get the PX4 Firmware vendor version
Definition: version.c:240
float dt
Definition: px4io.c:73
#define LL_DELETE(head, del)
Definition: utlist.h:320
#define CONFIG_CDCACM_PRODUCTID
Definition: version.c:41
#define OK
Definition: uavcan_main.cpp:71
uint32_t px4_board_version(void)
get the board version (last 8 bytes should be silicon ID, if any)
Definition: version.c:250
mode
Definition: vtol_type.h:76
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
size_t size() const
Definition: List.hpp:143