aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: handle difference between SERIALn_PROTOCOL=1 and 2

Browse Source
This commit is contained in:
Andrew Tridgell 2016-05-16 14:05:09 +10:00
parent 78e9ef427e
commit 018b1143d2
3 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/GCS_MAVLink/GCS.h
View File

@ -313,7 +313,9 @@ private:
// pointer to static dataflash for logging of text messages
static DataFlash_Class *dataflash_p;
static const AP_SerialManager *serialmanager_p;
// a vehicle can optionally snoop on messages for other systems
static void (*msg_snoop)(const mavlink_message_t* msg);

35
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -58,8 +58,10 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
void
GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance)
{
// search for serial port
serialmanager_p = &serial_manager;
// search for serial port
AP_HAL::UARTDriver *uart;
uart = serial_manager.find_serial(protocol, instance);
if (uart == NULL) {
@ -101,17 +103,30 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager
// and init the gcs instance
init(uart, mav_chan);
// load signing key
load_signing_key();
AP_SerialManager::SerialProtocol mavlink_protocol = serialmanager_p->get_mavlink_protocol(mav_chan);
mavlink_status_t *status = mavlink_get_channel_status(chan);
if ((status && status->signing == NULL) ||
chan == MAVLINK_COMM_0) {
// if signing is off start by sending MAVLink1.
if (status == nullptr) {
return;
}
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) {
// load signing key
load_signing_key();
if (status->signing == NULL) {
// if signing is off start by sending MAVLink1.
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
} else if (status) {
// user has asked to only send MAVLink1
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
if (chan == MAVLINK_COMM_0) {
// Always start with MAVLink1 on first port for now, to allow for recovery
// after experiments with MAVLink2
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
}
}
/**
@ -939,7 +954,9 @@ GCS_MAVLINK::update(run_cli_fn run_cli)
mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
}
if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
serialmanager_p &&
serialmanager_p->get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) {
// if we receive any MAVLink2 packets on a connection
// currently sending MAVLink1 then switch to sending
// MAVLink2

3
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -45,6 +45,9 @@ MAVLink_routing GCS_MAVLINK::routing;
// static dataflash pointer to support logging text messages
DataFlash_Class *GCS_MAVLINK::dataflash_p;
// static AP_SerialManager pointer
const AP_SerialManager *GCS_MAVLINK::serialmanager_p;
// snoop function for vehicle types that want to see messages for
// other targets
void (*GCS_MAVLINK::msg_snoop)(const mavlink_message_t* msg) = NULL;