diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp new file mode 100644 index 0000000000..d79747a19b --- /dev/null +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -0,0 +1,851 @@ +#include "CompassCalibrator.h" +#include + +extern const AP_HAL::HAL& hal; + +//////////////////////////////////////////////////////////// +///////////////////// PUBLIC INTERFACE ///////////////////// +//////////////////////////////////////////////////////////// + +CompassCalibrator::CompassCalibrator(): +_tolerance(COMPASS_CAL_DEFAULT_TOLERANCE) +{ + clear(); +} + +void CompassCalibrator::clear() { + set_status(COMPASS_CAL_NOT_STARTED); +} + +void CompassCalibrator::start(bool retry, bool autosave, float delay) { + if(running()) { + return; + } + _autosave = autosave; + _attempt = 1; + _retry = retry; + _delay_start_sec = delay; + _start_time_ms = hal.scheduler->millis(); + set_status(COMPASS_CAL_WAITING_TO_START); +} + +void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals) { + if (_status != COMPASS_CAL_SUCCESS) { + return; + } + + offsets = _sphere_param.named.offset; + diagonals = _ellipsoid_param.named.diag; + offdiagonals = _ellipsoid_param.named.offdiag; +} + +float CompassCalibrator::get_completion_percent() const { + // first sampling step is 1/3rd of the progress bar + // never return more than 99% unless _status is COMPASS_CAL_SUCCESS + switch(_status) { + case COMPASS_CAL_NOT_STARTED: + case COMPASS_CAL_WAITING_TO_START: + return 0.0f; + case COMPASS_CAL_SAMPLING_STEP_ONE: + return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES; + case COMPASS_CAL_SAMPLING_STEP_TWO: + return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned)); + case COMPASS_CAL_SUCCESS: + return 100.0f; + case COMPASS_CAL_FAILED: + default: + return 0.0f; + }; +} + +bool CompassCalibrator::running() const { + return _status == COMPASS_CAL_SAMPLING_STEP_ONE || _status == COMPASS_CAL_SAMPLING_STEP_TWO; +} + +void CompassCalibrator::check_for_timeout() { + uint32_t tnow = hal.scheduler->millis(); + if(running() && tnow - _last_sample_ms > 1000) { + set_status(COMPASS_CAL_FAILED); + } +} + +void CompassCalibrator::new_sample(const Vector3f& sample) { + _last_sample_ms = hal.scheduler->millis(); + + if(_status == COMPASS_CAL_WAITING_TO_START) { + set_status(COMPASS_CAL_SAMPLING_STEP_ONE); + } + + if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { + _sample_buffer[_samples_collected].set(sample); + _samples_collected++; + } +} + +void CompassCalibrator::run_fit_chunk() { + if(!running() || _samples_collected != COMPASS_CAL_NUM_SAMPLES) { + return; + } + + if(_fit_step == 0) { + //initialize _fitness before starting a fit + _fitness = calc_mean_squared_residuals(_sphere_param,_ellipsoid_param); + } + + if(_status == COMPASS_CAL_SAMPLING_STEP_ONE) { + if(_fit_step < 7) { + run_sphere_fit(1); + _fit_step++; + } else { + if(_fitness > sq(_tolerance*50.0f)) { + set_status(COMPASS_CAL_FAILED); + } + + set_status(COMPASS_CAL_SAMPLING_STEP_TWO); + } + } else if(_status == COMPASS_CAL_SAMPLING_STEP_TWO) { + if(_fit_step < 3) { + run_sphere_fit(1); + } else if(_fit_step < 6) { + run_ellipsoid_fit(1); + } else if(_fit_step%2 == 0 && _fit_step < 35) { + run_sphere_fit(1); + } else if(_fit_step%2 == 1 && _fit_step < 35) { + run_ellipsoid_fit(1); + } else { + if(fit_acceptable()) { + set_status(COMPASS_CAL_SUCCESS); + } else { + set_status(COMPASS_CAL_FAILED); + } + } + _fit_step++; + } +} + +///////////////////////////////////////////////////////////// +////////////////////// PRIVATE METHODS ////////////////////// +///////////////////////////////////////////////////////////// +void CompassCalibrator::reset_state() { + _samples_collected = 0; + _samples_thinned = 0; + _fitness = -1.0f; + _sphere_param.named.radius = 200; + _sphere_param.named.offset.zero(); + _ellipsoid_param.named.diag = Vector3f(1.0f,1.0f,1.0f); + _ellipsoid_param.named.offdiag.zero(); + _fit_step = 0; +} + +bool CompassCalibrator::set_status(compass_cal_status_t status) { + if (status != COMPASS_CAL_NOT_STARTED && _status == status) { + return true; + } + + switch(status) { + case COMPASS_CAL_NOT_STARTED: + reset_state(); + _status = COMPASS_CAL_NOT_STARTED; + + if(_sample_buffer != NULL) { + free(_sample_buffer); + _sample_buffer = NULL; + } + return true; + + case COMPASS_CAL_WAITING_TO_START: + reset_state(); + _status = COMPASS_CAL_WAITING_TO_START; + + set_status(COMPASS_CAL_SAMPLING_STEP_ONE); + return true; + + case COMPASS_CAL_SAMPLING_STEP_ONE: + if(_status != COMPASS_CAL_WAITING_TO_START) { + return false; + } + + if(_attempt == 1 && (hal.scheduler->millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) { + return false; + } + + + if(_sample_buffer != NULL) { + _status = COMPASS_CAL_SAMPLING_STEP_ONE; + return true; + } + + _sample_buffer = (CompassSample*)malloc(sizeof(CompassSample)*COMPASS_CAL_NUM_SAMPLES); + + if(_sample_buffer != NULL) { + _status = COMPASS_CAL_SAMPLING_STEP_ONE; + return true; + } + + return false; + + case COMPASS_CAL_SAMPLING_STEP_TWO: + if(_status != COMPASS_CAL_SAMPLING_STEP_ONE) { + return false; + } + _fit_step = 0; + thin_samples(); + _status = COMPASS_CAL_SAMPLING_STEP_TWO; + return true; + + case COMPASS_CAL_SUCCESS: + if(_status != COMPASS_CAL_SAMPLING_STEP_TWO) { + return false; + } + + if(_sample_buffer != NULL) { + free(_sample_buffer); + _sample_buffer = NULL; + } + + _status = COMPASS_CAL_SUCCESS; + return true; + + case COMPASS_CAL_FAILED: + if(_status == COMPASS_CAL_NOT_STARTED) { + return false; + } + + if(_retry && set_status(COMPASS_CAL_WAITING_TO_START)) { + _attempt++; + return true; + } + + if(_sample_buffer != NULL) { + free(_sample_buffer); + _sample_buffer = NULL; + } + + _status = COMPASS_CAL_FAILED; + return true; + + default: + return false; + }; +} + +bool CompassCalibrator::fit_acceptable() { + //TODO check all params + return _fitness <= sq(_tolerance); +} + +void CompassCalibrator::thin_samples() { + if(_sample_buffer == NULL) { + return; + } + + _samples_thinned = 0; + // shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle + // this is so that adjacent samples don't get sequentially eliminated + for(uint16_t i=_samples_collected-1; i>=1; i--) { + uint16_t j = get_random() % (i+1); + CompassSample temp = _sample_buffer[i]; + _sample_buffer[i] = _sample_buffer[j]; + _sample_buffer[j] = temp; + } + + for(uint16_t i=0; i < _samples_collected; i++) { + if(!accept_sample(_sample_buffer[i])) { + _sample_buffer[i] = _sample_buffer[_samples_collected-1]; + _samples_collected --; + _samples_thinned ++; + } + } +} + +bool CompassCalibrator::accept_sample(const Vector3f& sample) +{ + if(_sample_buffer == NULL) { + return false; + } + + float max_distance = fabsf(5.38709f * _sphere_param.named.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 2.0f; + + for (uint16_t i = 0; i<_samples_collected; i++){ + float distance = (sample - _sample_buffer[i].get()).length(); + if(distance < max_distance) { + return false; + } + } + return true; +} + +bool CompassCalibrator::accept_sample(const CompassSample& sample) { + return accept_sample(sample.get()); +} + +float CompassCalibrator::calc_residual(const Vector3f& sample, const sphere_param_t& sp, const ellipsoid_param_t& ep) const { + Matrix3f softiron( + ep.named.diag.x , ep.named.offdiag.x , ep.named.offdiag.y, + ep.named.offdiag.x , ep.named.diag.y , ep.named.offdiag.z, + ep.named.offdiag.y , ep.named.offdiag.z , ep.named.diag.z + ); + + return fabsf(sp.named.radius) - (softiron*(sample+sp.named.offset)).length(); +} + +float CompassCalibrator::calc_mean_squared_residuals() const +{ + return calc_mean_squared_residuals(_sphere_param,_ellipsoid_param); +} + +float CompassCalibrator::calc_mean_squared_residuals(const sphere_param_t& sp, const ellipsoid_param_t& ep) const +{ + if(_sample_buffer == NULL) { + return -1.0f; + } + float sum = 0.0f; + for(uint16_t i=0; i < _samples_collected; i++){ + Vector3f sample = _sample_buffer[i].get(); + float resid = calc_residual(sample, sp, ep); + sum += sq(resid); + } + sum /= _samples_collected; + return sum; +} + +void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const sphere_param_t& sp, sphere_param_t &ret) const +{ + const Vector3f &offset = sp.named.offset; + const Vector3f &diag = _ellipsoid_param.named.diag; + const Vector3f &offdiag = _ellipsoid_param.named.offdiag; + + ret.named.radius = sp.named.radius/fabsf(sp.named.radius); + + ret.named.offset.x = -((2.0f*offdiag.y*(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+2.0f*diag.x*(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+2.0f*offdiag.x*(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z)))/(2.0f*sqrtf(sq(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+sq(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+sq(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z))))); + ret.named.offset.y = -((2.0f*offdiag.z*(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+2.0f*offdiag.x*(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+2.0f*diag.y*(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z)))/(2.0f*sqrtf(sq(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+sq(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+sq(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z))))); + ret.named.offset.z = -((2.0f*diag.z*(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+2.0f*offdiag.y*(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+2.0f*offdiag.z*(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z)))/(2.0f*sqrtf(sq(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+sq(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+sq(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z))))); +} + +void CompassCalibrator::run_sphere_fit(uint8_t max_iterations) +{ + if(_sample_buffer == NULL) { + return; + } + float fitness = _fitness; + sphere_param_t fit_param = _sphere_param; + + for(uint8_t iterations=0; iterations smax) { + pipk = k - 1; + smax = s; + } + } + + if (A[c + pipk] != 0.0f) { + if (pipk != 0) { + ipiv[j] = (int32_t)((j + pipk) + 1); + ix = j; + pipk += j; + for (k = 0; k < 6; k++) { + smax = A[ix]; + A[ix] = A[pipk]; + A[pipk] = smax; + ix += 6; + pipk += 6; + } + } + + i0 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i0; jy++) { + A[jy] /= A[c]; + } + } + + pipk = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + smax = A[jy]; + if (A[jy] != 0.0f) { + ix = c + 1; + i0 = (pipk - j) + 12; + for (ijA = 7 + pipk; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 6; + pipk += 6; + } + } + + for (i0 = 0; i0 < 6; i0++) { + p[i0] = (int32_t)(1 + i0); + } + + for (k = 0; k < 5; k++) { + if (ipiv[k] > 1 + k) { + pipk = p[ipiv[k] - 1]; + p[ipiv[k] - 1] = p[k]; + p[k] = (int32_t)pipk; + } + } + + for (k = 0; k < 6; k++) { + y[k + 6 * (p[k] - 1)] = 1.0; + for (j = k; j + 1 < 7; j++) { + if (y[j + 6 * (p[k] - 1)] != 0.0f) { + for (jy = j + 1; jy + 1 < 7; jy++) { + y[jy + 6 * (p[k] - 1)] -= y[j + 6 * (p[k] - 1)] * A[jy + 6 * j]; + } + } + } + } + + for (j = 0; j < 6; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + pipk = 6 * k; + if (y[k + c] != 0.0f) { + y[k + c] /= A[k + pipk]; + for (jy = 0; jy + 1 <= k; jy++) { + y[jy + c] -= y[k + c] * A[jy + pipk]; + } + } + } + } + return true; +} + +bool CompassCalibrator::inverse3x3(float m[], float invOut[]) +{ + float inv[9]; + // computes the inverse of a matrix m + float det = m[0] * (m[4] * m[8] - m[7] * m[5]) - + m[1] * (m[3] * m[8] - m[5] * m[6]) + + m[2] * (m[3] * m[7] - m[4] * m[6]); + if(fabsf(det) < 1.0e-20f){ + return false; + } + + float invdet = 1 / det; + + inv[0] = (m[4] * m[8] - m[7] * m[5]) * invdet; + inv[1] = (m[2] * m[7] - m[1] * m[8]) * invdet; + inv[2] = (m[1] * m[5] - m[2] * m[4]) * invdet; + inv[3] = (m[5] * m[6] - m[5] * m[8]) * invdet; + inv[4] = (m[0] * m[8] - m[2] * m[6]) * invdet; + inv[5] = (m[3] * m[2] - m[0] * m[5]) * invdet; + inv[6] = (m[3] * m[7] - m[6] * m[4]) * invdet; + inv[7] = (m[6] * m[1] - m[0] * m[7]) * invdet; + inv[8] = (m[0] * m[4] - m[3] * m[1]) * invdet; + + for(uint8_t i = 0; i < 9; i++){ + invOut[i] = inv[i]; + } + + return true; +} + +/* + * matrix inverse code only for 4x4 square matrix copied from + * gluInvertMatrix implementation in + * opengl for 4x4 matrices. + * + * @param m, input 4x4 matrix + * @param invOut, Output inverted 4x4 matrix + * @returns false = matrix is Singular, true = matrix inversion successful + * Known Issues/ Possible Enhancements: + * -Will need a different implementation for more number + * of parameters like in the case of addition of soft + * iron calibration + */ +bool CompassCalibrator::inverse4x4(float m[],float invOut[]) +{ + float inv[16], det; + uint8_t i; + + inv[0] = m[5] * m[10] * m[15] - + m[5] * m[11] * m[14] - + m[9] * m[6] * m[15] + + m[9] * m[7] * m[14] + + m[13] * m[6] * m[11] - + m[13] * m[7] * m[10]; + + inv[4] = -m[4] * m[10] * m[15] + + m[4] * m[11] * m[14] + + m[8] * m[6] * m[15] - + m[8] * m[7] * m[14] - + m[12] * m[6] * m[11] + + m[12] * m[7] * m[10]; + + inv[8] = m[4] * m[9] * m[15] - + m[4] * m[11] * m[13] - + m[8] * m[5] * m[15] + + m[8] * m[7] * m[13] + + m[12] * m[5] * m[11] - + m[12] * m[7] * m[9]; + + inv[12] = -m[4] * m[9] * m[14] + + m[4] * m[10] * m[13] + + m[8] * m[5] * m[14] - + m[8] * m[6] * m[13] - + m[12] * m[5] * m[10] + + m[12] * m[6] * m[9]; + + inv[1] = -m[1] * m[10] * m[15] + + m[1] * m[11] * m[14] + + m[9] * m[2] * m[15] - + m[9] * m[3] * m[14] - + m[13] * m[2] * m[11] + + m[13] * m[3] * m[10]; + + inv[5] = m[0] * m[10] * m[15] - + m[0] * m[11] * m[14] - + m[8] * m[2] * m[15] + + m[8] * m[3] * m[14] + + m[12] * m[2] * m[11] - + m[12] * m[3] * m[10]; + + inv[9] = -m[0] * m[9] * m[15] + + m[0] * m[11] * m[13] + + m[8] * m[1] * m[15] - + m[8] * m[3] * m[13] - + m[12] * m[1] * m[11] + + m[12] * m[3] * m[9]; + + inv[13] = m[0] * m[9] * m[14] - + m[0] * m[10] * m[13] - + m[8] * m[1] * m[14] + + m[8] * m[2] * m[13] + + m[12] * m[1] * m[10] - + m[12] * m[2] * m[9]; + + inv[2] = m[1] * m[6] * m[15] - + m[1] * m[7] * m[14] - + m[5] * m[2] * m[15] + + m[5] * m[3] * m[14] + + m[13] * m[2] * m[7] - + m[13] * m[3] * m[6]; + + inv[6] = -m[0] * m[6] * m[15] + + m[0] * m[7] * m[14] + + m[4] * m[2] * m[15] - + m[4] * m[3] * m[14] - + m[12] * m[2] * m[7] + + m[12] * m[3] * m[6]; + + inv[10] = m[0] * m[5] * m[15] - + m[0] * m[7] * m[13] - + m[4] * m[1] * m[15] + + m[4] * m[3] * m[13] + + m[12] * m[1] * m[7] - + m[12] * m[3] * m[5]; + + inv[14] = -m[0] * m[5] * m[14] + + m[0] * m[6] * m[13] + + m[4] * m[1] * m[14] - + m[4] * m[2] * m[13] - + m[12] * m[1] * m[6] + + m[12] * m[2] * m[5]; + + inv[3] = -m[1] * m[6] * m[11] + + m[1] * m[7] * m[10] + + m[5] * m[2] * m[11] - + m[5] * m[3] * m[10] - + m[9] * m[2] * m[7] + + m[9] * m[3] * m[6]; + + inv[7] = m[0] * m[6] * m[11] - + m[0] * m[7] * m[10] - + m[4] * m[2] * m[11] + + m[4] * m[3] * m[10] + + m[8] * m[2] * m[7] - + m[8] * m[3] * m[6]; + + inv[11] = -m[0] * m[5] * m[11] + + m[0] * m[7] * m[9] + + m[4] * m[1] * m[11] - + m[4] * m[3] * m[9] - + m[8] * m[1] * m[7] + + m[8] * m[3] * m[5]; + + inv[15] = m[0] * m[5] * m[10] - + m[0] * m[6] * m[9] - + m[4] * m[1] * m[10] + + m[4] * m[2] * m[9] + + m[8] * m[1] * m[6] - + m[8] * m[2] * m[5]; + + det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; + + if(fabsf(det) < 1.0e-20f){ + return false; + } + + det = 1.0f / det; + + for (i = 0; i < 16; i++) + invOut[i] = inv[i] * det; + return true; +} + +float CompassCalibrator::det6x6(const float C[36]) +{ + float f; + float A[36]; + int8_t ipiv[6]; + int32_t i0; + int32_t j; + int32_t c; + int32_t iy; + int32_t ix; + float smax; + int32_t jy; + float s; + int32_t b_j; + int32_t ijA; + bool isodd; + memcpy(&A[0], &C[0], 36U * sizeof(float)); + for (i0 = 0; i0 < 6; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + smax = fabsf(A[c]); + for (jy = 2; jy <= 6 - j; jy++) { + ix++; + s = fabsf(A[ix]); + if (s > smax) { + iy = jy - 1; + smax = s; + } + } + + if (A[c + iy] != 0.0f) { + if (iy != 0) { + ipiv[j] = (int8_t)((j + iy) + 1); + ix = j; + iy += j; + for (jy = 0; jy < 6; jy++) { + smax = A[ix]; + A[ix] = A[iy]; + A[iy] = smax; + ix += 6; + iy += 6; + } + } + + i0 = (c - j) + 6; + for (iy = c + 1; iy + 1 <= i0; iy++) { + A[iy] /= A[c]; + } + } + + iy = c; + jy = c + 6; + for (b_j = 1; b_j <= 5 - j; b_j++) { + smax = A[jy]; + if (A[jy] != 0.0f) { + ix = c + 1; + i0 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + f = A[0]; + isodd = FALSE; + for (jy = 0; jy < 5; jy++) { + f *= A[(jy + 6 * (1 + jy)) + 1]; + if (ipiv[jy] > 1 + jy) { + isodd = !isodd; + } + } + + if (isodd) { + f = -f; + } + + return f; +} + +uint16_t CompassCalibrator::get_random(void) +{ + static uint32_t m_z = 1234; + static uint32_t m_w = 76542; + m_z = 36969 * (m_z & 65535) + (m_z >> 16); + m_w = 18000 * (m_w & 65535) + (m_w >> 16); + return ((m_z << 16) + m_w) & 0xFFFF; +} + +////////////////////////////////////////////////////////// +//////////// CompassSample public interface ////////////// +////////////////////////////////////////////////////////// + +Vector3f CompassCalibrator::CompassSample::get() const { + Vector3f out; + out.x = (float)x*2048.0f/32700.0f; + out.y = (float)y*2048.0f/32700.0f; + out.z = (float)z*2048.0f/32700.0f; + return out; +} + +void CompassCalibrator::CompassSample::set(const Vector3f &in) { + x = (int16_t)(in.x*32700.0f/2048.0f + 0.5f); + y = (int16_t)(in.y*32700.0f/2048.0f + 0.5f); + z = (int16_t)(in.z*32700.0f/2048.0f + 0.5f); +} diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h new file mode 100644 index 0000000000..e9e38d3245 --- /dev/null +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -0,0 +1,125 @@ +#include + +#define COMPASS_CAL_NUM_SPHERE_PARAMS 4 +#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 6 +#define COMPASS_CAL_NUM_SAMPLES 300 + +//RMS tolerance +#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f + +enum compass_cal_status_t { + COMPASS_CAL_NOT_STARTED=0, + COMPASS_CAL_WAITING_TO_START=1, + COMPASS_CAL_SAMPLING_STEP_ONE=2, + COMPASS_CAL_SAMPLING_STEP_TWO=3, + COMPASS_CAL_SUCCESS=4, + COMPASS_CAL_FAILED=5 +}; + +class CompassCalibrator { +public: + CompassCalibrator(); + + void start(bool retry=false, bool autosave=false, float delay=0.0f); + void clear(); + + void new_sample(const Vector3f &sample); + void run_fit_chunk(); + + void check_for_timeout(); + + void set_tolerance(float tolerance) { _tolerance = tolerance; } + + void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); + + bool running() const; + float get_completion_percent() const; + enum compass_cal_status_t get_status() const { return _status; } + float get_fitness() const { return sqrtf(_fitness); } + bool get_autosave() const { return _autosave; } + uint8_t get_attempt() const { return _attempt; } + +private: + union sphere_param_t { + sphere_param_t(){}; + struct { + float radius; + Vector3f offset; + } named; + + float array[COMPASS_CAL_NUM_SPHERE_PARAMS]; + }; + + union ellipsoid_param_t { + ellipsoid_param_t(){}; + struct { + Vector3f diag; + Vector3f offdiag; + } named; + + float array[COMPASS_CAL_NUM_ELLIPSOID_PARAMS]; + }; + + class CompassSample { + public: + Vector3f get() const; + void set(const Vector3f &in); + private: + int16_t x; + int16_t y; + int16_t z; + }; + + bool fit_acceptable(); + + bool _autosave; + + bool _retry; + uint8_t _attempt; + uint32_t _start_time_ms; + float _delay_start_sec; + + float calc_residual(const Vector3f& sample, const sphere_param_t& sp, const ellipsoid_param_t& ep) const; + float calc_mean_squared_residuals(const sphere_param_t& sp, const ellipsoid_param_t& ep) const; + float calc_mean_squared_residuals() const; + + void calc_sphere_jacob(const Vector3f& sample, const sphere_param_t& sp, sphere_param_t& ret) const; + void run_sphere_fit(uint8_t max_iterations=20); + + void calc_ellipsoid_jacob(const Vector3f& sample, const ellipsoid_param_t& sp, ellipsoid_param_t& ret) const; + void run_ellipsoid_fit(uint8_t max_iterations=20); + + // returns true if sample should be added to buffer + bool accept_sample(const Vector3f &sample); + bool accept_sample(const CompassSample &sample); + + void thin_samples(); + + bool set_status(compass_cal_status_t status); + void reset_state(); + + uint32_t _last_sample_ms; + + uint16_t _fit_step; + + enum compass_cal_status_t _status; + + CompassSample *_sample_buffer; + uint16_t _samples_collected; + uint16_t _samples_thinned; + + // mean squared residuals + float _fitness; + + float _tolerance; + + sphere_param_t _sphere_param; + ellipsoid_param_t _ellipsoid_param; + + // math helpers + bool inverse6x6(const float m[],float invOut[]); + float det6x6(const float m[]); + bool inverse4x4(float m[],float invOut[]); + bool inverse3x3(float m[], float invOut[]); + uint16_t get_random(); +};