Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -04:00
Code Issues 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),
// @Param: ROT_AUTO
// @DisplayName: Automatically set orientation
// @Description: When enabled this will automatically set the orientation of external compasses on successful completion of compass calibration
// @Values: 0:Disabled,1:Enabled
AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 1),
// @DisplayName: Automatically check orientation
// @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:CheckOnly,2:CheckAndFix
AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 2),
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);
}
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;
_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_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());
}

57
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -320,7 +320,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
return true;
case COMPASS_CAL_FAILED:
if (status == COMPASS_CAL_BAD_ORIENTATION) {
if (_status == COMPASS_CAL_BAD_ORIENTATION) {
// don't overwrite bad orientation status
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)
{
@ -743,6 +751,7 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
// rotate to body frame for this rotation
v.rotate(r);
// apply offsets, rotating them for the orientation we are testing
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
@ -750,23 +759,28 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro
v += rot_offsets;
// rotate the sample from body frame back to earth frame
Matrix3f rot = sample.att.get_rotmat();
Vector3f efield = rot * v;
// earth field is the mag sample in earth frame
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)
{
if (!_auto_orientation) {
if (!_check_orientation) {
// we are not checking orientation
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)) {
// 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++) {
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
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;
float bestv = sum_error[0];
float bestv = variance[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (sum_error[r] < bestv) {
bestv = sum_error[r];
if (variance[r] < bestv) {
bestv = variance[r];
besti = r;
}
}
// consider this a pass if the best orientation is 4x better
// square error than 2nd best
float second_best = besti==ROTATION_NONE?sum_error[1]:sum_error[0];
// consider this a pass if the best orientation is 2x better
// variance than 2nd best
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)) {
if (r != besti) {
if (sum_error[r] < second_best) {
second_best = sum_error[r];
if (variance[r] < second_best) {
second_best = variance[r];
}
}
}
@ -813,14 +830,14 @@ bool CompassCalibrator::calculate_orientation(void)
// if the orientation matched then allow for a low threshold
pass = true;
} else {
pass = _orientation_confidence > 4;
pass = _orientation_confidence > variance_threshold;
}
if (!pass) {
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, "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);
} else {
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;
}
if (!_is_external) {
// we can't change the orientation on internal compasses
if (!_is_external || !_fix_orientation) {
// we won't change the orientation, but we set _orientation
// for reporting purposes
_orientation = besti;
set_status(COMPASS_CAL_BAD_ORIENTATION);
return false;
}

8
libraries/AP_Compass/CompassCalibrator.h
View File

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