diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 86b69e127b..5ace458893 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -27,6 +27,7 @@ void Compass::cal_update() } if (_calibrator[i].check_for_timeout()) { + 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(); } @@ -59,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, "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, "Compass cal requires GPS lock"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Compass cal requires GPS lock"); return false; } } @@ -313,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, "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; } @@ -365,19 +366,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 +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, "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(); @@ -450,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, "Mag: WMM table error"); + gcs().send_text(MAV_SEVERITY_ERROR, COMPASS_CAL_LOG_TEXT_PREFIX "Mag: WMM table error"); return MAV_RESULT_FAILED; } @@ -475,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, "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, "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 a403d7d5b5..0d7b009584 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 DEFERRED_LOG_QUEUE_SIZE 5 + extern const AP_HAL::HAL& hal; //////////////////////////////////////////////////////////// @@ -74,6 +76,7 @@ extern const AP_HAL::HAL& hal; //////////////////////////////////////////////////////////// CompassCalibrator::CompassCalibrator() + : _deferred_logs(DEFERRED_LOG_QUEUE_SIZE) { stop(); } @@ -206,6 +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, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) fitness is diverging", _compass_idx); set_status(Status::FAILED); failure = true; } else { @@ -220,9 +224,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, 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; } @@ -366,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 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 message end"); return true; default: @@ -375,6 +395,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 +408,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) { + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) _fitness (%f) <= sq(_tolerance) (%f)", _compass_idx, _fitness, sq(_tolerance)); + } } - return false; + + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) fit_acceptable() is false", _compass_idx); + return acceptable; } void CompassCalibrator::thin_samples() @@ -451,6 +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) { + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) accept_sample() is false", _compass_idx); return false; } } @@ -570,10 +598,12 @@ void CompassCalibrator::run_sphere_fit() } if (!inverse(JTJ, JTJ, 4)) { + 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)) { + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) run_sphere_fit: JTJ2 matrix is singular", _compass_idx); return; } @@ -609,6 +639,9 @@ void CompassCalibrator::run_sphere_fit() _params = fit1_params; update_completion_mask(); } + else { + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_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 +719,12 @@ void CompassCalibrator::run_ellipsoid_fit() } if (!inverse(JTJ, JTJ, 9)) { + 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)) { + defer_send_text(MAV_SEVERITY_WARNING, COMPASS_CAL_LOG_TEXT_PREFIX "Mag(%u) run_ellipsoid_fit: JTJ2 matrix is singular", _compass_idx); return; } @@ -773,10 +808,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 +913,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, 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, "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, "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, "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) { @@ -957,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, "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); @@ -969,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; + hal.util->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; };