49 #include <nuttx/fs/fs.h> 51 #include <arpa/inet.h> 53 #include <sys/socket.h> 56 #if defined(CONFIG_NET) || !defined(__PX4_NUTTX) 58 #include <netinet/in.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> 86 #define DEFAULT_BAUD_RATE 57600 87 #define DEFAULT_DEVICE_NAME "/dev/ttyS1" 89 #define HASH_PARAM "_HASH_CHECK" 91 #if defined(CONFIG_NET) || defined(__PX4_POSIX) 93 # define DEFAULT_REMOTE_PORT_UDP 14550 94 #endif // CONFIG_NET || __PX4_POSIX
98 #if defined(MAVLINK_UDP) 100 #endif // MAVLINK_UDP 124 static int start(
int argc,
char *argv[]);
129 void display_status();
134 void display_status_streams();
136 static int stream_command(
int argc,
char *argv[]);
138 static int instance_count();
140 static Mavlink *new_instance();
157 void set_proto_version(
unsigned version);
159 static int destroy_all_instances();
161 static int get_status_all_instances(
bool show_streams_status);
163 static bool serial_instance_exists(
const char *device_name,
Mavlink *
self);
165 static void forward_message(
const mavlink_message_t *
msg,
Mavlink *
self);
167 static int get_uart_fd(
unsigned index);
188 MAVLINK_MODE_NORMAL = 0,
203 BROADCAST_MODE_OFF = 0,
205 BROADCAST_MODE_MULTICAST
209 FLOW_CONTROL_OFF = 0,
217 case MAVLINK_MODE_NORMAL:
220 case MAVLINK_MODE_CUSTOM:
223 case MAVLINK_MODE_ONBOARD:
226 case MAVLINK_MODE_OSD:
229 case MAVLINK_MODE_MAGIC:
232 case MAVLINK_MODE_CONFIG:
235 case MAVLINK_MODE_IRIDIUM:
238 case MAVLINK_MODE_MINIMAL:
241 case MAVLINK_MODE_EXTVISION:
244 case MAVLINK_MODE_EXTVISIONMIN:
245 return "ExtVisionMin";
266 #if defined(MAVLINK_UDP) 267 static Mavlink *get_instance_for_network_port(
unsigned long port);
269 bool broadcast_enabled() {
return _param_mav_broadcast.get() == BROADCAST_MODE_ON; }
270 #endif // MAVLINK_UDP 278 static void set_boot_complete();
285 unsigned get_free_tx_buf();
287 static int start_helper(
int argc,
char *argv[]);
297 int set_hil_enabled(
bool hil_enabled);
331 void send_bytes(
const uint8_t *buf,
unsigned packet_len);
343 void resend_message(mavlink_message_t *msg) { _mavlink_resend_uart(_channel, msg); }
345 void handle_message(
const mavlink_message_t *msg);
367 void configure_stream_threadsafe(
const char *stream_name,
float rate = -1.0
f);
376 void send_statustext_info(
const char *
string);
383 void send_statustext_critical(
const char *
string);
390 void send_statustext_emergency(
const char *
string);
401 void send_statustext(
unsigned char severity,
const char *
string);
406 void send_autopilot_capabilites();
411 void send_protocol_version();
424 bool should_transmit() {
return (_transmitting_enabled && _boot_complete && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
426 bool message_buffer_write(
const void *ptr,
int size);
465 #if defined(MAVLINK_UDP) 466 unsigned short get_network_port() {
return _network_port; }
468 unsigned short get_remote_port() {
return _remote_port; }
470 const in_addr query_netmask_addr(
const int socket_fd,
const ifreq &ifreq);
472 const in_addr compute_broadcast_addr(
const in_addr &host_addr,
const in_addr &netmask_addr);
474 struct sockaddr_in &get_client_source_address() {
return _src_addr; }
476 void set_client_source_initialized() { _src_addr_initialized =
true; }
478 bool get_client_source_initialized() {
return _src_addr_initialized; }
502 if (_mavlink_ulog) {
return; }
508 if (_mavlink_ulog) { _mavlink_ulog_stop_requested =
true; }
512 void set_uorb_main_fd(
int fd,
unsigned int interval);
543 bool _transmitting_enabled{
true};
544 bool _transmitting_enabled_commanded{
false};
545 bool _first_heartbeat_sent{
false};
551 bool _task_running{
true};
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};
558 mavlink_message_t _mavlink_buffer {};
559 mavlink_status_t _mavlink_status {};
562 bool _hil_enabled{
false};
563 bool _generate_rc{
false};
564 bool _is_usb_uart{
false};
565 bool _wait_to_transmit{
false};
566 bool _received_messages{
false};
568 unsigned _main_loop_delay{1000};
576 volatile bool _mavlink_ulog_stop_requested{
false};
580 mavlink_channel_t _channel{MAVLINK_COMM_0};
584 pthread_t _receive_thread {};
586 bool _forwarding_on{
false};
591 int _baudrate{57600};
593 float _rate_mult{1.0f};
595 bool _radio_status_available{
false};
596 bool _radio_status_critical{
false};
597 float _radio_status_mult{1.0f};
604 unsigned int _mavlink_param_queue_index{0};
606 bool _mavlink_link_termination_allowed{
false};
608 char *_subscribe_to_stream{
nullptr};
609 float _subscribe_to_stream_rate{0.0f};
610 bool _udp_initialised{
false};
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};
620 unsigned _bytes_tx{0};
621 unsigned _bytes_txerr{0};
622 unsigned _bytes_rx{0};
623 uint64_t _bytes_timestamp{0};
625 #if defined(MAVLINK_UDP) 626 sockaddr_in _myaddr {};
627 sockaddr_in _src_addr {};
628 sockaddr_in _bcast_addr {};
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};
637 unsigned short _network_port{14556};
638 unsigned short _remote_port{DEFAULT_REMOTE_PORT_UDP};
639 #endif // MAVLINK_UDP 641 const char *_interface_name{
nullptr};
660 pthread_mutex_t _message_buffer_mutex {};
661 pthread_mutex_t _send_mutex {};
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,
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
683 void mavlink_update_parameters();
687 const bool force_flow_control =
false);
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;
701 int configure_stream(
const char *stream_name,
const float rate = -1.0
f);
709 int configure_streams_to_default(
const char *configure_single_stream =
nullptr);
711 int message_buffer_init(
int size);
713 void message_buffer_destroy();
715 int message_buffer_count();
719 int message_buffer_get_ptr(
void **ptr,
bool *is_part);
723 void pass_message(
const mavlink_message_t *msg);
725 void publish_telemetry_status();
727 void check_requested_subscriptions();
735 void check_radio_config();
740 void update_rate_mult();
742 #if defined(MAVLINK_UDP) 743 void find_broadcast_address();
746 #endif // MAVLINK_UDP 751 void set_instance_id();
volatile bool _task_should_exit
bool get_has_received_messages()
static char _protocol[64]
void begin_send()
This is the beginning of a MAVLINK_START_UART_SEND/MAVLINK_END_UART_SEND transaction.
static bool boot_complete()
bool forward_heartbeats_enabled() const
bool get_forward_externalsp()
measure the time elapsed performing an event
void set_manual_input_mode_generation(bool generation_enabled)
Set manual input generation mode.
MAVLink 1.0 message formatters definition.
List< MavlinkOrbSubscription * > _subscriptions
bool get_flow_control_enabled()
static const char * mavlink_mode_str(enum MAVLINK_MODE mode)
void message_buffer_mark_read(int n)
void unlockMessageBufferMutex(void)
int send_packet(int uart_fd, EscPacket &packet, int responder)
Sends a packet to all ESCs and requests a specific ESC to respond.
void lockMessageBufferMutex(void)
void set_data_rate(int rate)
An intrusive linked list.
List< MavlinkStream * > _streams
int message_buffer_is_empty() const
void count_rxbytes(unsigned n)
Count received bytes.
static hrt_abstime & get_first_start_time()
mavlink_status_t * get_status()
uint64_t get_start_time()
ringbuffer::RingBuffer * get_logbuffer()
bool get_manual_input_mode_generation()
Get the manual input generation mode.
int enable_flow_control(int uart_fd, bool enabled)
Enables/disables flow control for open UART device.
Global flash based parameter store.
#define DEFAULT_DEVICE_NAME
Protocol get_protocol() const
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
bool hash_check_enabled() const
mavlink_system_t mavlink_system
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
static MavlinkULog * try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
try to start a new stream.
DEFINE_PARAMETERS((ParamInt< px4::params::MAV_SYS_ID >) _param_mav_sys_id,(ParamInt< px4::params::MAV_COMP_ID >) _param_mav_comp_id,(ParamInt< px4::params::MAV_PROTO_VER >) _param_mav_proto_ver,(ParamInt< px4::params::MAV_RADIO_ID >) _param_mav_radio_id,(ParamInt< px4::params::MAV_TYPE >) _param_mav_type,(ParamBool< px4::params::MAV_USEHILGPS >) _param_mav_usehilgps,(ParamBool< px4::params::MAV_FWDEXTSP >) _param_mav_fwdextsp,(ParamBool< px4::params::MAV_HASH_CHK_EN >) _param_mav_hash_chk_en,(ParamBool< px4::params::MAV_HB_FORW_EN >) _param_mav_hb_forw_en,(ParamBool< px4::params::MAV_ODOM_LP >) _param_mav_odom_lp,(ParamInt< px4::params::SYS_HITL >) _param_sys_hitl) perf_counter_t _loop_perf
void request_stop_ulog_streaming()
orb_advert_t * get_mavlink_log_pub()
static hrt_abstime _first_start_time
static bool _boot_complete
bool odometry_loopback_enabled() const
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
int get_instance_id() const
float get_rate_mult() const
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Mavlink commands sender with support for retransmission.
void task_main(int argc, char *argv[])
mavlink_channel_t get_channel() const
struct ping_statistics_s & get_ping_statistics()
Get the ping statistics of this MAVLink link.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
mavlink_message_t * get_buffer()
void set_protocol(Protocol p)
Set communication protocol for this mavlink instance.
MavlinkULog * get_ulog_streaming()
get ulog streaming if active, nullptr otherwise
telemetry_status_s & get_telemetry_status()
Get the receive status of this MAVLink link.
uORB subscription definition.
void resend_message(mavlink_message_t *msg)
Resend message as is, don't change sequence number and CRC.
bool get_wait_to_transmit()
A shell to be used via MAVLink.
void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component)
ULog data streaming via MAVLink.
int get_system_id() const
Get the MAVLink system id.
List< MavlinkStream * > & get_streams()
void set_wait_to_transmit(bool wait)
unsigned get_system_type()
#define DEFAULT_BAUD_RATE
unsigned get_main_loop_delay() const
int get_component_id() const
Get the MAVLink component id.
void count_txbytes(unsigned n)
Count transmitted bytes.
measure the interval between instances of an event
void set_telemetry_status_type(uint8_t type)
void count_txerrbytes(unsigned n)
Count bytes not transmitted because of errors.
Performance measuring tools.
void set_has_received_messages(bool received_messages)