diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 356f72c84c..efc2e12f90 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -37,8 +37,6 @@ const MAV_MISSION_TYPE GCS_MAVLINK::supported_mission_types[] = { */ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) { - char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; - hal.util->vsnprintf(text, sizeof(text), fmt, arg_list); uint8_t mask = GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(); if (!update_send_has_been_called) { // we have not yet initialised the streaming-channel-mask, @@ -46,7 +44,7 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) // it to all channels: mask = (1<<_num_gcs)-1; } - send_statustext(severity, mask, text); + send_textv(severity, fmt, arg_list, mask); } void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fbc7e476e6..5c345748db 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -838,8 +838,8 @@ public: void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4); 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 void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t mask); + 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 @@ -938,6 +938,7 @@ private: void update_sensor_status_flags(); + void service_statustext(void); #if HAL_MEM_CLASS <= HAL_MEM_CLASS_192 || CONFIG_HAL_BOARD == HAL_BOARD_SITL static const uint8_t _status_capacity = 5; #else diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 500ba3dd5d..14aaf78cae 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -600,12 +600,10 @@ void GCS_MAVLINK::handle_param_value(const mavlink_message_t &msg) void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...) const { - char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; va_list arg_list; va_start(arg_list, fmt); - hal.util->vsnprintf(text, sizeof(text), fmt, arg_list); + gcs().send_textv(severity, fmt, arg_list, (1<vsnprintf(text, sizeof(text), fmt, arg_list) > MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - if (strlen(text) > MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN) { AP_HAL::panic("Statustext (%s) too long", text); - } #endif + } AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 8322f4f157..057c3f6b79 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -85,7 +85,10 @@ private: 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); } + void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t dest_bitmask) override { + hal.console->printf("TOGCS: "); + hal.console->vprintf(fmt, arg_list); + } MAV_TYPE frame_type() const override { return MAV_TYPE_FIXED_WING; } uint32_t custom_mode() const override { return 3; } // magic number