dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MAVLink: Update complete app to support MAVLink 1 & MAVLink 2. Add MAV_PROTO_VER param to switch between them

This commit is contained in:
Lorenz Meier 2016-05-14 15:17:32 +02:00
parent acb2e52389
commit 84800dfd87
5 changed files with 70 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/mavlink/mavlink.c
View File

@ -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];
}

42
src/modules/mavlink/mavlink_main.cpp
View File

@ -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()) {

21
src/modules/mavlink/mavlink_main.h
View File

@ -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;

9
src/modules/mavlink/mavlink_params.c
View File

@ -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
*

4
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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);