Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: support auto orientation for new rotations

Browse Source
This commit is contained in:
Iampete1 2021-07-23 20:48:40 +01:00 committed by Andrew Tridgell
parent dbd95f8163
commit 36a80d46e3
4 changed files with 121 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass.cpp
View File

@ -512,7 +512,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Param: AUTO_ROT
// @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
// @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix,3:use same tolerance to auto rotate 45 deg rotations
AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, HAL_COMPASS_AUTO_ROT_DEFAULT),
#endif

2
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -82,7 +82,7 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
if (_rotate_auto) {
enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
if (r != ROTATION_CUSTOM) {
_calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2);
_calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2, _rotate_auto>=3);
}
}
_cal_saved[prio] = false;

124
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -87,7 +87,7 @@ void CompassCalibrator::stop()
_status_set_requested = true;
}
void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation, bool always_45_deg)
{
WITH_SEMAPHORE(state_sem);
cal_settings.check_orientation = true;
@ -95,6 +95,7 @@ void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_exter
cal_settings.orig_orientation = orientation;
cal_settings.is_external = is_external;
cal_settings.fix_orientation = fix_orientation;
cal_settings.always_45_deg = always_45_deg;
}
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance)
@ -251,6 +252,7 @@ void CompassCalibrator::update_cal_settings()
_delay_start_sec = cal_settings.delay_start_sec;
_start_time_ms = cal_settings.start_time_ms;
_compass_idx = cal_settings.compass_idx;
_always_45_deg = cal_settings.always_45_deg;
}
// update completion mask based on latest sample
@ -909,11 +911,14 @@ bool CompassCalibrator::calculate_orientation(void)
// this function is very slow
EXPECT_DELAY_MS(1000);
float variance[ROTATION_MAX_AUTO_ROTATION+1] {};
// Skip 4 rotations, see: auto_rotation_index()
float variance[ROTATION_MAX-4] {};
_orientation_solution = _orientation;
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
Rotation r = auto_rotation_index(n);
// calculate the average implied earth field across all samples
Vector3f total_ef {};
for (uint32_t i=0; i<_samples_collected; i++) {
@ -927,33 +932,44 @@ bool CompassCalibrator::calculate_orientation(void)
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
float err = (efield - avg_efield).length_squared();
// divide by number of samples collected to get the variance
variance[r] += err / _samples_collected;
variance[n] += err / _samples_collected;
}
}
// find the rotation with the lowest variance
enum Rotation besti = ROTATION_NONE;
float bestv = variance[0];
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
if (variance[r] < bestv) {
bestv = variance[r];
enum Rotation besti_90 = ROTATION_NONE;
float bestv_90 = variance[0];
for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
Rotation r = auto_rotation_index(n);
if (variance[n] < bestv) {
bestv = variance[n];
besti = r;
}
if (right_angle_rotation(r) && variance[n] < bestv_90) {
bestv_90 = variance[n];
besti_90 = r;
}
}
// 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];
enum Rotation besti2 = ROTATION_NONE;
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
if (!rotation_equal(besti, r)) {
if (variance[r] < second_best) {
second_best = variance[r];
enum Rotation besti2 = besti==ROTATION_NONE?ROTATION_YAW_45:ROTATION_NONE;
float second_best_90 = besti_90==ROTATION_NONE?variance[2]:variance[0];
enum Rotation besti2_90 = besti_90==ROTATION_NONE?ROTATION_YAW_90:ROTATION_NONE;
for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
Rotation r = auto_rotation_index(n);
if (besti != r) {
if (variance[n] < second_best) {
second_best = variance[n];
besti2 = r;
}
}
if (right_angle_rotation(r) && (besti_90 != r) && (variance[n] < second_best_90)) {
second_best_90 = variance[n];
besti2_90 = r;
}
}
_orientation_confidence = second_best/bestv;
@ -963,7 +979,21 @@ bool CompassCalibrator::calculate_orientation(void)
// if the orientation matched then allow for a low threshold
pass = true;
} else {
pass = _orientation_confidence > variance_threshold;
if (_orientation_confidence > 4.0) {
// very confident, always pass
pass = true;
} else if (_always_45_deg && (_orientation_confidence > 2.0)) {
// use same tolerance for all rotations
pass = true;
} else {
// just consider 90's
_orientation_confidence = second_best_90 / bestv_90;
pass = _orientation_confidence > 2.0;
besti = besti_90;
besti2 = besti2_90;
}
}
if (!pass) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
@ -1059,3 +1089,65 @@ bool CompassCalibrator::fix_radius(void)
return true;
}
// convert index to rotation, this allows to skip some rotations
Rotation CompassCalibrator::auto_rotation_index(uint8_t n) const
{
if (n < ROTATION_PITCH_180_YAW_90) {
return (Rotation)n;
}
// ROTATION_PITCH_180_YAW_90 (26) is the same as ROTATION_ROLL_180_YAW_90 (10)
// ROTATION_PITCH_180_YAW_270 (27) is the same as ROTATION_ROLL_180_YAW_270 (14)
n += 2;
if (n < ROTATION_ROLL_90_PITCH_68_YAW_293) {
return (Rotation)n;
}
// ROTATION_ROLL_90_PITCH_68_YAW_293 is for solo leg compass, not expecting to see this in other vehicles
n += 1;
if (n < ROTATION_PITCH_7) {
return (Rotation)n;
}
// ROTATION_PITCH_7 is too close to ROTATION_NONE to tell the difference in compass cal
n += 1;
if (n < ROTATION_MAX) {
return (Rotation)n;
}
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return ROTATION_NONE;
};
bool CompassCalibrator::right_angle_rotation(Rotation r) const
{
switch (r) {
case ROTATION_NONE:
case ROTATION_YAW_90:
case ROTATION_YAW_180:
case ROTATION_YAW_270:
case ROTATION_ROLL_180:
case ROTATION_ROLL_180_YAW_90:
case ROTATION_PITCH_180:
case ROTATION_ROLL_180_YAW_270:
case ROTATION_ROLL_90:
case ROTATION_ROLL_90_YAW_90:
case ROTATION_ROLL_270:
case ROTATION_ROLL_270_YAW_90:
case ROTATION_PITCH_90:
case ROTATION_PITCH_270:
case ROTATION_PITCH_180_YAW_90:
case ROTATION_PITCH_180_YAW_270:
case ROTATION_ROLL_90_PITCH_90:
case ROTATION_ROLL_180_PITCH_90:
case ROTATION_ROLL_270_PITCH_90:
case ROTATION_ROLL_90_PITCH_180:
case ROTATION_ROLL_270_PITCH_180:
case ROTATION_ROLL_90_PITCH_270:
case ROTATION_ROLL_180_PITCH_270:
case ROTATION_ROLL_270_PITCH_270:
case ROTATION_ROLL_90_PITCH_180_YAW_90:
case ROTATION_ROLL_90_YAW_270:
return true;
default:
return false;
}
}

12
libraries/AP_Compass/CompassCalibrator.h
View File

@ -21,7 +21,7 @@ public:
void new_sample(const Vector3f& sample);
// set compass's initial orientation and whether it should be automatically fixed (if required)
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation);
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation, bool always_45_deg);
// running is true if actively calculating offsets, diagonals or offdiagonals
bool running();
@ -83,6 +83,7 @@ public:
float delay_start_sec;
uint32_t start_time_ms;
uint8_t compass_idx;
bool always_45_deg;
} cal_settings;
// Get calibration result
@ -91,6 +92,14 @@ public:
// Get current Calibration state
const State get_state();
protected:
// convert index to rotation, this allows to skip some rotations
// protected so CompassCalibrator_index_test can see it
Rotation auto_rotation_index(uint8_t n) const;
// return true if this is a right angle rotation
bool right_angle_rotation(Rotation r) const;
private:
// results
@ -230,6 +239,7 @@ private:
bool _is_external; // true if compass is external (provided by caller)
bool _check_orientation; // true if orientation should be automatically checked
bool _fix_orientation; // true if orientation should be fixed if necessary
bool _always_45_deg; // true if orentation should considder 45deg with equal tolerance
float _orientation_confidence; // measure of confidence in automatic orientation detection
CompassSample _last_sample;