From 84800dfd87d1eddcd973bb1a653d6212f73415e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 May 2016 15:17:32 +0200 Subject: [PATCH] MAVLink: Update complete app to support MAVLink 1 & MAVLink 2. Add MAV_PROTO_VER param to switch between them --- src/modules/mavlink/mavlink.c | 18 ---------- src/modules/mavlink/mavlink_main.cpp | 42 ++++++++++++++++++++---- src/modules/mavlink/mavlink_main.h | 21 ++++++++++++ src/modules/mavlink/mavlink_params.c | 9 +++++ src/modules/mavlink/mavlink_receiver.cpp | 4 +++ 5 files changed, 70 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bb6dc44789..ec4b71bf4d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -50,21 +50,3 @@ mavlink_system_t mavlink_system = { 1, 1 }; // System ID, 1-255, Component/Subsystem ID, 1-255 - -/* - * Internal function to give access to the channel status for each channel - */ -extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[channel]; -} - -/* - * Internal function to give access to the channel buffer for each channel - */ -extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) -{ - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[channel]; -} diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7b1530e2f9..13bb66cba2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -106,8 +106,6 @@ static const int ERROR = -1; static Mavlink *_mavlink_instances = nullptr; -static const mavlink_crc_entry_t mavlink_message_meta[] = MAVLINK_MESSAGE_CRCS; - /** * mavlink app start / stop handling function * @@ -133,10 +131,28 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length) } } +/* + * Internal function to give access to the channel status for each channel + */ +mavlink_status_t *mavlink_get_channel_status(uint8_t channel) +{ + return Mavlink::get_status_for_instance(channel); +} + +/* + * Internal function to give access to the channel buffer for each channel + */ +mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) +{ + return Mavlink::get_buffer_for_instance(channel); +} + static void usage(void); bool Mavlink::_boot_complete = false; bool Mavlink::_config_link_on = false; +mavlink_message_t Mavlink::_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS] = {}; +mavlink_status_t Mavlink::_mavlink_status[MAVLINK_COMM_NUM_BUFFERS] = {}; Mavlink::Mavlink() : _device_name("/dev/ttyS1"), @@ -183,6 +199,7 @@ Mavlink::Mavlink() : _last_write_success_time(0), _last_write_try_time(0), _mavlink_start_time(0), + _protocol_version(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -297,6 +314,18 @@ Mavlink::~Mavlink() } } +void +Mavlink::set_proto_version(unsigned version) +{ + _protocol_version = version; + + if (version == 1 || ((version == 0) && !_received_messages)) { + get_status()->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else if (version == 2) { + get_status()->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + } +} + void Mavlink::count_txerr() { @@ -493,6 +522,7 @@ void Mavlink::mavlink_update_system(void) if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); + _param_proto_ver = param_find("MAV_PROTO_VER"); _param_radio_id = param_find("MAV_RADIO_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); @@ -509,6 +539,10 @@ void Mavlink::mavlink_update_system(void) int32_t component_id; param_get(_param_component_id, &component_id); + int32_t proto; + param_get(_param_proto_ver, &proto); + set_proto_version(proto); + param_get(_param_radio_id, &_radio_id); /* only allow system ID and component ID updates @@ -841,7 +875,6 @@ Mavlink::send_packet() ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); struct telemetry_status_s &tstatus = get_rx_status(); - warnx("UDP count: %d", ret); /* resend message via broadcast if no valid connection exists */ if ((_mode != MAVLINK_MODE_ONBOARD) && @@ -858,8 +891,6 @@ Mavlink::send_packet() if (bret <= 0) { PX4_WARN("sending broadcast failed, errno: %d: %s", errno, strerror(errno)); - } else { - warnx("BROADCAST count: %d", bret); } } } @@ -877,7 +908,6 @@ Mavlink::send_packet() void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) { - warnx("send bytes"); /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (!should_transmit()) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b0b682d5cb..42ee722c20 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -114,6 +114,23 @@ public: static Mavlink *get_instance_for_network_port(unsigned long port); + static mavlink_message_t *get_buffer_for_instance(unsigned instance) { return &_mavlink_buffer[instance]; } + + mavlink_message_t *get_buffer() { return Mavlink::get_buffer_for_instance(_instance_id); } + + static mavlink_status_t *get_status_for_instance(unsigned instance) { return &_mavlink_status[instance]; } + + mavlink_status_t *get_status() { return Mavlink::get_status_for_instance(_instance_id); } + + /** + * Set the MAVLink version + * + * Currently supporting v1 and v2 + * + * @param version MAVLink version + */ + void set_proto_version(unsigned version); + static int destroy_all_instances(); static int get_status_all_instances(); @@ -385,6 +402,8 @@ private: orb_advert_t _mavlink_log_pub; bool _task_running; static bool _boot_complete; + static mavlink_message_t _mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; + static mavlink_status_t _mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ @@ -444,6 +463,7 @@ private: uint64_t _last_write_success_time; uint64_t _last_write_try_time; uint64_t _mavlink_start_time; + int32_t _protocol_version; unsigned _bytes_tx; unsigned _bytes_txerr; @@ -486,6 +506,7 @@ private: param_t _param_system_id; param_t _param_component_id; + param_t _param_proto_ver; param_t _param_radio_id; param_t _param_system_type; param_t _param_use_hil_gps; diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index ad7e9712cf..44964b9840 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -47,6 +47,15 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); */ PARAM_DEFINE_INT32(MAV_COMP_ID, 1); +/** + * MAVLink protocol version + * @group MAVLink + * @value 0 Default to 1, switch to 2 if GCS sends version 2 + * @value 1 Always use version 1 + * @value 2 Always use version 2 + */ +PARAM_DEFINE_INT32(MAV_PROTO_VER, 1); + /** * MAVLink Radio ID * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d31b7e0ad1..f5ec194177 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2084,6 +2084,10 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { + + /* check if we received version 2 */ + // XXX todo _mavlink->set_proto_version(2); + /* handle generic messages and commands */ handle_message(&msg);