mirror of https://github.com/ArduPilot/ardupilot
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:
parent
4acc06df87
commit
8b0f40b402
|
@ -444,6 +444,12 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
|||
// @Increment: 1
|
||||
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
|
||||
};
|
||||
|
||||
|
|
|
@ -417,6 +417,9 @@ private:
|
|||
// 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
|
||||
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
|
||||
float _thr;
|
||||
|
||||
|
|
|
@ -61,6 +61,9 @@ 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);
|
||||
}
|
||||
_cal_saved[i] = false;
|
||||
_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_offdiagonals(i,offdiag);
|
||||
|
||||
if (_state[i].external && _rotate_auto) {
|
||||
_state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
|
||||
}
|
||||
|
||||
if (!is_calibrating()) {
|
||||
AP_Notify::events.compass_cal_saved = 1;
|
||||
}
|
||||
|
|
|
@ -60,6 +60,7 @@
|
|||
#include "CompassCalibrator.h"
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_GeodesicGrid.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
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)) {
|
||||
update_completion_mask(sample);
|
||||
_sample_buffer[_samples_collected].set(sample);
|
||||
_sample_buffer[_samples_collected].att.set_from_ahrs();
|
||||
_samples_collected++;
|
||||
}
|
||||
}
|
||||
|
@ -195,6 +197,7 @@ void CompassCalibrator::update(bool &failure) {
|
|||
} else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
|
||||
if (_fit_step >= 35) {
|
||||
if(fit_acceptable()) {
|
||||
calculate_orientation();
|
||||
set_status(COMPASS_CAL_SUCCESS);
|
||||
} else {
|
||||
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);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1,3 +1,5 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
|
||||
|
@ -32,9 +34,16 @@ public:
|
|||
|
||||
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 get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
|
||||
enum Rotation get_orientation(void) { return _orientation; }
|
||||
|
||||
float get_completion_percent() const;
|
||||
completion_mask_t& get_completion_mask();
|
||||
|
@ -59,17 +68,31 @@ private:
|
|||
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 {
|
||||
public:
|
||||
Vector3f get() const;
|
||||
void set(const Vector3f &in);
|
||||
AttitudeSample att;
|
||||
private:
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
};
|
||||
|
||||
|
||||
enum Rotation _orientation;
|
||||
bool _is_external;
|
||||
bool _auto_orientation;
|
||||
|
||||
enum compass_cal_status_t _status;
|
||||
|
||||
|
@ -136,4 +159,7 @@ private:
|
|||
* Reset and update #_completion_mask with the current samples.
|
||||
*/
|
||||
void update_completion_mask();
|
||||
|
||||
Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
|
||||
void calculate_orientation();
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue