Skip to content

Feature/apm 156 add more error messages on failure #234

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 5 commits into
base: mttr-rebase-4.0
Choose a base branch
from
Open

Feature/apm 156 add more error messages on failure #234

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
5664da2
APM-156: Add more debug messages on compass calibration failures
dat-tran-matternet Apr 13, 2022
756a2a2
APM-156: Defer warning logs until an error occurs
dat-tran-matternet Apr 14, 2022
350f2c3
APM-156: Use hal.util version of vsnprintf
dat-tran-matternet Apr 14, 2022
ce298f8
APM-156: Fix logic when poping messages
dat-tran-matternet Apr 15, 2022
3d6cd20
APM-156: Changing wording of start and end messages for deferred mess…
dat-tran-matternet Apr 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 12 additions & 11 deletions libraries/AP_Compass/AP_Compass_Calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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();
Expand All @@ -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;
}

Expand All @@ -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;
}

Expand Down
73 changes: 63 additions & 10 deletions libraries/AP_Compass/CompassCalibrator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,13 +67,16 @@
#define FIELD_RADIUS_MIN 150
#define FIELD_RADIUS_MAX 950

#define DEFERRED_LOG_QUEUE_SIZE 5

extern const AP_HAL::HAL& hal;

////////////////////////////////////////////////////////////
///////////////////// PUBLIC INTERFACE /////////////////////
////////////////////////////////////////////////////////////

CompassCalibrator::CompassCalibrator()
: _deferred_logs(DEFERRED_LOG_QUEUE_SIZE)
{
stop();
}
Expand Down Expand Up @@ -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 {
Expand All @@ -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;
}
Expand Down Expand Up @@ -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:
Expand All @@ -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 &&
Expand All @@ -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()
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
Expand All @@ -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);
}
12 changes: 12 additions & 0 deletions libraries/AP_Compass/CompassCalibrator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#pragma once

#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Math/AP_Math.h>

#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
Expand All @@ -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();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<mavlink_msg> _deferred_logs;
};