PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_main.h
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.h
36  *
37  * MAVLink 2.0 protocol interface definition.
38  *
39  * @author Lorenz Meier <lorenz@px4.io>
40  * @author Anton Babushkin <anton.babushkin@me.com>
41  */
42 
43 #pragma once
44 
45 #include <pthread.h>
46 #include <stdbool.h>
47 
48 #ifdef __PX4_NUTTX
49 #include <nuttx/fs/fs.h>
50 #else
51 #include <arpa/inet.h>
52 #include <drivers/device/device.h>
53 #include <sys/socket.h>
54 #endif
55 
56 #if defined(CONFIG_NET) || !defined(__PX4_NUTTX)
57 #include <net/if.h>
58 #include <netinet/in.h>
59 #endif
60 
61 #include <containers/List.hpp>
63 #include <parameters/param.h>
64 #include <perf/perf_counter.h>
65 #include <px4_platform_common/cli.h>
66 #include <px4_platform_common/px4_config.h>
67 #include <px4_platform_common/defines.h>
68 #include <px4_platform_common/getopt.h>
69 #include <px4_platform_common/module.h>
70 #include <px4_platform_common/module_params.h>
71 #include <px4_platform_common/posix.h>
72 #include <systemlib/mavlink_log.h>
79 
80 #include "mavlink_command_sender.h"
81 #include "mavlink_messages.h"
83 #include "mavlink_shell.h"
84 #include "mavlink_ulog.h"
85 
86 #define DEFAULT_BAUD_RATE 57600
87 #define DEFAULT_DEVICE_NAME "/dev/ttyS1"
88 
89 #define HASH_PARAM "_HASH_CHECK"
90 
91 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
92 # define MAVLINK_UDP
93 # define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec
94 #endif // CONFIG_NET || __PX4_POSIX
95 
96 enum class Protocol {
97  SERIAL = 0,
98 #if defined(MAVLINK_UDP)
99  UDP,
100 #endif // MAVLINK_UDP
101 };
102 
103 using namespace time_literals;
104 
105 class Mavlink : public ModuleParams
106 {
107 
108 public:
109  /**
110  * Constructor
111  */
112  Mavlink();
113 
114  /**
115  * Destructor, also kills the mavlinks task.
116  */
117  ~Mavlink();
118 
119  /**
120  * Start the mavlink task.
121  *
122  * @return OK on success.
123  */
124  static int start(int argc, char *argv[]);
125 
126  /**
127  * Display the mavlink status.
128  */
129  void display_status();
130 
131  /**
132  * Display the status of all enabled streams.
133  */
134  void display_status_streams();
135 
136  static int stream_command(int argc, char *argv[]);
137 
138  static int instance_count();
139 
140  static Mavlink *new_instance();
141 
142  static Mavlink *get_instance(int instance);
143 
144  static Mavlink *get_instance_for_device(const char *device_name);
145 
146  mavlink_message_t *get_buffer() { return &_mavlink_buffer; }
147 
148  mavlink_status_t *get_status() { return &_mavlink_status; }
149 
150  /**
151  * Set the MAVLink version
152  *
153  * Currently supporting v1 and v2
154  *
155  * @param version MAVLink version
156  */
157  void set_proto_version(unsigned version);
158 
159  static int destroy_all_instances();
160 
161  static int get_status_all_instances(bool show_streams_status);
162 
163  static bool serial_instance_exists(const char *device_name, Mavlink *self);
164 
165  static void forward_message(const mavlink_message_t *msg, Mavlink *self);
166 
167  static int get_uart_fd(unsigned index);
168 
169  int get_uart_fd() const { return _uart_fd; }
170 
171  /**
172  * Get the MAVLink system id.
173  *
174  * @return The system ID of this vehicle
175  */
176  int get_system_id() const { return mavlink_system.sysid; }
177 
178  /**
179  * Get the MAVLink component id.
180  *
181  * @return The component ID of this vehicle
182  */
183  int get_component_id() const { return mavlink_system.compid; }
184 
185  const char *_device_name{DEFAULT_DEVICE_NAME};
186 
188  MAVLINK_MODE_NORMAL = 0,
198 
199  MAVLINK_MODE_COUNT
200  };
201 
203  BROADCAST_MODE_OFF = 0,
205  BROADCAST_MODE_MULTICAST
206  };
207 
209  FLOW_CONTROL_OFF = 0,
211  FLOW_CONTROL_ON
212  };
213 
214  static const char *mavlink_mode_str(enum MAVLINK_MODE mode)
215  {
216  switch (mode) {
217  case MAVLINK_MODE_NORMAL:
218  return "Normal";
219 
220  case MAVLINK_MODE_CUSTOM:
221  return "Custom";
222 
223  case MAVLINK_MODE_ONBOARD:
224  return "Onboard";
225 
226  case MAVLINK_MODE_OSD:
227  return "OSD";
228 
229  case MAVLINK_MODE_MAGIC:
230  return "Magic";
231 
232  case MAVLINK_MODE_CONFIG:
233  return "Config";
234 
235  case MAVLINK_MODE_IRIDIUM:
236  return "Iridium";
237 
238  case MAVLINK_MODE_MINIMAL:
239  return "Minimal";
240 
241  case MAVLINK_MODE_EXTVISION:
242  return "ExtVision";
243 
244  case MAVLINK_MODE_EXTVISIONMIN:
245  return "ExtVisionMin";
246 
247  default:
248  return "Unknown";
249  }
250  }
251 
252  enum MAVLINK_MODE get_mode() { return _mode; }
253 
254  bool get_hil_enabled() { return _hil_enabled; }
255 
256  bool get_use_hil_gps() { return _param_mav_usehilgps.get(); }
257 
258  bool get_forward_externalsp() { return _param_mav_fwdextsp.get(); }
259 
260  bool get_flow_control_enabled() { return _flow_control_mode; }
261 
262  bool get_forwarding_on() { return _forwarding_on; }
263 
264  bool is_connected() { return (hrt_elapsed_time(&_tstatus.heartbeat_time) < 3_s); }
265 
266 #if defined(MAVLINK_UDP)
267  static Mavlink *get_instance_for_network_port(unsigned long port);
268 
269  bool broadcast_enabled() { return _param_mav_broadcast.get() == BROADCAST_MODE_ON; }
270 #endif // MAVLINK_UDP
271 
272  /**
273  * Set the boot complete flag on all instances
274  *
275  * Setting the flag unblocks parameter transmissions, which are gated
276  * beforehand to ensure that the system is fully initialized.
277  */
278  static void set_boot_complete();
279 
280  /**
281  * Get the free space in the transmit buffer
282  *
283  * @return free space in the UART TX buffer
284  */
285  unsigned get_free_tx_buf();
286 
287  static int start_helper(int argc, char *argv[]);
288 
289  /**
290  * Enable / disable Hardware in the Loop simulation mode.
291  *
292  * @param hil_enabled The new HIL enable/disable state.
293  * @return OK if the HIL state changed, ERROR if the
294  * requested change could not be made or was
295  * redundant.
296  */
297  int set_hil_enabled(bool hil_enabled);
298 
299  /**
300  * Set manual input generation mode
301  *
302  * Set to true to generate RC_INPUT messages on the system bus from
303  * MAVLink messages.
304  *
305  * @param generation_enabled If set to true, generate RC_INPUT messages
306  */
307  void set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; }
308 
309  /**
310  * Set communication protocol for this mavlink instance
311  */
312  void set_protocol(Protocol p) { _protocol = p; }
313 
314  /**
315  * Get the manual input generation mode
316  *
317  * @return true if manual inputs should generate RC data
318  */
319  bool get_manual_input_mode_generation() { return _generate_rc; }
320 
321  /**
322  * This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction
323  */
324  void begin_send() { pthread_mutex_lock(&_send_mutex); }
325 
326  /**
327  * Send bytes out on the link.
328  *
329  * On a network port these might actually get buffered to form a packet.
330  */
331  void send_bytes(const uint8_t *buf, unsigned packet_len);
332 
333  /**
334  * Flush the transmit buffer and send one MAVLink packet
335  *
336  * @return the number of bytes sent or -1 in case of error
337  */
338  int send_packet();
339 
340  /**
341  * Resend message as is, don't change sequence number and CRC.
342  */
343  void resend_message(mavlink_message_t *msg) { _mavlink_resend_uart(_channel, msg); }
344 
345  void handle_message(const mavlink_message_t *msg);
346 
347  /**
348  * Add a mavlink orb topic subscription while ensuring that only a single object exists
349  * for a given topic id and instance.
350  * @param topic orb topic id
351  * @param instance topic instance
352  * @param disable_sharing if true, force creating a new instance
353  */
354  MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance = 0, bool disable_sharing = false);
355 
356  int get_instance_id() const { return _instance_id; }
357 
358  /**
359  * Enable / disable hardware flow control.
360  *
361  * @param enabled True if hardware flow control should be enabled
362  */
363  int enable_flow_control(enum FLOW_CONTROL_MODE enabled);
364 
365  mavlink_channel_t get_channel() const { return _channel; }
366 
367  void configure_stream_threadsafe(const char *stream_name, float rate = -1.0f);
368 
369  orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
370 
371  /**
372  * Send a status text with loglevel INFO
373  *
374  * @param string the message to send (will be capped by mavlink max string length)
375  */
376  void send_statustext_info(const char *string);
377 
378  /**
379  * Send a status text with loglevel CRITICAL
380  *
381  * @param string the message to send (will be capped by mavlink max string length)
382  */
383  void send_statustext_critical(const char *string);
384 
385  /**
386  * Send a status text with loglevel EMERGENCY
387  *
388  * @param string the message to send (will be capped by mavlink max string length)
389  */
390  void send_statustext_emergency(const char *string);
391 
392  /**
393  * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
394  * only on this mavlink connection. Useful for reporting communication specific, not system-wide info
395  * only to client interested in it. Message will be not sent immediately but queued in buffer as
396  * for mavlink_log_xxx().
397  *
398  * @param string the message to send (will be capped by mavlink max string length)
399  * @param severity the log level
400  */
401  void send_statustext(unsigned char severity, const char *string);
402 
403  /**
404  * Send the capabilities of this autopilot in terms of the MAVLink spec
405  */
406  void send_autopilot_capabilites();
407 
408  /**
409  * Send the protocol version of MAVLink
410  */
411  void send_protocol_version();
412 
413  List<MavlinkStream *> &get_streams() { return _streams; }
414 
415  float get_rate_mult() const { return _rate_mult; }
416 
417  float get_baudrate() { return _baudrate; }
418 
419  /* Functions for waiting to start transmission until message received. */
420  void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
421  bool get_has_received_messages() { return _received_messages; }
422  void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
423  bool get_wait_to_transmit() { return _wait_to_transmit; }
424  bool should_transmit() { return (_transmitting_enabled && _boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
425 
426  bool message_buffer_write(const void *ptr, int size);
427 
428  void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
429  void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
430 
431  /**
432  * Count transmitted bytes
433  */
434  void count_txbytes(unsigned n) { _bytes_tx += n; };
435 
436  /**
437  * Count bytes not transmitted because of errors
438  */
439  void count_txerrbytes(unsigned n) { _bytes_txerr += n; };
440 
441  /**
442  * Count received bytes
443  */
444  void count_rxbytes(unsigned n) { _bytes_rx += n; };
445 
446  /**
447  * Get the receive status of this MAVLink link
448  */
449  telemetry_status_s &get_telemetry_status() { return _tstatus; }
450 
451  void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; }
452 
453  void update_radio_status(const radio_status_s &radio_status);
454 
455  ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; }
456 
457  unsigned get_system_type() { return _param_mav_type.get(); }
458 
459  Protocol get_protocol() const { return _protocol; }
460 
461  int get_socket_fd() { return _socket_fd; };
462 
463  bool _task_should_exit{false}; /**< Mavlink task should exit iff true. */
464 
465 #if defined(MAVLINK_UDP)
466  unsigned short get_network_port() { return _network_port; }
467 
468  unsigned short get_remote_port() { return _remote_port; }
469 
470  const in_addr query_netmask_addr(const int socket_fd, const ifreq &ifreq);
471 
472  const in_addr compute_broadcast_addr(const in_addr &host_addr, const in_addr &netmask_addr);
473 
474  struct sockaddr_in &get_client_source_address() { return _src_addr; }
475 
476  void set_client_source_initialized() { _src_addr_initialized = true; }
477 
478  bool get_client_source_initialized() { return _src_addr_initialized; }
479 #endif
480 
481  uint64_t get_start_time() { return _mavlink_start_time; }
482 
483  static bool boot_complete() { return _boot_complete; }
484 
485  bool is_usb_uart() { return _is_usb_uart; }
486 
487  int get_data_rate() { return _datarate; }
488  void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
489 
490  unsigned get_main_loop_delay() const { return _main_loop_delay; }
491 
492  /** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
493  * Returns nullptr if shell cannot be created */
494  MavlinkShell *get_shell();
495  /** close the Mavlink shell if it is open */
496  void close_shell();
497 
498  /** get ulog streaming if active, nullptr otherwise */
499  MavlinkULog *get_ulog_streaming() { return _mavlink_ulog; }
500  void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component)
501  {
502  if (_mavlink_ulog) { return; }
503 
504  _mavlink_ulog = MavlinkULog::try_start(_datarate, 0.7f, target_system, target_component);
505  }
507  {
508  if (_mavlink_ulog) { _mavlink_ulog_stop_requested = true; }
509  }
510 
511 
512  void set_uorb_main_fd(int fd, unsigned int interval);
513 
514  bool ftp_enabled() const { return _ftp_on; }
515 
516  bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); }
517  bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); }
518  bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); }
519 
521  uint64_t last_ping_time;
522  uint32_t last_ping_seq;
523  uint32_t dropped_packets;
524  float last_rtt;
525  float mean_rtt;
526  float max_rtt;
527  float min_rtt;
528  };
529 
530  /**
531  * Get the ping statistics of this MAVLink link
532  */
533  struct ping_statistics_s &get_ping_statistics() { return _ping_stats; }
534 
535  static hrt_abstime &get_first_start_time() { return _first_start_time; }
536 
537 protected:
538  Mavlink *next{nullptr};
539 
540 private:
541  int _instance_id{0};
542 
543  bool _transmitting_enabled{true};
544  bool _transmitting_enabled_commanded{false};
545  bool _first_heartbeat_sent{false};
546 
547  orb_advert_t _mavlink_log_pub{nullptr};
548 
549  uORB::PublicationQueued<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
550 
551  bool _task_running{true};
552  static bool _boot_complete;
553  static constexpr int MAVLINK_MAX_INSTANCES{4};
554  static constexpr int MAVLINK_MIN_INTERVAL{1500};
555  static constexpr int MAVLINK_MAX_INTERVAL{10000};
556  static constexpr float MAVLINK_MIN_MULTIPLIER{0.0005f};
557 
558  mavlink_message_t _mavlink_buffer {};
559  mavlink_status_t _mavlink_status {};
560 
561  /* states */
562  bool _hil_enabled{false}; /**< Hardware In the Loop mode */
563  bool _generate_rc{false}; /**< Generate RC messages from manual input MAVLink messages */
564  bool _is_usb_uart{false}; /**< Port is USB */
565  bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */
566  bool _received_messages{false}; /**< Whether we've received valid mavlink messages. */
567 
568  unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */
569 
572 
573  MavlinkShell *_mavlink_shell{nullptr};
574  MavlinkULog *_mavlink_ulog{nullptr};
575 
576  volatile bool _mavlink_ulog_stop_requested{false};
577 
578  MAVLINK_MODE _mode{MAVLINK_MODE_NORMAL};
579 
580  mavlink_channel_t _channel{MAVLINK_COMM_0};
581 
582  ringbuffer::RingBuffer _logbuffer{5, sizeof(mavlink_log_s)};
583 
584  pthread_t _receive_thread {};
585 
586  bool _forwarding_on{false};
587  bool _ftp_on{false};
588 
589  int _uart_fd{-1};
590 
591  int _baudrate{57600};
592  int _datarate{1000}; ///< data rate for normal streams (attitude, position, etc.)
593  float _rate_mult{1.0f};
594 
595  bool _radio_status_available{false};
596  bool _radio_status_critical{false};
597  float _radio_status_mult{1.0f};
598 
599  /**
600  * If the queue index is not at 0, the queue sending
601  * logic will send parameters from the current index
602  * to len - 1, the end of the param list.
603  */
604  unsigned int _mavlink_param_queue_index{0};
605 
606  bool _mavlink_link_termination_allowed{false};
607 
608  char *_subscribe_to_stream{nullptr};
609  float _subscribe_to_stream_rate{0.0f}; ///< rate of stream to subscribe to (0=disable, -1=unlimited, -2=default)
610  bool _udp_initialised{false};
611 
613 
614  uint64_t _last_write_success_time{0};
615  uint64_t _last_write_try_time{0};
616  uint64_t _mavlink_start_time{0};
617  int32_t _protocol_version_switch{-1};
618  int32_t _protocol_version{0};
619 
620  unsigned _bytes_tx{0};
621  unsigned _bytes_txerr{0};
622  unsigned _bytes_rx{0};
623  uint64_t _bytes_timestamp{0};
624 
625 #if defined(MAVLINK_UDP)
626  sockaddr_in _myaddr {};
627  sockaddr_in _src_addr {};
628  sockaddr_in _bcast_addr {};
629 
630  bool _src_addr_initialized{false};
631  bool _broadcast_address_found{false};
632  bool _broadcast_address_not_found_warned{false};
633  bool _broadcast_failed_warned{false};
634  uint8_t _network_buf[MAVLINK_MAX_PACKET_LEN] {};
635  unsigned _network_buf_len{0};
636 
637  unsigned short _network_port{14556};
638  unsigned short _remote_port{DEFAULT_REMOTE_PORT_UDP};
639 #endif // MAVLINK_UDP
640 
641  const char *_interface_name{nullptr};
642 
643  int _socket_fd{-1};
645 
646  radio_status_s _rstatus {};
647  telemetry_status_s _tstatus {};
648 
649  ping_statistics_s _ping_stats {};
650 
653  int read_ptr;
654  int size;
655  char *data;
656  };
657 
658  mavlink_message_buffer _message_buffer {};
659 
660  pthread_mutex_t _message_buffer_mutex {};
661  pthread_mutex_t _send_mutex {};
662 
664  (ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
665  (ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
666  (ParamInt<px4::params::MAV_PROTO_VER>) _param_mav_proto_ver,
667  (ParamInt<px4::params::MAV_RADIO_ID>) _param_mav_radio_id,
668  (ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
669  (ParamBool<px4::params::MAV_USEHILGPS>) _param_mav_usehilgps,
670  (ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
671 #if defined(MAVLINK_UDP)
672  (ParamInt<px4::params::MAV_BROADCAST>) _param_mav_broadcast,
673 #endif // MAVLINK_UDP
674  (ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
675  (ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
676  (ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
677  (ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
678  )
679 
680  perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, "mavlink_el")}; /**< loop performance counter */
681  perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, "mavlink_int")}; /**< loop interval performance counter */
682 
683  void mavlink_update_parameters();
684 
685  int mavlink_open_uart(const int baudrate = DEFAULT_BAUD_RATE,
686  const char *uart_name = DEFAULT_DEVICE_NAME,
687  const bool force_flow_control = false);
688 
689  static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
690  static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
691  static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;
692 
694 
695  /**
696  * Configure a single stream.
697  * @param stream_name
698  * @param rate streaming rate in Hz, -1 = unlimited rate
699  * @return 0 on success, <0 on error
700  */
701  int configure_stream(const char *stream_name, const float rate = -1.0f);
702 
703  /**
704  * Configure default streams according to _mode for either all streams or only a single
705  * stream.
706  * @param configure_single_stream: if nullptr, configure all streams, else only a single stream
707  * @return 0 on success, <0 on error
708  */
709  int configure_streams_to_default(const char *configure_single_stream = nullptr);
710 
711  int message_buffer_init(int size);
712 
713  void message_buffer_destroy();
714 
715  int message_buffer_count();
716 
717  int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); }
718 
719  int message_buffer_get_ptr(void **ptr, bool *is_part);
720 
721  void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; }
722 
723  void pass_message(const mavlink_message_t *msg);
724 
725  void publish_telemetry_status();
726 
727  void check_requested_subscriptions();
728 
729  /**
730  * Check the configuration of a connected radio
731  *
732  * This convenience function allows to re-configure a connected
733  * radio without removing it from the main system harness.
734  */
735  void check_radio_config();
736 
737  /**
738  * Update rate mult so total bitrate will be equal to _datarate.
739  */
740  void update_rate_mult();
741 
742 #if defined(MAVLINK_UDP)
743  void find_broadcast_address();
744 
745  void init_udp();
746 #endif // MAVLINK_UDP
747 
748 
749  void set_channel();
750 
751  void set_instance_id();
752 
753  /**
754  * Main mavlink task.
755  */
756  int task_main(int argc, char *argv[]);
757 
758  // Disallow copy construction and move assignment.
759  Mavlink(const Mavlink &) = delete;
760  Mavlink operator=(const Mavlink &) = delete;
761 };
static char _protocol[64]
measure the time elapsed performing an event
Definition: perf_counter.h:56
int send_packet(int uart_fd, EscPacket &packet, int responder)
Sends a packet to all ESCs and requests a specific ESC to respond.
static Mode _mode
Definition: motor_ramp.cpp:81
An intrusive linked list.
LidarLite * instance
Definition: ll40ls.cpp:65
int enable_flow_control(int uart_fd, bool enabled)
Enables/disables flow control for open UART device.
Global flash based parameter store.
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
Header common to all counters.
#define perf_alloc(a, b)
Definition: px4io.h:59
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
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
void task_main(int argc, char *argv[])
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
static int start()
Definition: dataman.cpp:1452
const char * device_name
Object metadata.
Definition: uORB.h:50
bus configurations
mode
Definition: vtol_type.h:76
measure the interval between instances of an event
Definition: perf_counter.h:57
Performance measuring tools.