Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: improved error reporting, check all compasses 

this uses extensions to the MAG_CAL_REPORT message to convey failures
of orientation checking.

It also checks all compasses, external or internal. It only tries to
fix the orientation if it is external
This commit is contained in:
Andrew Tridgell 2018-07-17 16:11:28 +10:00
parent f10e9fe171
commit d15a4ad92a
3 changed files with 50 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -61,11 +61,11 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
// lot noisier
_calibrator[i].set_tolerance(_calibration_threshold*2);
}
if (_state[i].external && _rotate_auto) {
_calibrator[i].set_orientation((enum Rotation)_state[i].orientation.get(), _state[i].external);
if (_rotate_auto) {
_calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external);
}
_cal_saved[i] = false;
_calibrator[i].start(retry, delay, get_offsets_max());
_calibrator[i].start(retry, delay, get_offsets_max(), i);
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);
@ -222,8 +222,9 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
}
uint8_t cal_status = _calibrator[compass_id].get_status();
if ((cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED)) {
if (cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED ||
cal_status == COMPASS_CAL_BAD_ORIENTATION) {
float fitness = _calibrator[compass_id].get_fitness();
Vector3f ofs, diag, offdiag;
_calibrator[compass_id].get_calibration(ofs, diag, offdiag);
@ -236,7 +237,10 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
fitness,
ofs.x, ofs.y, ofs.z,
diag.x, diag.y, diag.z,
offdiag.x, offdiag.y, offdiag.z
offdiag.x, offdiag.y, offdiag.z,
_calibrator[compass_id].get_orientation_confidence(),
_calibrator[compass_id].get_original_orientation(),
_calibrator[compass_id].get_orientation()
);
}
}
@ -250,6 +254,7 @@ Compass::is_calibrating() const
case COMPASS_CAL_NOT_STARTED:
case COMPASS_CAL_SUCCESS:
case COMPASS_CAL_FAILED:
case COMPASS_CAL_BAD_ORIENTATION:
break;
default:
return true;

40
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -80,7 +80,8 @@ void CompassCalibrator::clear() {
set_status(COMPASS_CAL_NOT_STARTED);
}
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
{
if(running()) {
return;
}
@ -89,6 +90,7 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
_retry = retry;
_delay_start_sec = delay;
_start_time_ms = AP_HAL::millis();
_compass_idx = compass_idx;
set_status(COMPASS_CAL_WAITING_TO_START);
}
@ -116,6 +118,7 @@ float CompassCalibrator::get_completion_percent() const {
case COMPASS_CAL_SUCCESS:
return 100.0f;
case COMPASS_CAL_FAILED:
case COMPASS_CAL_BAD_ORIENTATION:
default:
return 0.0f;
};
@ -317,6 +320,13 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
return true;
case COMPASS_CAL_FAILED:
if (status == COMPASS_CAL_BAD_ORIENTATION) {
// don't overwrite bad orientation status
return false;
}
FALLTHROUGH;
case COMPASS_CAL_BAD_ORIENTATION:
if(_status == COMPASS_CAL_NOT_STARTED) {
return false;
}
@ -331,9 +341,9 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
_sample_buffer = nullptr;
}
_status = COMPASS_CAL_FAILED;
_status = status;
return true;
default:
return false;
};
@ -752,7 +762,7 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
bool CompassCalibrator::calculate_orientation(void)
{
if (!_auto_orientation) {
// only calculate orientation for external compasses
// we are not checking orientation
return true;
}
@ -795,25 +805,29 @@ bool CompassCalibrator::calculate_orientation(void)
}
}
}
_orientation_confidence = second_best/bestv;
bool pass;
if (besti == _orientation) {
// if the orientation matched then allow for a low threshold
pass = true;
} else {
pass = (second_best / bestv) > 4;
pass = _orientation_confidence > 4;
}
if (!pass) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad orientation: %u %.1f\n", besti, second_best/bestv);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
} else if (besti == _orientation) {
// no orientation change
gcs().send_text(MAV_SEVERITY_INFO, "Good orientation: %u %.1f\n", besti, second_best/bestv);
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
} else if (!_is_external) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "New orientation: %u was %u %.1f\n", besti, _orientation, second_best/bestv);
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f\n", _compass_idx, besti, _orientation, _orientation_confidence);
}
if (!pass) {
// give an indication of orientation failure by showing a very high fitness
_fitness += 1000;
set_status(COMPASS_CAL_BAD_ORIENTATION);
return false;
}
@ -821,6 +835,12 @@ bool CompassCalibrator::calculate_orientation(void)
// no orientation change
return true;
}
if (!_is_external) {
// we can't change the orientation on internal compasses
set_status(COMPASS_CAL_BAD_ORIENTATION);
return false;
}
// correct the offsets for the new orientation
Vector3f rot_offsets = _params.offset;

11
libraries/AP_Compass/CompassCalibrator.h
View File

@ -15,7 +15,8 @@ enum compass_cal_status_t {
COMPASS_CAL_RUNNING_STEP_ONE=2,
COMPASS_CAL_RUNNING_STEP_TWO=3,
COMPASS_CAL_SUCCESS=4,
COMPASS_CAL_FAILED=5
COMPASS_CAL_FAILED=5,
COMPASS_CAL_BAD_ORIENTATION=6,
};
class CompassCalibrator {
@ -24,7 +25,7 @@ public:
CompassCalibrator();
void start(bool retry, float delay, uint16_t offset_max);
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
void clear();
void update(bool &failure);
@ -37,6 +38,7 @@ public:
void set_orientation(enum Rotation orientation, bool is_external) {
_auto_orientation = true;
_orientation = orientation;
_orig_orientation = orientation;
_is_external = is_external;
}
@ -44,11 +46,13 @@ public:
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
enum Rotation get_orientation(void) { return _orientation; }
enum Rotation get_original_orientation(void) { return _orig_orientation; }
float get_completion_percent() const;
completion_mask_t& get_completion_mask();
enum compass_cal_status_t get_status() const { return _status; }
float get_fitness() const { return sqrtf(_fitness); }
float get_orientation_confidence() const { return _orientation_confidence; }
uint8_t get_attempt() const { return _attempt; }
private:
@ -91,8 +95,10 @@ private:
};
enum Rotation _orientation;
enum Rotation _orig_orientation;
bool _is_external;
bool _auto_orientation;
uint8_t _compass_idx;
enum compass_cal_status_t _status;
@ -119,6 +125,7 @@ private:
float _ellipsoid_lambda;
uint16_t _samples_collected;
uint16_t _samples_thinned;
float _orientation_confidence;
bool set_status(compass_cal_status_t status);