From 5664da2478f93125aca68742c6ee6932155b6dc5 Mon Sep 17 00:00:00 2001 From: Dat Tran Date: Wed, 13 Apr 2022 12:54:28 -0500 Subject: [PATCH 1/5] APM-156: Add more debug messages on compass calibration failures --- .../AP_Compass/AP_Compass_Calibration.cpp | 25 ++++++----- libraries/AP_Compass/CompassCalibrator.cpp | 44 ++++++++++++++----- 2 files changed, 48 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 86b69e127b..661e7cfae5 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -7,6 +7,8 @@ #include "AP_Compass.h" +#define LOG_TEXT_PREFIX "MagCal: " + const extern AP_HAL::HAL& hal; #if COMPASS_CAL_ENABLED @@ -27,6 +29,7 @@ void Compass::cal_update() } if (_calibrator[i].check_for_timeout()) { + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Compass cal timed-out"); AP_Notify::events.compass_cal_failed = 1; cancel_calibration_all(); } @@ -59,12 +62,12 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) } Priority prio = Priority(i); if (_priority_did_list[prio] != _priority_did_stored_list[prio]) { - gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change"); + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Compass cal requires reboot after priority change"); return false; } if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) { - gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock"); + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Compass cal requires GPS lock"); return false; } } @@ -313,7 +316,7 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet) case MAV_CMD_DO_START_MAG_CAL: { result = MAV_RESULT_ACCEPTED; if (hal.util->get_soft_armed()) { - gcs().send_text(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration"); + gcs().send_text(MAV_SEVERITY_NOTICE, LOG_TEXT_PREFIX "Disarm to allow compass calibration"); result = MAV_RESULT_FAILED; break; } @@ -365,19 +368,19 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet) result = MAV_RESULT_FAILED; break; } - + uint8_t mag_mask = packet.param1; - + if (mag_mask == 0) { // 0 means all cancel_calibration_all(); break; } - + _cancel_calibration_mask(mag_mask); break; } } - + return result; } @@ -436,7 +439,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, // get AHRS position. If unavailable then try GPS location if (!AP::ahrs().get_position(loc)) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { - gcs().send_text(MAV_SEVERITY_ERROR, "Mag: no position available"); + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag: no position available"); return MAV_RESULT_FAILED; } loc = AP::gps().location(); @@ -450,7 +453,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float declination; float inclination; if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) { - gcs().send_text(MAV_SEVERITY_ERROR, "Mag: WMM table error"); + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag: WMM table error"); return MAV_RESULT_FAILED; } @@ -475,13 +478,13 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, continue; } if (!healthy(i)) { - gcs().send_text(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i); + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag[%u]: unhealthy\n", i); return MAV_RESULT_FAILED; } Vector3f measurement; if (!get_uncorrected_field(i, measurement)) { - gcs().send_text(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i); + gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag[%u]: bad uncorrected field", i); return MAV_RESULT_FAILED; } diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index a403d7d5b5..cfda2114d5 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -67,6 +67,8 @@ #define FIELD_RADIUS_MIN 150 #define FIELD_RADIUS_MAX 950 +#define LOG_TEXT_PREFIX "MagCal: " + extern const AP_HAL::HAL& hal; //////////////////////////////////////////////////////////// @@ -206,6 +208,7 @@ void CompassCalibrator::update(bool &failure) if (_status == Status::RUNNING_STEP_ONE) { if (_fit_step >= 10) { if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag(%u) fitness is diverging", _compass_idx); set_status(Status::FAILED); failure = true; } else { @@ -220,9 +223,15 @@ void CompassCalibrator::update(bool &failure) } } else if (_status == Status::RUNNING_STEP_TWO) { if (_fit_step >= 35) { - if (fit_acceptable() && fix_radius() && calculate_orientation()) { + bool is_fit_acceptable = fit_acceptable(); + bool is_fix_radius = fix_radius(); + bool is_calculate_orientation = calculate_orientation(); + + if (is_fit_acceptable && is_fix_radius && is_calculate_orientation) { set_status(Status::SUCCESS); } else { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag(%u) step two failed: fit_accpt(%d), fix_rad(%d), calc_ortn(%d)", _compass_idx, + is_fit_acceptable, is_fix_radius, is_calculate_orientation); set_status(Status::FAILED); failure = true; } @@ -375,6 +384,8 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) bool CompassCalibrator::fit_acceptable() { + bool acceptable = false; + if (!isnan(_fitness) && _params.radius > FIELD_RADIUS_MIN && _params.radius < FIELD_RADIUS_MAX && fabsf(_params.offset.x) < _offset_max && @@ -386,9 +397,14 @@ bool CompassCalibrator::fit_acceptable() fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1 fabsf(_params.offdiag.y) < 1.0f && fabsf(_params.offdiag.z) < 1.0f ) { - return _fitness <= sq(_tolerance); + acceptable = (_fitness <= sq(_tolerance)); + if (!acceptable) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) _fitness (%f) <= sq(_tolerance) (%f)", _compass_idx, _fitness, sq(_tolerance)); + } } - return false; + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) fit_acceptable() is false", _compass_idx); + return acceptable; } void CompassCalibrator::thin_samples() @@ -451,6 +467,7 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample, uint16_t skip_inde if (i != skip_index) { float distance = (sample - _sample_buffer[i].get()).length(); if (distance < min_distance) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) accept_sample() is false", _compass_idx); return false; } } @@ -570,10 +587,12 @@ void CompassCalibrator::run_sphere_fit() } if (!inverse(JTJ, JTJ, 4)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ matrix is singular", _compass_idx); return; } if (!inverse(JTJ2, JTJ2, 4)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ2 matrix is singular", _compass_idx); return; } @@ -609,6 +628,9 @@ void CompassCalibrator::run_sphere_fit() _params = fit1_params; update_completion_mask(); } + else { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) fitness is nan", _compass_idx); + } } void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const @@ -686,10 +708,12 @@ void CompassCalibrator::run_ellipsoid_fit() } if (!inverse(JTJ, JTJ, 9)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ matrix is singular", _compass_idx); return; } if (!inverse(JTJ2, JTJ2, 9)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ2 matrix is singular", _compass_idx); return; } @@ -773,10 +797,10 @@ Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) /* calculate the implied earth field for a compass sample and compass rotation. This is used to check for consistency between - samples. + samples. If the orientation is correct then when rotated the same (or - similar) earth field should be given for all samples. + similar) earth field should be given for all samples. Note that this earth field uses an arbitrary north reference, so it may not match the true earth field. @@ -878,16 +902,16 @@ bool CompassCalibrator::calculate_orientation(void) pass = _orientation_confidence > variance_threshold; } if (!pass) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx, + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, LOG_TEXT_PREFIX "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx, besti, besti2, (double)_orientation_confidence); (void)besti2; } else if (besti == _orientation) { // no orientation change - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_TEXT_PREFIX "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); } else if (!_is_external || !_fix_orientation) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, LOG_TEXT_PREFIX "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_TEXT_PREFIX "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); } if (!pass) { @@ -957,7 +981,7 @@ bool CompassCalibrator::fix_radius(void) if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) { // don't allow more than 30% scale factor correction - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f", + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag(%u) bad radius %.0f expected %.0f", _compass_idx, _params.radius, expected_radius); From 756a2a2d013f4b0b40ef62c12846221dff0b3b56 Mon Sep 17 00:00:00 2001 From: Dat Tran Date: Thu, 14 Apr 2022 18:06:23 -0500 Subject: [PATCH 2/5] APM-156: Defer warning logs until an error occurs --- .../AP_Compass/AP_Compass_Calibration.cpp | 18 +++--- libraries/AP_Compass/CompassCalibrator.cpp | 61 ++++++++++++++----- libraries/AP_Compass/CompassCalibrator.h | 12 ++++ 3 files changed, 65 insertions(+), 26 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 661e7cfae5..5ace458893 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -7,8 +7,6 @@ #include "AP_Compass.h" -#define LOG_TEXT_PREFIX "MagCal: " - const extern AP_HAL::HAL& hal; #if COMPASS_CAL_ENABLED @@ -29,7 +27,7 @@ void Compass::cal_update() } if (_calibrator[i].check_for_timeout()) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Compass cal timed-out"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Compass cal timed-out"); AP_Notify::events.compass_cal_failed = 1; cancel_calibration_all(); } @@ -62,12 +60,12 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) } Priority prio = Priority(i); if (_priority_did_list[prio] != _priority_did_stored_list[prio]) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Compass cal requires reboot after priority change"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Compass cal requires reboot after priority change"); return false; } if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Compass cal requires GPS lock"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Compass cal requires GPS lock"); return false; } } @@ -316,7 +314,7 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet) case MAV_CMD_DO_START_MAG_CAL: { result = MAV_RESULT_ACCEPTED; if (hal.util->get_soft_armed()) { - gcs().send_text(MAV_SEVERITY_NOTICE, LOG_TEXT_PREFIX "Disarm to allow compass calibration"); + gcs().send_text(MAV_SEVERITY_NOTICE, COMPASS_CAL_LOG_TEXT_PREFIX "Disarm to allow compass calibration"); result = MAV_RESULT_FAILED; break; } @@ -439,7 +437,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, // get AHRS position. If unavailable then try GPS location if (!AP::ahrs().get_position(loc)) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag: no position available"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag: no position available"); return MAV_RESULT_FAILED; } loc = AP::gps().location(); @@ -453,7 +451,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float declination; float inclination; if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag: WMM table error"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag: WMM table error"); return MAV_RESULT_FAILED; } @@ -478,13 +476,13 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, continue; } if (!healthy(i)) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag[%u]: unhealthy\n", i); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag[%u]: unhealthy\n", i); return MAV_RESULT_FAILED; } Vector3f measurement; if (!get_uncorrected_field(i, measurement)) { - gcs().send_text(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag[%u]: bad uncorrected field", i); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag[%u]: bad uncorrected field", i); return MAV_RESULT_FAILED; } diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index cfda2114d5..53840dfa5c 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -67,7 +67,7 @@ #define FIELD_RADIUS_MIN 150 #define FIELD_RADIUS_MAX 950 -#define LOG_TEXT_PREFIX "MagCal: " +#define DEFERRED_LOG_QUEUE_SIZE 5 extern const AP_HAL::HAL& hal; @@ -76,6 +76,7 @@ extern const AP_HAL::HAL& hal; //////////////////////////////////////////////////////////// CompassCalibrator::CompassCalibrator() + : _deferred_logs(DEFERRED_LOG_QUEUE_SIZE) { stop(); } @@ -208,7 +209,7 @@ void CompassCalibrator::update(bool &failure) if (_status == Status::RUNNING_STEP_ONE) { if (_fit_step >= 10) { if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag(%u) fitness is diverging", _compass_idx); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) fitness is diverging", _compass_idx); set_status(Status::FAILED); failure = true; } else { @@ -230,7 +231,7 @@ void CompassCalibrator::update(bool &failure) if (is_fit_acceptable && is_fix_radius && is_calculate_orientation) { set_status(Status::SUCCESS); } else { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag(%u) step two failed: fit_accpt(%d), fix_rad(%d), calc_ortn(%d)", _compass_idx, + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) step two failed: fit_accpt(%d), fix_rad(%d), calc_ortn(%d)", _compass_idx, is_fit_acceptable, is_fix_radius, is_calculate_orientation); set_status(Status::FAILED); failure = true; @@ -375,6 +376,16 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) } _status = status; + + // Write all warning text that were queued up before the error. + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "DEFERRED LOGS BEGIN"); + + mavlink_msg msg; + while (! _deferred_logs.pop(msg)) { + gcs().send_statustext(msg.severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), msg.text); + } + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "DEFERRED LOGS END"); return true; default: @@ -399,11 +410,11 @@ bool CompassCalibrator::fit_acceptable() fabsf(_params.offdiag.z) < 1.0f ) { acceptable = (_fitness <= sq(_tolerance)); if (!acceptable) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) _fitness (%f) <= sq(_tolerance) (%f)", _compass_idx, _fitness, sq(_tolerance)); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) _fitness (%f) <= sq(_tolerance) (%f)", _compass_idx, _fitness, sq(_tolerance)); } } - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) fit_acceptable() is false", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) fit_acceptable() is false", _compass_idx); return acceptable; } @@ -467,7 +478,7 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample, uint16_t skip_inde if (i != skip_index) { float distance = (sample - _sample_buffer[i].get()).length(); if (distance < min_distance) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) accept_sample() is false", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) accept_sample() is false", _compass_idx); return false; } } @@ -587,12 +598,12 @@ void CompassCalibrator::run_sphere_fit() } if (!inverse(JTJ, JTJ, 4)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ matrix is singular", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ matrix is singular", _compass_idx); return; } if (!inverse(JTJ2, JTJ2, 4)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ2 matrix is singular", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ2 matrix is singular", _compass_idx); return; } @@ -629,7 +640,7 @@ void CompassCalibrator::run_sphere_fit() update_completion_mask(); } else { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) fitness is nan", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) fitness is nan", _compass_idx); } } @@ -708,12 +719,12 @@ void CompassCalibrator::run_ellipsoid_fit() } if (!inverse(JTJ, JTJ, 9)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ matrix is singular", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ matrix is singular", _compass_idx); return; } if (!inverse(JTJ2, JTJ2, 9)) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ2 matrix is singular", _compass_idx); + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ2 matrix is singular", _compass_idx); return; } @@ -902,16 +913,16 @@ bool CompassCalibrator::calculate_orientation(void) pass = _orientation_confidence > variance_threshold; } if (!pass) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, LOG_TEXT_PREFIX "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx, + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx, besti, besti2, (double)_orientation_confidence); (void)besti2; } else if (besti == _orientation) { // no orientation change - GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_TEXT_PREFIX "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); } else if (!_is_external || !_fix_orientation) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, LOG_TEXT_PREFIX "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence); } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_TEXT_PREFIX "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); } if (!pass) { @@ -981,7 +992,7 @@ bool CompassCalibrator::fix_radius(void) if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) { // don't allow more than 30% scale factor correction - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_TEXT_PREFIX "Mag(%u) bad radius %.0f expected %.0f", + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) bad radius %.0f expected %.0f", _compass_idx, _params.radius, expected_radius); @@ -993,3 +1004,21 @@ bool CompassCalibrator::fix_radius(void) return true; } + +/* + Defer log messages until an error occurs. + */ +void CompassCalibrator::defer_send_text(MAV_SEVERITY severity, const char *fmt, ...) +{ + if (fmt == NULL) { + return; + } + + va_list arg_list; + va_start(arg_list, fmt); + mavlink_msg msg; + msg.severity = severity; + vsnprintf(msg.text, sizeof(msg.text), fmt, arg_list); + _deferred_logs.push_force(msg); + va_end(arg_list); +} diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 329596477e..ac5ef079e6 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -1,5 +1,6 @@ #pragma once +#include #include #define COMPASS_CAL_NUM_SPHERE_PARAMS 4 @@ -9,6 +10,8 @@ #define COMPASS_MIN_SCALE_FACTOR 0.85 #define COMPASS_MAX_SCALE_FACTOR 1.4 +#define COMPASS_CAL_LOG_TEXT_PREFIX "MagCal: " + class CompassCalibrator { public: CompassCalibrator(); @@ -162,6 +165,8 @@ class CompassCalibrator { // fix radius to compensate for sensor scaling errors bool fix_radius(); + void defer_send_text(MAV_SEVERITY severity, const char *fmt, ...); + uint8_t _compass_idx; // index of the compass providing data Status _status; // current state of calibrator uint32_t _last_sample_ms; // system time of last sample received for timeout @@ -195,4 +200,11 @@ class CompassCalibrator { bool _check_orientation; // true if orientation should be automatically checked bool _fix_orientation; // true if orientation should be fixed if necessary float _orientation_confidence; // measure of confidence in automatic orientation detection + + struct mavlink_msg { + MAV_SEVERITY severity; + char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1]; + }; + + ObjectBuffer _deferred_logs; }; From 350f2c3edb1d7039a5db95ee777e6759044e1afd Mon Sep 17 00:00:00 2001 From: Dat Tran Date: Thu, 14 Apr 2022 18:16:06 -0500 Subject: [PATCH 3/5] APM-156: Use hal.util version of vsnprintf --- libraries/AP_Compass/CompassCalibrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 53840dfa5c..d5439f9e5b 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -1018,7 +1018,7 @@ void CompassCalibrator::defer_send_text(MAV_SEVERITY severity, const char *fmt, va_start(arg_list, fmt); mavlink_msg msg; msg.severity = severity; - vsnprintf(msg.text, sizeof(msg.text), fmt, arg_list); + hal.util->vsnprintf(msg.text, sizeof(msg.text), fmt, arg_list); _deferred_logs.push_force(msg); va_end(arg_list); } From ce298f821deeb23007bc2f06cc60f3fd29714aca Mon Sep 17 00:00:00 2001 From: Dat Tran Date: Fri, 15 Apr 2022 08:34:43 -0500 Subject: [PATCH 4/5] APM-156: Fix logic when poping messages --- libraries/AP_Compass/CompassCalibrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index d5439f9e5b..b864b704a7 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -381,7 +381,7 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "DEFERRED LOGS BEGIN"); mavlink_msg msg; - while (! _deferred_logs.pop(msg)) { + while (_deferred_logs.pop(msg)) { gcs().send_statustext(msg.severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), msg.text); } From 3d6cd2030faa95c1dac74aa56a1390764e645b70 Mon Sep 17 00:00:00 2001 From: Dat Tran Date: Mon, 18 Apr 2022 13:09:59 -0500 Subject: [PATCH 5/5] APM-156: Changing wording of start and end messages for deferred messages --- libraries/AP_Compass/CompassCalibrator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index b864b704a7..0d7b009584 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -378,14 +378,14 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) _status = status; // Write all warning text that were queued up before the error. - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "DEFERRED LOGS BEGIN"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Deferred message start..."); mavlink_msg msg; while (_deferred_logs.pop(msg)) { gcs().send_statustext(msg.severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), msg.text); } - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "DEFERRED LOGS END"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Deferred message end"); return true; default: