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

GCS_MAVLink: support for a singleton

Browse Source
This commit is contained in:
Peter Barker 2016-05-30 20:05:00 +10:00 committed by Andrew Tridgell
parent 29b16dbafd
commit cc28ff49e9
4 changed files with 90 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
libraries/GCS_MAVLink/GCS.h
View File

@ -22,12 +22,6 @@
#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
#if HAL_CPU_CLASS <= HAL_CPU_CLASS_150 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY 5
#else
#define GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY 30
#endif
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
@ -79,7 +73,6 @@ enum ap_message {
MSG_RETRY_DEFERRED // this must be last
};
///
/// @class GCS_MAVLINK
/// @brief MAVLink transport control class
@ -104,13 +97,6 @@ public:
virtual void packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg);
struct statustext_t {
uint8_t bitmask;
mavlink_statustext_t msg;
};
static ObjectArray<statustext_t> _statustext_queue;
// accessor for uart
AP_HAL::UARTDriver *get_uart() { return _port; }
@ -177,14 +163,15 @@ public:
// over active channels to send to all active channels
static uint8_t active_channel_mask(void) { return mavlink_active; }
// return a bitmap of streaming channels
static uint8_t streaming_channel_mask(void) { return chan_is_streaming; }
/*
send a statustext message to active MAVLink connections, or a specific
one. This function is static so it can be called from any library.
*/
static void send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...);
static void send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan, const char *fmt, ...);
static void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
static void service_statustext(void);
// send a PARAM_VALUE message to all active MAVLink connections.
static void send_parameter_value_all(const char *param_name, ap_var_type param_type, float param_value);
@ -206,20 +193,6 @@ public:
*/
static bool find_by_mavtype(uint8_t mav_type, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel) { return routing.find_by_mavtype(mav_type, sysid, compid, channel); }
/*
set a dataflash pointer for logging
*/
static void set_dataflash(DataFlash_Class *dataflash) {
dataflash_p = dataflash;
}
/*
set a frsky_telem pointer for queueing
*/
static void register_frsky_telemetry_callback(AP_Frsky_Telem *frsky_telemetry) {
frsky_telemetry_p = frsky_telemetry;
}
// update signing timestamp on GPS lock
static void update_signing_timestamp(uint64_t timestamp_usec);
@ -381,9 +354,6 @@ private:
// mavlink routing object
static MAVLink_routing routing;
// pointer to static dataflash for logging of text messages
static DataFlash_Class *dataflash_p;
// pointer to static frsky_telem for queueing of text messages
static AP_Frsky_Telem *frsky_telemetry_p;
@ -422,3 +392,71 @@ private:
bool signing_enabled(void) const;
static void save_signing_timestamp(bool force_save_now);
};
/// @class GCS
/// @brief global GCS object
class GCS
{
public:
GCS() {
if (_singleton == nullptr) {
_singleton = this;
} else {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// this is a serious problem, but we don't need to kill a
// real vehicle
AP_HAL::panic("GCS must be singleton");
#endif
}
};
static class GCS *instance() {
return _singleton;
}
void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
void service_statustext(void);
/*
set a dataflash pointer for logging
*/
void set_dataflash(DataFlash_Class *dataflash) {
dataflash_p = dataflash;
}
// pointer to static dataflash for logging of text messages
DataFlash_Class *dataflash_p;
/*
set a frsky_telem pointer for queueing
*/
void register_frsky_telemetry_callback(AP_Frsky_Telem *frsky_telemetry) {
frsky_telemetry_p = frsky_telemetry;
}
// static frsky_telem pointer to support queueing text messages
AP_Frsky_Telem *frsky_telemetry_p;
private:
static GCS *_singleton;
struct statustext_t {
uint8_t bitmask;
mavlink_statustext_t msg;
};
#if HAL_CPU_CLASS <= HAL_CPU_CLASS_150 || CONFIG_HAL_BOARD == HAL_BOARD_SITL
static const uint8_t _status_capacity = 5;
#else
static const uint8_t _status_capacity = 30;
#endif
ObjectArray<statustext_t> _statustext_queue{_status_capacity};
};
GCS &gcs();

25
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -34,9 +34,10 @@ extern const AP_HAL::HAL& hal;
uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
uint8_t GCS_MAVLINK::mavlink_active = 0;
uint8_t GCS_MAVLINK::chan_is_streaming = 0;
ObjectArray<GCS_MAVLINK::statustext_t> GCS_MAVLINK::_statustext_queue(GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY);
uint32_t GCS_MAVLINK::reserve_param_space_start_ms;
GCS *GCS::_singleton = nullptr;
GCS_MAVLINK::GCS_MAVLINK()
{
AP_Param::setup_object_defaults(this, var_info);
@ -1326,7 +1327,7 @@ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ..
hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list);
va_end(arg_list);
text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = 0;
send_statustext(severity, mavlink_active | chan_is_streaming, text);
gcs().send_statustext(severity, mavlink_active | chan_is_streaming, text);
}
/*
@ -1339,14 +1340,14 @@ void GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan,
va_start(arg_list, fmt);
hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list);
va_end(arg_list);
send_statustext(severity, (1<<dest_chan), text);
gcs().send_statustext(severity, (1<<dest_chan), text);
}
/*
send a statustext text string to specific MAVLink bitmask
*/
void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text)
void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text)
{
if (dataflash_p != nullptr) {
dataflash_p->Log_Write_Message(text);
@ -1359,7 +1360,7 @@ void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, c
// filter destination ports to only allow active ports.
statustext_t statustext{};
statustext.bitmask = (mavlink_active | chan_is_streaming) & dest_bitmask;
statustext.bitmask = (GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask() ) & dest_bitmask;
if (!statustext.bitmask) {
// nowhere to send
return;
@ -1376,17 +1377,18 @@ void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, c
// try and send immediately if possible
service_statustext();
}
/*
send a statustext message to specific MAVLink connections in a bitmask
*/
void GCS_MAVLINK::service_statustext(void)
void GCS::service_statustext(void)
{
// create bitmask of what mavlink ports we should send this text to.
// note, if sending to all ports, we only need to store the bitmask for each and the string only once.
// once we send over a link, clear the port but other busy ports bit may stay allowing for faster links
// to clear the bit and send quickly but slower links to still store the string. Regardless of mixed
// bitrates of ports, a maximum of GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY strings can be buffered. Downside
// is if you have a super slow link mixed with a faster port, if there are GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY
// bitrates of ports, a maximum of _status_capacity strings can be buffered. Downside
// is if you have a super slow link mixed with a faster port, if there are _status_capacity
// strings in the slow queue then the next item can not be queued for the faster link
if (_statustext_queue.empty()) {
@ -1394,7 +1396,7 @@ void GCS_MAVLINK::service_statustext(void)
return;
}
for (uint8_t idx=0; idx<GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY; ) {
for (uint8_t idx=0; idx<_status_capacity; ) {
statustext_t *statustext = _statustext_queue[idx];
if (statustext == nullptr) {
break;
@ -1847,3 +1849,8 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
break;
}
}
GCS &gcs()
{
return *GCS::instance();
}

6
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -46,12 +46,6 @@ static uint8_t mavlink_locked_mask;
// routing table
MAVLink_routing GCS_MAVLINK::routing;
// static dataflash pointer to support logging text messages
DataFlash_Class *GCS_MAVLINK::dataflash_p;
// static frsky_telem pointer to support queueing text messages
AP_Frsky_Telem *GCS_MAVLINK::frsky_telemetry_p;
// static AP_SerialManager pointer
const AP_SerialManager *GCS_MAVLINK::serialmanager_p;

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

@ -30,7 +30,7 @@ private:
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
static GCS_MAVLINK_routing gcs[MAVLINK_COMM_NUM_BUFFERS];
static GCS_MAVLINK_routing gcs_link[MAVLINK_COMM_NUM_BUFFERS];
extern mavlink_system_t mavlink_system;
@ -42,8 +42,8 @@ static MAVLink_routing routing;
void setup(void)
{
hal.console->printf("routing test startup...\n");
gcs[0].init(hal.uartA, MAVLINK_COMM_0);
hal.console->printf("routing test startup...");
gcs_link[0].init(hal.uartA, MAVLINK_COMM_0);
}
void loop(void)