AP_Compass: implement automatic compass orientation

this automatically determines the compass orientation when doing a 3D
compass calibration, if COMPASS_ROT_AUTO is enabled.
This commit is contained in:
Andrew Tridgell 2018-07-16 18:21:53 +10:00
parent 4acc06df87
commit 8b0f40b402
5 changed files with 173 additions and 1 deletions

View File

@ -444,6 +444,12 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1 // @Increment: 1
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
// @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),
AP_GROUPEND AP_GROUPEND
}; };

View File

@ -417,6 +417,9 @@ private:
// 0 = disabled, 1 = enabled for throttle, 2 = enabled for current // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Int8 _motor_comp_type; AP_Int8 _motor_comp_type;
// automatic compass orientation on calibration
AP_Int8 _rotate_auto;
// throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
float _thr; float _thr;

View File

@ -61,6 +61,9 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
// lot noisier // lot noisier
_calibrator[i].set_tolerance(_calibration_threshold*2); _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);
}
_cal_saved[i] = false; _cal_saved[i] = false;
_calibrator[i].start(retry, delay, get_offsets_max()); _calibrator[i].start(retry, delay, get_offsets_max());
@ -147,6 +150,10 @@ 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) {
_state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
}
if (!is_calibrating()) { if (!is_calibrating()) {
AP_Notify::events.compass_cal_saved = 1; AP_Notify::events.compass_cal_saved = 1;
} }

View File

@ -60,6 +60,7 @@
#include "CompassCalibrator.h" #include "CompassCalibrator.h"
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_GeodesicGrid.h> #include <AP_Math/AP_GeodesicGrid.h>
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -167,6 +168,7 @@ void CompassCalibrator::new_sample(const Vector3f& sample) {
if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
update_completion_mask(sample); update_completion_mask(sample);
_sample_buffer[_samples_collected].set(sample); _sample_buffer[_samples_collected].set(sample);
_sample_buffer[_samples_collected].att.set_from_ahrs();
_samples_collected++; _samples_collected++;
} }
} }
@ -195,6 +197,7 @@ void CompassCalibrator::update(bool &failure) {
} else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) { } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
if (_fit_step >= 35) { if (_fit_step >= 35) {
if(fit_acceptable()) { if(fit_acceptable()) {
calculate_orientation();
set_status(COMPASS_CAL_SUCCESS); set_status(COMPASS_CAL_SUCCESS);
} else { } else {
set_status(COMPASS_CAL_FAILED); set_status(COMPASS_CAL_FAILED);
@ -696,3 +699,130 @@ void CompassCalibrator::CompassSample::set(const Vector3f &in) {
y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y); y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z); z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
} }
void CompassCalibrator::AttitudeSample::set_from_ahrs(void) {
const Matrix3f &dcm = AP::ahrs().get_DCM_rotation_body_to_ned();
float roll_rad, pitch_rad, yaw_rad;
dcm.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
roll = constrain_int16(127 * (roll_rad / M_PI), -127, 127);
pitch = constrain_int16(127 * (pitch_rad / M_PI_2), -127, 127);
yaw = constrain_int16(127 * (yaw_rad / M_PI), -127, 127);
}
Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) {
float roll_rad, pitch_rad, yaw_rad;
roll_rad = roll * (M_PI / 127);
pitch_rad = pitch * (M_PI_2 / 127);
yaw_rad = yaw * (M_PI / 127);
Matrix3f dcm;
dcm.from_euler(roll_rad, pitch_rad, yaw_rad);
return dcm;
}
/*
calculate the implied earth field for a compass sample and compass rotation
*/
Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r)
{
Vector3f v = sample.get();
// convert the sample back to sensor frame
v.rotate_inverse(_orientation);
// rotate to body frame for this rotation
v.rotate(r);
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
rot_offsets.rotate(r);
v += rot_offsets;
Matrix3f rot = sample.att.get_rotmat();
Vector3f efield = rot * v;
return efield;
}
/*
calculate compass orientation using the attitude estimate associated with each sample
*/
void CompassCalibrator::calculate_orientation(void)
{
if (!_auto_orientation) {
// only calculate orientation for external compasses
return;
}
float sum_error[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
Vector3f total_ef {};
for (uint32_t i=0; i<_samples_collected; i++) {
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
total_ef += efield;
}
Vector3f avg_efield = total_ef / _samples_collected;
// now calculate the square error for this rotation against the average earth field
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;
}
}
// find the rotation with the lowest square error
enum Rotation besti = ROTATION_NONE;
float bestv = sum_error[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (sum_error[r] < bestv) {
bestv = sum_error[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];
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];
}
}
}
bool pass = (second_best / bestv) > 4;
if (!pass) {
hal.console->printf("Bad orientation estimation: %u %f\n", besti, second_best/bestv);
} else if (besti == _orientation) {
// no orientation change
hal.console->printf("Good orientation: %u %f\n", besti, second_best/bestv);
} else {
hal.console->printf("New orientation: %u was %u %f\n", besti, _orientation, second_best/bestv);
}
if (!pass) {
return;
}
if (_orientation != besti) {
// correct the offsets for the new orientation
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
rot_offsets.rotate(besti);
_params.offset = rot_offsets;
Vector3f &diagonals = _params.diag;
Vector3f &offdiagonals = _params.offdiag;
// we should calculated the corrected diagonals and off-diagonals here, but
// for now just avoid eliptical correction if we're changing the orientation
diagonals = Vector3f(1,1,1);
offdiagonals.zero();
_orientation = besti;
}
}

View File

@ -1,3 +1,5 @@
#pragma once
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4 #define COMPASS_CAL_NUM_SPHERE_PARAMS 4
@ -32,9 +34,16 @@ public:
bool running() const; bool running() const;
void set_orientation(enum Rotation orientation, bool is_external) {
_auto_orientation = true;
_orientation = orientation;
_is_external = is_external;
}
void set_tolerance(float tolerance) { _tolerance = tolerance; } void set_tolerance(float tolerance) { _tolerance = tolerance; }
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
enum Rotation get_orientation(void) { return _orientation; }
float get_completion_percent() const; float get_completion_percent() const;
completion_mask_t& get_completion_mask(); completion_mask_t& get_completion_mask();
@ -59,17 +68,31 @@ private:
Vector3f offdiag; Vector3f offdiag;
}; };
// compact class for approximate attitude, to save memory
class AttitudeSample {
public:
Matrix3f get_rotmat();
void set_from_ahrs();
private:
int8_t roll;
int8_t pitch;
int8_t yaw;
};
class CompassSample { class CompassSample {
public: public:
Vector3f get() const; Vector3f get() const;
void set(const Vector3f &in); void set(const Vector3f &in);
AttitudeSample att;
private: private:
int16_t x; int16_t x;
int16_t y; int16_t y;
int16_t z; int16_t z;
}; };
enum Rotation _orientation;
bool _is_external;
bool _auto_orientation;
enum compass_cal_status_t _status; enum compass_cal_status_t _status;
@ -136,4 +159,7 @@ private:
* Reset and update #_completion_mask with the current samples. * Reset and update #_completion_mask with the current samples.
*/ */
void update_completion_mask(); void update_completion_mask();
Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
void calculate_orientation();
}; };