From cc28ff49e9e5b141985fe75d1600a2a8c4f78c0f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 30 May 2016 20:05:00 +1000 Subject: [PATCH] GCS_MAVLink: support for a singleton --- libraries/GCS_MAVLink/GCS.h | 104 ++++++++++++------ libraries/GCS_MAVLink/GCS_Common.cpp | 25 +++-- libraries/GCS_MAVLink/GCS_MAVLink.cpp | 6 - .../GCS_MAVLink/examples/routing/routing.cpp | 6 +- 4 files changed, 90 insertions(+), 51 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 754d0fc082..454aa4f8d1 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -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_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_queue{_status_capacity}; + +}; + +GCS &gcs(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 700bfb4e45..2ec94cbd93 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -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_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<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; idxprintf("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)