From 11b9737b3414e450bd8fe3259b61dbd8d6f39f63 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2019 11:05:49 +1000 Subject: [PATCH] AP_AccelCal: remove wrapper around send_text With our statustext queueing system this check is not just wrong but redundant. --- libraries/AP_AccelCal/AP_AccelCal.cpp | 32 ++++++--------------------- libraries/AP_AccelCal/AP_AccelCal.h | 1 - 2 files changed, 7 insertions(+), 26 deletions(-) diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp index 72bdb2c9c0..19c61141b2 100644 --- a/libraries/AP_AccelCal/AP_AccelCal.cpp +++ b/libraries/AP_AccelCal/AP_AccelCal.cpp @@ -19,6 +19,13 @@ #define AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS 1000 +#define _printf(fmt, args ...) do { \ + if (_gcs != nullptr) { \ + _gcs->send_text(MAV_SEVERITY_CRITICAL, fmt, ## args); \ + } \ + } while (0) + + const extern AP_HAL::HAL& hal; static bool _start_collect_sample; @@ -375,28 +382,3 @@ bool AP_AccelCal::gcs_vehicle_position(float position) return false; } - -void AP_AccelCal::_printf(const char* fmt, ...) -{ - if (!_gcs) { - return; - } - char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; - va_list ap; - va_start(ap, fmt); - hal.util->vsnprintf(msg, sizeof(msg), fmt, ap); - va_end(ap); - - AP_HAL::UARTDriver *uart = _gcs->get_uart(); - /* - * to ensure these messages get to the user we need to wait for the - * port send buffer to have enough room - */ - while (uart->txspace() < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) { - hal.scheduler->delay(1); - } - -#if !APM_BUILD_TYPE(APM_BUILD_Replay) - _gcs->send_text(MAV_SEVERITY_CRITICAL, "%s", msg); -#endif -} diff --git a/libraries/AP_AccelCal/AP_AccelCal.h b/libraries/AP_AccelCal/AP_AccelCal.h index 8914cc3725..4a1ee508da 100644 --- a/libraries/AP_AccelCal/AP_AccelCal.h +++ b/libraries/AP_AccelCal/AP_AccelCal.h @@ -74,7 +74,6 @@ private: uint8_t _num_active_calibrators; AccelCalibrator* get_calibrator(uint8_t i); - void _printf(const char*, ...); }; class AP_AccelCal_Client {