aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: make COMPASS_ROT_AUTO take 3 values

Browse Source 
0 for disabled, 1 for check only, 2 for check and fix
This commit is contained in:
Andrew Tridgell 2018-07-18 10:09:21 +10:00
parent d15a4ad92a
commit 196ba0e858
4 changed files with 49 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Compass/AP_Compass.cpp
View File

@ -445,10 +445,10 @@ const AP_Param::GroupInfo Compass::var_info[] = {
AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT), AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
// @Param: ROT_AUTO // @Param: ROT_AUTO
// @DisplayName: Automatically set orientation // @DisplayName: Automatically check orientation
// @Description: When enabled this will automatically set the orientation of external compasses on successful completion of compass calibration // @Description: When enabled this will automatically check the orientation of compasses on successful completion of compass calibration. If set to 2 then external compasses will have their orientation automatically corrected.
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix
AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 1), AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 2),
AP_GROUPEND AP_GROUPEND
}; };

4
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -62,7 +62,7 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
_calibrator[i].set_tolerance(_calibration_threshold*2); _calibrator[i].set_tolerance(_calibration_threshold*2);
} }
if (_rotate_auto) { if (_rotate_auto) {
_calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external); _calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external, _rotate_auto>=2);
} }
_cal_saved[i] = false; _cal_saved[i] = false;
_calibrator[i].start(retry, delay, get_offsets_max(), i); _calibrator[i].start(retry, delay, get_offsets_max(), i);
@ -150,7 +150,7 @@ Compass::_accept_calibration(uint8_t i)
set_and_save_diagonals(i,diag); set_and_save_diagonals(i,diag);
set_and_save_offdiagonals(i,offdiag); set_and_save_offdiagonals(i,offdiag);
if (_state[i].external && _rotate_auto) { if (_state[i].external && _rotate_auto >= 2) {
_state[i].orientation.set_and_save_ifchanged(cal.get_orientation()); _state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
} }

57
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -320,7 +320,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
return true; return true;
case COMPASS_CAL_FAILED: case COMPASS_CAL_FAILED:
if (status == COMPASS_CAL_BAD_ORIENTATION) { if (_status == COMPASS_CAL_BAD_ORIENTATION) {
// don't overwrite bad orientation status // don't overwrite bad orientation status
return false; return false;
} }
@ -731,7 +731,15 @@ Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) {
} }
/* /*
calculate the implied earth field for a compass sample and compass rotation calculate the implied earth field for a compass sample and compass
rotation. This is used to check for consistency between
samples.
If the orientation is correct then when rotated the same (or
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.
*/ */
Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r) Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r)
{ {
@ -743,6 +751,7 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
// rotate to body frame for this rotation // rotate to body frame for this rotation
v.rotate(r); v.rotate(r);
// apply offsets, rotating them for the orientation we are testing
Vector3f rot_offsets = _params.offset; Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation); rot_offsets.rotate_inverse(_orientation);
@ -750,23 +759,28 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
v += rot_offsets; v += rot_offsets;
// rotate the sample from body frame back to earth frame
Matrix3f rot = sample.att.get_rotmat(); Matrix3f rot = sample.att.get_rotmat();
Vector3f efield = rot * v; Vector3f efield = rot * v;
// earth field is the mag sample in earth frame
return efield; return efield;
} }
/* /*
calculate compass orientation using the attitude estimate associated with each sample calculate compass orientation using the attitude estimate associated
with each sample, and fix orientation on external compasses if
the feature is enabled
*/ */
bool CompassCalibrator::calculate_orientation(void) bool CompassCalibrator::calculate_orientation(void)
{ {
if (!_auto_orientation) { if (!_check_orientation) {
// we are not checking orientation // we are not checking orientation
return true; return true;
} }
float sum_error[ROTATION_MAX] {}; float variance[ROTATION_MAX] {};
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) { for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
// calculate the average implied earth field across all samples // calculate the average implied earth field across all samples
@ -781,27 +795,30 @@ bool CompassCalibrator::calculate_orientation(void)
for (uint32_t i=0; i<_samples_collected; i++) { for (uint32_t i=0; i<_samples_collected; i++) {
Vector3f efield = calculate_earth_field(_sample_buffer[i], r); Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
float err = (efield - avg_efield).length_squared(); float err = (efield - avg_efield).length_squared();
sum_error[r] += err; // divide by number of samples collected to get the variance
variance[r] += err / _samples_collected;
} }
} }
// find the rotation with the lowest square error // find the rotation with the lowest variance
enum Rotation besti = ROTATION_NONE; enum Rotation besti = ROTATION_NONE;
float bestv = sum_error[0]; float bestv = variance[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) { for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (sum_error[r] < bestv) { if (variance[r] < bestv) {
bestv = sum_error[r]; bestv = variance[r];
besti = r; besti = r;
} }
} }
// consider this a pass if the best orientation is 4x better // consider this a pass if the best orientation is 2x better
// square error than 2nd best // variance than 2nd best
float second_best = besti==ROTATION_NONE?sum_error[1]:sum_error[0]; const float variance_threshold = 2.0;
float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) { for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (r != besti) { if (r != besti) {
if (sum_error[r] < second_best) { if (variance[r] < second_best) {
second_best = sum_error[r]; second_best = variance[r];
} }
} }
} }
@ -813,14 +830,14 @@ bool CompassCalibrator::calculate_orientation(void)
// if the orientation matched then allow for a low threshold // if the orientation matched then allow for a low threshold
pass = true; pass = true;
} else { } else {
pass = _orientation_confidence > 4; pass = _orientation_confidence > variance_threshold;
} }
if (!pass) { if (!pass) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
} else if (besti == _orientation) { } else if (besti == _orientation) {
// no orientation change // no orientation change
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
} else if (!_is_external) { } else if (!_is_external || !_fix_orientation) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence);
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f\n", _compass_idx, besti, _orientation, _orientation_confidence); gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f\n", _compass_idx, besti, _orientation, _orientation_confidence);
@ -836,8 +853,10 @@ bool CompassCalibrator::calculate_orientation(void)
return true; return true;
} }
if (!_is_external) { if (!_is_external || !_fix_orientation) {
// we can't change the orientation on internal compasses // we won't change the orientation, but we set _orientation
// for reporting purposes
_orientation = besti;
set_status(COMPASS_CAL_BAD_ORIENTATION); set_status(COMPASS_CAL_BAD_ORIENTATION);
return false; return false;
} }

8
libraries/AP_Compass/CompassCalibrator.h
View File

@ -35,11 +35,12 @@ public:
bool running() const; bool running() const;
void set_orientation(enum Rotation orientation, bool is_external) { void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) {
_auto_orientation = true; _check_orientation = true;
_orientation = orientation; _orientation = orientation;
_orig_orientation = orientation; _orig_orientation = orientation;
_is_external = is_external; _is_external = is_external;
_fix_orientation = fix_orientation;
} }
void set_tolerance(float tolerance) { _tolerance = tolerance; } void set_tolerance(float tolerance) { _tolerance = tolerance; }
@ -97,7 +98,8 @@ private:
enum Rotation _orientation; enum Rotation _orientation;
enum Rotation _orig_orientation; enum Rotation _orig_orientation;
bool _is_external; bool _is_external;
bool _auto_orientation; bool _check_orientation;
bool _fix_orientation;
uint8_t _compass_idx; uint8_t _compass_idx;
enum compass_cal_status_t _status; enum compass_cal_status_t _status;