Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: avoid allocating a GCS_MAVLINK per mavlink channel

Browse Source
This commit is contained in:
Peter Barker 2018-12-10 18:56:32 +11:00 committed by Peter Barker
parent b070945319
commit 74670a77be
6 changed files with 168 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/GCS_MAVLink/GCS.cpp
View File

@ -52,10 +52,7 @@ void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
return;
}
for (uint8_t i=0; i<num_gcs(); i++) {
GCS_MAVLINK &c = chan(i);
if (!c.initialised) {
continue;
}
GCS_MAVLINK &c = *chan(i);
if (!c.is_active()) {
continue;
}
@ -89,12 +86,12 @@ bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protoco
if (c >= num_gcs()) {
return false;
}
if (chan(c).alternative.handler && handler) {
if (chan(c)->alternative.handler && handler) {
// already have one installed - we may need to add support for
// multiple alternative handlers
return false;
}
chan(c).alternative.handler = handler;
chan(c)->alternative.handler = handler;
return true;
}

58
libraries/GCS_MAVLink/GCS.h
View File

@ -37,6 +37,19 @@
}
#define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
#define GCS_MAVLINK_NUM_STREAM_RATES 10
class GCS_MAVLINK_Parameters
{
public:
GCS_MAVLINK_Parameters();
static const struct AP_Param::GroupInfo var_info[];
// saveable rate of each stream
AP_Int16 streamRates[GCS_MAVLINK_NUM_STREAM_RATES];
};
///
/// @class GCS_MAVLINK
/// @brief MAVLink transport control class
@ -45,11 +58,13 @@ class GCS_MAVLINK
{
public:
friend class GCS;
GCS_MAVLINK();
GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters, AP_HAL::UARTDriver &uart);
virtual ~GCS_MAVLINK() {}
void update_receive(uint32_t max_time_us=1000);
void update_send();
void init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan);
void setup_uart(uint8_t instance);
bool init(uint8_t instance);
void send_message(enum ap_message id);
void send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4);
void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) const;
@ -101,11 +116,6 @@ public:
virtual uint8_t sysid_my_gcs() const = 0;
virtual bool sysid_enforce() const { return false; }
static const struct AP_Param::GroupInfo var_info[];
// set to true if this GCS link is active
bool initialised;
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
@ -123,6 +133,12 @@ public:
NUM_STREAMS
};
// streams must be moved out into the top level for
// GCS_MAVLINK_Parameters to be able to use it. This is an
// extensive change, so we 'll just keep them in sync with a
// static assert for now:
static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates");
bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
// return true if this channel has hardware flow control
bool have_flow_control();
@ -280,7 +296,7 @@ protected:
uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); }
// saveable rate of each stream
AP_Int16 streamRates[NUM_STREAMS];
AP_Int16 *streamRates;
virtual bool persist_streamrates() const { return false; }
void handle_request_data_stream(const mavlink_message_t &msg);
@ -701,9 +717,10 @@ public:
void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
void service_statustext(void);
virtual GCS_MAVLINK &chan(const uint8_t ofs) = 0;
virtual const GCS_MAVLINK &chan(const uint8_t ofs) const = 0;
virtual uint8_t num_gcs() const = 0;
virtual GCS_MAVLINK *chan(const uint8_t ofs) = 0;
virtual const GCS_MAVLINK *chan(const uint8_t ofs) const = 0;
// return the number of valid GCS objects
uint8_t num_gcs() const { return _num_gcs; };
void send_message(enum ap_message id);
void send_mission_item_reached_message(uint16_t mission_index);
void send_named_float(const char *name, float value) const;
@ -719,7 +736,6 @@ public:
void update_send();
void update_receive();
virtual void setup_uarts();
// minimum amount of time (in microseconds) that must remain in
// the main scheduler loop before we are allowed to send any
@ -728,6 +744,10 @@ public:
virtual uint16_t min_loop_time_remaining_for_message_send_us() const {
return 200;
}
void setup_console();
void setup_uarts();
bool out_of_time() const;
// frsky backend
@ -740,7 +760,7 @@ public:
bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler);
// get the VFR_HUD throttle
int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0).vfr_hud_throttle():0; }
int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0)->vfr_hud_throttle():0; }
// update uart pass-thru
void update_passthru();
@ -753,16 +773,26 @@ public:
protected:
virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart) = 0;
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
void update_sensor_status_flags();
virtual void update_vehicle_sensor_status_flags() {}
GCS_MAVLINK_Parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS];
uint8_t _num_gcs;
GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS];
private:
static GCS *_singleton;
void create_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart);
struct statustext_t {
uint8_t bitmask;
mavlink_statustext_t msg;

149
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -76,55 +76,29 @@ uint8_t GCS_MAVLINK::mavlink_private = 0;
GCS *GCS::_singleton = nullptr;
GCS_MAVLINK::GCS_MAVLINK()
GCS_MAVLINK::GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters,
AP_HAL::UARTDriver &uart)
{
AP_Param::setup_object_defaults(this, var_info);
_port = &uart;
streamRates = parameters.streamRates;
}
void
GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
{
if (!valid_channel(mav_chan)) {
return;
}
_port = port;
chan = mav_chan;
mavlink_comm_port[chan] = _port;
_queued_parameter = nullptr;
snprintf(_perf_packet_name, sizeof(_perf_packet_name), "GCS_Packet_%u", chan);
_perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_packet_name);
snprintf(_perf_update_name, sizeof(_perf_update_name), "GCS_Update_%u", chan);
_perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_update_name);
initialised = true;
}
/*
setup a UART, handling begin() and init()
*/
void
GCS_MAVLINK::setup_uart(uint8_t instance)
bool GCS_MAVLINK::init(uint8_t instance)
{
// search for serial port
const AP_SerialManager& serial_manager = AP::serialmanager();
const AP_SerialManager::SerialProtocol protocol = AP_SerialManager::SerialProtocol_MAVLink;
AP_HAL::UARTDriver *uart = serial_manager.find_serial(protocol, instance);
if (uart == nullptr) {
// return immediately if not found
return;
}
// get associated mavlink channel
mavlink_channel_t mav_chan;
if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)) {
if (!serial_manager.get_mavlink_channel(protocol, instance, chan)) {
// return immediately in unlikely case mavlink channel cannot be found
return;
return false;
}
// and init the gcs instance
if (!valid_channel(chan)) {
return false;
}
/*
@ -135,29 +109,35 @@ GCS_MAVLINK::setup_uart(uint8_t instance)
0x20 at 115200 on startup, which tells the bootloader to reset
and boot normally
*/
uart->begin(115200);
AP_HAL::UARTDriver::flow_control old_flow_control = uart->get_flow_control();
uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
_port->begin(115200);
AP_HAL::UARTDriver::flow_control old_flow_control = _port->get_flow_control();
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
for (uint8_t i=0; i<3; i++) {
hal.scheduler->delay(1);
uart->write(0x30);
uart->write(0x20);
_port->write(0x30);
_port->write(0x20);
}
// since tcdrain() and TCSADRAIN may not be implemented...
hal.scheduler->delay(1);
uart->set_flow_control(old_flow_control);
_port->set_flow_control(old_flow_control);
// now change back to desired baudrate
uart->begin(serial_manager.find_baudrate(protocol, instance));
_port->begin(serial_manager.find_baudrate(protocol, instance));
// and init the gcs instance
init(uart, mav_chan);
mavlink_comm_port[chan] = _port;
AP_SerialManager::SerialProtocol mavlink_protocol = AP::serialmanager().get_mavlink_protocol(mav_chan);
// create performance counters
snprintf(_perf_packet_name, sizeof(_perf_packet_name), "GCS_Packet_%u", chan);
_perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_packet_name);
snprintf(_perf_update_name, sizeof(_perf_update_name), "GCS_Update_%u", chan);
_perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_update_name);
AP_SerialManager::SerialProtocol mavlink_protocol = serial_manager.get_mavlink_protocol(chan);
mavlink_status_t *status = mavlink_get_channel_status(chan);
if (status == nullptr) {
return;
return false;
}
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) {
@ -178,6 +158,8 @@ GCS_MAVLINK::setup_uart(uint8_t instance)
// after experiments with MAVLink2
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
return true;
}
void GCS_MAVLINK::send_meminfo(void)
@ -1845,9 +1827,7 @@ void GCS::service_statustext(void)
void GCS::send_message(enum ap_message id)
{
for (uint8_t i=0; i<num_gcs(); i++) {
if (chan(i).initialised) {
chan(i).send_message(id);
}
chan(i)->send_message(id);
}
}
@ -1872,9 +1852,7 @@ void GCS::update_send()
_missionitemprotocol_rally->update();
}
for (uint8_t i=0; i<num_gcs(); i++) {
if (chan(i).initialised) {
chan(i).update_send();
}
chan(i)->update_send();
}
WITH_SEMAPHORE(_statustext_sem);
service_statustext();
@ -1883,9 +1861,7 @@ void GCS::update_send()
void GCS::update_receive(void)
{
for (uint8_t i=0; i<num_gcs(); i++) {
if (chan(i).initialised) {
chan(i).update_receive();
}
chan(i)->update_receive();
}
// also update UART pass-thru, if enabled
update_passthru();
@ -1894,17 +1870,62 @@ void GCS::update_receive(void)
void GCS::send_mission_item_reached_message(uint16_t mission_index)
{
for (uint8_t i=0; i<num_gcs(); i++) {
if (chan(i).initialised) {
chan(i).mission_item_reached_index = mission_index;
chan(i).send_message(MSG_MISSION_ITEM_REACHED);
}
chan(i)->mission_item_reached_index = mission_index;
chan(i)->send_message(MSG_MISSION_ITEM_REACHED);
}
}
void GCS::setup_console()
{
AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, 0);
if (uart == nullptr) {
// this is probably not going to end well.
return;
}
if (ARRAY_SIZE(chan_parameters) == 0) {
return;
}
create_gcs_mavlink_backend(chan_parameters[0], *uart);
}
GCS_MAVLINK_Parameters::GCS_MAVLINK_Parameters()
{
AP_Param::setup_object_defaults(this, var_info);
}
void GCS::create_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params, AP_HAL::UARTDriver &uart)
{
if (_num_gcs >= ARRAY_SIZE(chan_parameters)) {
return;
}
_chan[_num_gcs] = new_gcs_mavlink_backend(params, uart);
if (_chan[_num_gcs] == nullptr) {
return;
}
if (!_chan[_num_gcs]->init(_num_gcs)) {
delete _chan[_num_gcs];
_chan[_num_gcs] = nullptr;
return;
}
_num_gcs++;
}
void GCS::setup_uarts()
{
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
chan(i).setup_uart(i);
if (i >= ARRAY_SIZE(chan_parameters)) {
// should not happen
break;
}
AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, i);
if (uart == nullptr) {
// no more mavlink uarts
break;
}
create_gcs_mavlink_backend(chan_parameters[i], *uart);
}
if (frsky == nullptr) {

36
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -23,6 +23,12 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {};
*/
class GCS_MAVLINK_Dummy : public GCS_MAVLINK
{
public:
using GCS_MAVLINK::GCS_MAVLINK;
private:
uint32_t telem_delay() const override { return 0; }
void handleMessage(const mavlink_message_t &msg) override {}
bool try_send_message(enum ap_message id) override { return true; }
@ -53,10 +59,32 @@ extern const AP_HAL::HAL& hal;
class GCS_Dummy : public GCS
{
GCS_MAVLINK_Dummy dummy_backend;
uint8_t num_gcs() const override { return 1; }
GCS_MAVLINK_Dummy &chan(const uint8_t ofs) override { return dummy_backend; }
const GCS_MAVLINK_Dummy &chan(const uint8_t ofs) const override { return dummy_backend; };
public:
using GCS::GCS;
protected:
GCS_MAVLINK_Dummy *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
AP_HAL::UARTDriver &uart) override {
return new GCS_MAVLINK_Dummy(params, uart);
}
private:
GCS_MAVLINK_Dummy *chan(const uint8_t ofs) override {
if (ofs > _num_gcs) {
AP::internalerror().error(AP_InternalError::error_t::gcs_offset);
return nullptr;
}
return (GCS_MAVLINK_Dummy *)_chan[ofs];
};
const GCS_MAVLINK_Dummy *chan(const uint8_t ofs) const override {
if (ofs > _num_gcs) {
AP::internalerror().error(AP_InternalError::error_t::gcs_offset);
return nullptr;
}
return (GCS_MAVLINK_Dummy *)_chan[ofs];
};
void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) override { hal.console->printf("TOGCS: %s\n", text); }

4
libraries/GCS_MAVLink/GCS_Param.cpp
View File

@ -36,10 +36,6 @@ bool GCS_MAVLINK::param_timer_registered;
void
GCS_MAVLINK::queued_param_send()
{
if (!initialised) {
return;
}
// send parameter async replies
uint8_t async_replies_sent_count = send_parameter_async_replies();

6
libraries/GCS_MAVLink/examples/routing/routing.cpp
View File

@ -7,17 +7,19 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS_Dummy.h>
#include <AP_Common/AP_FWVersion.h>
#include <AP_SerialManager/AP_SerialManager.h>
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
AP_SerialManager _serialmanager;
GCS_Dummy _gcs;
extern mavlink_system_t mavlink_system;
const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
AP_GROUPEND
};
@ -26,7 +28,7 @@ static MAVLink_routing routing;
void setup(void)
{
hal.console->printf("routing test startup...");
gcs().chan(0).init(hal.uartA, MAVLINK_COMM_0);
gcs().setup_console();
}
void loop(void)