From e16f1688945fee6602fcc76ca3ddfd406813323b Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 25 Feb 2015 18:01:10 -0800 Subject: [PATCH 01/39] AP_Compass: add optional publishing of raw compass fields --- libraries/AP_Compass/Compass.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 6038477c3aaf8..fd61a8eba11e6 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -123,6 +123,13 @@ class Compass const Vector3f &get_field(uint8_t i) const { return _field[i]; } const Vector3f &get_field(void) const { return get_field(get_primary()); } + virtual bool supports_raw_field() const { return false; } + virtual void get_raw_field(uint8_t i, Vector3f &field) const { } + void get_raw_field(Vector3f &field) const { get_raw_field(get_primary(),field); } + + virtual uint32_t last_raw_update_us(uint8_t i) const { return 0; } + uint32_t last_raw_update_us() const { return last_raw_update_us(get_primary()); } + /// Return the health of a compass bool healthy(uint8_t i) const { return _healthy[i]; } bool healthy(void) const { return healthy(get_primary()); } From 2f2ff5de3c0d700345f69ecacd7ec71cc0e5b28d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 25 Feb 2015 18:01:36 -0800 Subject: [PATCH 02/39] AP_Compass_PX4: publish raw compass measurements --- libraries/AP_Compass/AP_Compass_PX4.cpp | 49 ++++++++++++++++--------- libraries/AP_Compass/AP_Compass_PX4.h | 6 +++ 2 files changed, 37 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index 54d5a89122021..4807149d281b5 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -99,26 +99,16 @@ bool AP_Compass_PX4::read(void) for (uint8_t i=0; i<_num_instances; i++) { // avoid division by zero if we haven't received any mag reports - if (_count[i] == 0) continue; + if (_count[i] == 0) { + continue; + } + // take average of field measurements since last read() _sum[i] /= _count[i]; - _sum[i] *= 1000; - - // apply default board orientation for this compass type. This is - // a noop on most boards - _sum[i].rotate(MAG_BOARD_ORIENTATION); - if (_external[i]) { - // add user selectable orientation - _sum[i].rotate((enum Rotation)_orientation[i].get()); - } else { - // add in board orientation from AHRS - _sum[i].rotate(_board_orientation); - } - + // publish to _field _field[i] = _sum[i]; - apply_corrections(_field[i],i); - + _sum[i].zero(); _count[i] = 0; } @@ -132,12 +122,35 @@ void AP_Compass_PX4::accumulate(void) { struct mag_report mag_report; for (uint8_t i=0; i<_num_instances; i++) { + bool received_sample = false; + // get latest report while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) && mag_report.timestamp != _last_timestamp[i]) { - _sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z); - _count[i]++; _last_timestamp[i] = mag_report.timestamp; + received_sample = true; + } + + if(!received_sample) continue; + + _raw_field[i] = Vector3f(mag_report.x, mag_report.y, mag_report.z); + _raw_field[i] *= 1000; + + // apply default board orientation for this compass type. This is + // a noop on most boards + _raw_field[i].rotate(MAG_BOARD_ORIENTATION); + + if (_external[i]) { + // add user selectable orientation + _raw_field[i].rotate((enum Rotation)_orientation[i].get()); + } else { + // add in board orientation from AHRS + _raw_field[i].rotate(_board_orientation); } + + apply_corrections(_raw_field[i],i); + + _sum[i] += _raw_field[i]; + _count[i]++; } } diff --git a/libraries/AP_Compass/AP_Compass_PX4.h b/libraries/AP_Compass/AP_Compass_PX4.h index a01602c132eff..f12f61a5a4eff 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.h +++ b/libraries/AP_Compass/AP_Compass_PX4.h @@ -22,12 +22,18 @@ class AP_Compass_PX4 : public Compass // return the primary compass uint8_t get_primary(void) const; + bool supports_raw_field() const { return true; } + void get_raw_field(uint8_t i, Vector3f &field) const { field = _raw_field[i]; } + uint32_t last_raw_update_us(uint8_t i) const { return _last_timestamp[i]; } + private: uint8_t _num_instances; int _mag_fd[COMPASS_MAX_INSTANCES]; Vector3f _sum[COMPASS_MAX_INSTANCES]; uint32_t _count[COMPASS_MAX_INSTANCES]; uint64_t _last_timestamp[COMPASS_MAX_INSTANCES]; + + Vector3f _raw_field[COMPASS_MAX_INSTANCES]; }; #endif // AP_Compass_PX4_H From 5fc3d36c3afd1cc75481f64ff8857552874681fe Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 25 Feb 2015 19:14:15 -0800 Subject: [PATCH 03/39] Copter: run compass_accumulate at 100hz --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6408a4db84b25..79f2e8076da6f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -760,7 +760,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { run_nav_updates, 8, 80 }, { update_thr_average, 40, 10 }, { three_hz_loop, 133, 9 }, - { compass_accumulate, 8, 42 }, + { compass_accumulate, 4, 42 }, { barometer_accumulate, 8, 25 }, #if FRAME_CONFIG == HELI_FRAME { check_dynamic_flight, 8, 10 }, From eb12220cd716be68acbf0e4c1b0c0f91958787f7 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 7 Mar 2015 07:48:36 -0800 Subject: [PATCH 04/39] GCS_MAVLink: add MAG_CAL messages --- .../message_definitions/ardupilotmega.xml | 75 ++++++++++++++++++- 1 file changed, 74 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml index 28b9495a394b0..3c1463345dcd7 100644 --- a/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml @@ -33,6 +33,39 @@ Empty Empty + + + Initiate a magnetometer calibration + uint8_t bitmask of magnetometers (0 means all) + Automatically retry on failure (0=no retry, 1=retry). + Save without user input (0=require input, 1=autosave). + Delay (seconds) + Empty + Empty + Empty + + + + Initiate a magnetometer calibration + uint8_t bitmask of magnetometers (0 means all) + Empty + Empty + Empty + Empty + Empty + Empty + + + + Cancel a running magnetometer calibration + uint8_t bitmask of magnetometers (0 means all) + Empty + Empty + Empty + Empty + Empty + Empty + @@ -122,6 +155,15 @@ Custom Pattern using custom bytes fields + + + + + + + + + @@ -452,7 +494,7 @@ Camera Capture Feedback Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) - System ID + System ID Camera ID Image index Latitude in (deg * 1E7) @@ -529,6 +571,37 @@ Custom Byte Length Custom Bytes + + + Reports progress of compass calibration. + Compass being calibrated + Bitmask of compasses being calibrated + Status (see MAG_CAL_STATUS enum) + Attempt number + Completion percentage + Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + Body frame direction vector for display + Body frame direction vector for display + Body frame direction vector for display + + + + Reports results of completed compass calibration. Sent until MAG_CAL_ACK received. + Compass being calibrated + Bitmask of compasses being calibrated + Status (see MAG_CAL_STATUS enum) + 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + RMS milligauss residuals + X offset + Y offset + Z offset + X diagonal (matrix 11) + Y diagonal (matrix 22) + Z diagonal (matrix 33) + X off-diagonal (matrix 12 and 21) + Y off-diagonal (matrix 13 and 31) + Z off-diagonal (matrix 32 and 23) + From 5d71ac6766abfccfda0c8762ad05a8346cf076e4 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 7 Mar 2015 07:49:11 -0800 Subject: [PATCH 05/39] AP_Compass: add CompassCalibrator --- libraries/AP_Compass/CompassCalibrator.cpp | 851 +++++++++++++++++++++ libraries/AP_Compass/CompassCalibrator.h | 125 +++ 2 files changed, 976 insertions(+) create mode 100644 libraries/AP_Compass/CompassCalibrator.cpp create mode 100644 libraries/AP_Compass/CompassCalibrator.h diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp new file mode 100644 index 0000000000000..d79747a19bba6 --- /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 0000000000000..e9e38d3245465 --- /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(); +}; From d0b69b171cf32ac44295d8d9638ea946fccc7431 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 7 Mar 2015 07:50:06 -0800 Subject: [PATCH 06/39] AP_Compass: provide an interface for calibrating compasses --- libraries/AP_Compass/Compass.cpp | 203 +++++++++++++++++++++++++++++++ libraries/AP_Compass/Compass.h | 26 +++- 2 files changed, 227 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 5db2c854533f6..de649bc491805 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -1,6 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #include "Compass.h" +#include const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // index 0 was used for the old orientation matrix @@ -289,6 +290,205 @@ Compass::init() return true; } +bool +Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay) +{ + if(healthy(i)) { + _calibrator[i].start(retry, autosave, delay); + return true; + } else { + return false; + } +} + +bool +Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay) +{ + for(uint8_t i=0; i 1 + _dev_id[i].save(); +#endif + return true; + } else { + return false; + } +} + +bool +Compass::accept_calibration_mask(uint8_t mask) +{ + for(uint8_t i=0; i #include #include // ArduPilot Mega Declination Helper Library +#include +#include // compass product id #define AP_COMPASS_TYPE_UNKNOWN 0x00 @@ -78,12 +80,27 @@ class Compass /// virtual bool read(void) = 0; - - /// use spare CPU cycles to accumulate values from the compass if /// possible virtual void accumulate(void) = 0; + bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f); + bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f); + bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f); + + void cancel_calibration(uint8_t i); + void cancel_calibration_all(); + void cancel_calibration_mask(uint8_t mask); + + bool accept_calibration(uint8_t i); + bool accept_calibration_all(); + bool accept_calibration_mask(uint8_t mask); + + uint8_t get_healthy_mask() const; + + void send_mag_cal_progress(mavlink_channel_t chan); + void send_mag_cal_report(mavlink_channel_t chan); + /// Calculate the tilt-compensated heading_ variables. /// /// @param dcm_matrix The current orientation rotation matrix @@ -279,6 +296,8 @@ class Compass AP_Int32 _dev_id[COMPASS_MAX_INSTANCES]; // device id detected at init. saved to eeprom when offsets are saved allowing ram & eeprom values to be compared as consistency check #endif + CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]; + bool _null_init_done; ///< first-time-around flag used by offset nulling ///< used by offset correction @@ -296,5 +315,8 @@ class Compass enum Rotation _board_orientation; void apply_corrections(Vector3f &mag, uint8_t i); + +private: + uint8_t get_cal_mask(); }; #endif From 245adc31237c7c3f0ba251b64697bfa927456a01 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 7 Mar 2015 07:50:50 -0800 Subject: [PATCH 07/39] GCS_MAVLink: add MSG_MAG_CAL_PROGRESS and _REPORT --- libraries/GCS_MAVLink/GCS.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index f8c8f0a7d8aea..39169b9750dea 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -56,6 +56,8 @@ enum ap_message { MSG_MOUNT_STATUS, MSG_OPTICAL_FLOW, MSG_GIMBAL_REPORT, + MSG_MAG_CAL_PROGRESS, + MSG_MAG_CAL_REPORT, MSG_RETRY_DEFERRED // this must be last }; From b87c4f19ff8e67d7f7f2e1597882f62ef7e103d0 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 7 Mar 2015 07:51:34 -0800 Subject: [PATCH 08/39] GCS_MAVLink: run generate.sh --- .../v1.0/ardupilotmega/ardupilotmega.h | 28 +- .../ardupilotmega/mavlink_msg_mag_cal_ack.h | 281 ++++++++++ .../mavlink_msg_mag_cal_progress.h | 393 +++++++++++++ .../mavlink_msg_mag_cal_report.h | 521 ++++++++++++++++++ .../mavlink/v1.0/ardupilotmega/testsuite.h | 109 ++++ .../mavlink/v1.0/ardupilotmega/version.h | 2 +- .../include/mavlink/v1.0/common/version.h | 2 +- 7 files changed, 1330 insertions(+), 6 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_ack.h create mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h create mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 2f0ed0ed1e35f..119e740d0dae3 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -16,15 +16,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 37, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26, 185, 181, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 16, 0, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 22, 13, 255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 0, 0, 0, 0, 0, 0, 36, 60, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 29, 45, 4, 40, 2, 42, 26, 29, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 254, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 37, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26, 185, 181, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 16, 0, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 22, 13, 255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 0, 0, 0, 0, 0, 0, 36, 60, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 29, 45, 4, 40, 2, 42, 26, 29, 0, 0, 0, 0, 27, 44, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 254, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 82, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 0, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 0, 0, 0, 0, 0, 0, 154, 178, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 189, 52, 174, 229, 85, 97, 239, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 82, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 0, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 0, 0, 0, 0, 0, 0, 154, 178, 0, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 189, 52, 174, 229, 85, 97, 239, 72, 0, 0, 0, 0, 92, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, MAVLINK_MESSAGE_INFO_CAMERA_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK, MAVLINK_MESSAGE_INFO_BATTERY2, MAVLINK_MESSAGE_INFO_AHRS3, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION_REQUEST, MAVLINK_MESSAGE_INFO_GIMBAL_REPORT, MAVLINK_MESSAGE_INFO_GIMBAL_CONTROL, MAVLINK_MESSAGE_INFO_LED_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, MAVLINK_MESSAGE_INFO_CAMERA_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FEEDBACK, MAVLINK_MESSAGE_INFO_BATTERY2, MAVLINK_MESSAGE_INFO_AHRS3, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION_REQUEST, MAVLINK_MESSAGE_INFO_GIMBAL_REPORT, MAVLINK_MESSAGE_INFO_GIMBAL_CONTROL, MAVLINK_MESSAGE_INFO_LED_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MAG_CAL_PROGRESS, MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -102,7 +102,10 @@ typedef enum MAV_CMD MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle)| Viewing angle vertical of panorama (in degrees)| Speed of the horizontal rotation (in degrees per second)| Speed of the vertical rotation (in degrees per second)| */ MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in degrees compass heading (0..360). A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overriden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position in meters. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude, in meters AMSL| */ MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deploment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_ENUM_END=30003, /* | */ + MAV_CMD_DO_START_MAG_CAL=42424, /* Initiate a magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Automatically retry on failure (0=no retry, 1=retry).| Save without user input (0=require input, 1=autosave).| Delay (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_ACCEPT_MAG_CAL=42425, /* Initiate a magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CANCEL_MAG_CAL=42426, /* Cancel a running magnetometer calibration |uint8_t bitmask of magnetometers (0 means all)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_ENUM_END=42427, /* | */ } MAV_CMD; #endif @@ -220,6 +223,21 @@ typedef enum LED_CONTROL_PATTERN } LED_CONTROL_PATTERN; #endif +/** @brief */ +#ifndef HAVE_ENUM_MAG_CAL_STATUS +#define HAVE_ENUM_MAG_CAL_STATUS +typedef enum MAG_CAL_STATUS +{ + MAG_CAL_NOT_STARTED=0, /* | */ + MAG_CAL_WAITING_TO_START=1, /* | */ + MAG_CAL_SAMPLING_STEP_ONE=2, /* | */ + MAG_CAL_SAMPLING_STEP_TWO=3, /* | */ + MAG_CAL_SUCCESS=4, /* | */ + MAG_CAL_FAILED=5, /* | */ + MAG_CAL_STATUS_ENUM_END=6, /* | */ +} MAG_CAL_STATUS; +#endif + #include "../common/common.h" // MAVLINK VERSION @@ -270,6 +288,8 @@ typedef enum LED_CONTROL_PATTERN #include "./mavlink_msg_gimbal_report.h" #include "./mavlink_msg_gimbal_control.h" #include "./mavlink_msg_led_control.h" +#include "./mavlink_msg_mag_cal_progress.h" +#include "./mavlink_msg_mag_cal_report.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_ack.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_ack.h new file mode 100644 index 0000000000000..9a2e8ab3feba8 --- /dev/null +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_ack.h @@ -0,0 +1,281 @@ +// MESSAGE MAG_CAL_ACK PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_ACK 193 + +typedef struct __mavlink_mag_cal_ack_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t compass_id; ///< Compass ID + uint8_t accept_cal; ///< 0=Discard the calibration, 1=Save the calibration +} mavlink_mag_cal_ack_t; + +#define MAVLINK_MSG_ID_MAG_CAL_ACK_LEN 4 +#define MAVLINK_MSG_ID_193_LEN 4 + +#define MAVLINK_MSG_ID_MAG_CAL_ACK_CRC 133 +#define MAVLINK_MSG_ID_193_CRC 133 + + + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_ACK { \ + "MAG_CAL_ACK", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mag_cal_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mag_cal_ack_t, target_component) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mag_cal_ack_t, compass_id) }, \ + { "accept_cal", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mag_cal_ack_t, accept_cal) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param compass_id Compass ID + * @param accept_cal 0=Discard the calibration, 1=Save the calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t compass_id, uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#else + mavlink_mag_cal_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.compass_id = compass_id; + packet.accept_cal = accept_cal; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param compass_id Compass ID + * @param accept_cal 0=Discard the calibration, 1=Save the calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t compass_id,uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#else + mavlink_mag_cal_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.compass_id = compass_id; + packet.accept_cal = accept_cal; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_ack_t* mag_cal_ack) +{ + return mavlink_msg_mag_cal_ack_pack(system_id, component_id, msg, mag_cal_ack->target_system, mag_cal_ack->target_component, mag_cal_ack->compass_id, mag_cal_ack->accept_cal); +} + +/** + * @brief Encode a mag_cal_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_ack_t* mag_cal_ack) +{ + return mavlink_msg_mag_cal_ack_pack_chan(system_id, component_id, chan, msg, mag_cal_ack->target_system, mag_cal_ack->target_component, mag_cal_ack->compass_id, mag_cal_ack->accept_cal); +} + +/** + * @brief Send a mag_cal_ack message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param compass_id Compass ID + * @param accept_cal 0=Discard the calibration, 1=Save the calibration + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t compass_id, uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#else + mavlink_mag_cal_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.compass_id = compass_id; + packet.accept_cal = accept_cal; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t compass_id, uint8_t accept_cal) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, compass_id); + _mav_put_uint8_t(buf, 3, accept_cal); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, buf, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#else + mavlink_mag_cal_ack_t *packet = (mavlink_mag_cal_ack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->compass_id = compass_id; + packet->accept_cal = accept_cal; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN, MAVLINK_MSG_ID_MAG_CAL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_ACK, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_ACK UNPACKING + + +/** + * @brief Get field target_system from mag_cal_ack message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from mag_cal_ack message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field compass_id from mag_cal_ack message + * + * @return Compass ID + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field accept_cal from mag_cal_ack message + * + * @return 0=Discard the calibration, 1=Save the calibration + */ +static inline uint8_t mavlink_msg_mag_cal_ack_get_accept_cal(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Decode a mag_cal_ack message into a struct + * + * @param msg The message to decode + * @param mag_cal_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_ack_decode(const mavlink_message_t* msg, mavlink_mag_cal_ack_t* mag_cal_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_ack->target_system = mavlink_msg_mag_cal_ack_get_target_system(msg); + mag_cal_ack->target_component = mavlink_msg_mag_cal_ack_get_target_component(msg); + mag_cal_ack->compass_id = mavlink_msg_mag_cal_ack_get_compass_id(msg); + mag_cal_ack->accept_cal = mavlink_msg_mag_cal_ack_get_accept_cal(msg); +#else + memcpy(mag_cal_ack, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_ACK_LEN); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h new file mode 100644 index 0000000000000..c15853ff8361f --- /dev/null +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_progress.h @@ -0,0 +1,393 @@ +// MESSAGE MAG_CAL_PROGRESS PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS 191 + +typedef struct __mavlink_mag_cal_progress_t +{ + float direction_x; ///< Body frame direction vector for display + float direction_y; ///< Body frame direction vector for display + float direction_z; ///< Body frame direction vector for display + uint8_t compass_id; ///< Compass being calibrated + uint8_t cal_mask; ///< Bitmask of compasses being calibrated + uint8_t cal_status; ///< Status (see MAG_CAL_STATUS enum) + uint8_t attempt; ///< Attempt number + uint8_t completion_pct; ///< Completion percentage + uint8_t completion_mask[10]; ///< Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) +} mavlink_mag_cal_progress_t; + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN 27 +#define MAVLINK_MSG_ID_191_LEN 27 + +#define MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC 92 +#define MAVLINK_MSG_ID_191_CRC 92 + +#define MAVLINK_MSG_MAG_CAL_PROGRESS_FIELD_COMPLETION_MASK_LEN 10 + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_PROGRESS { \ + "MAG_CAL_PROGRESS", \ + 9, \ + { { "direction_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_progress_t, direction_x) }, \ + { "direction_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_progress_t, direction_y) }, \ + { "direction_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_progress_t, direction_z) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_mag_cal_progress_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_mag_cal_progress_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_mag_cal_progress_t, cal_status) }, \ + { "attempt", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_mag_cal_progress_t, attempt) }, \ + { "completion_pct", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_mag_cal_progress_t, completion_pct) }, \ + { "completion_mask", NULL, MAVLINK_TYPE_UINT8_T, 10, 17, offsetof(mavlink_mag_cal_progress_t, completion_mask) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_progress message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_PROGRESS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_progress message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t attempt,uint8_t completion_pct,const uint8_t *completion_mask,float direction_x,float direction_y,float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_PROGRESS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_progress struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_progress C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_progress_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_progress_t* mag_cal_progress) +{ + return mavlink_msg_mag_cal_progress_pack(system_id, component_id, msg, mag_cal_progress->compass_id, mag_cal_progress->cal_mask, mag_cal_progress->cal_status, mag_cal_progress->attempt, mag_cal_progress->completion_pct, mag_cal_progress->completion_mask, mag_cal_progress->direction_x, mag_cal_progress->direction_y, mag_cal_progress->direction_z); +} + +/** + * @brief Encode a mag_cal_progress struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_progress C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_progress_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_progress_t* mag_cal_progress) +{ + return mavlink_msg_mag_cal_progress_pack_chan(system_id, component_id, chan, msg, mag_cal_progress->compass_id, mag_cal_progress->cal_mask, mag_cal_progress->cal_status, mag_cal_progress->attempt, mag_cal_progress->completion_pct, mag_cal_progress->completion_mask, mag_cal_progress->direction_x, mag_cal_progress->direction_y, mag_cal_progress->direction_z); +} + +/** + * @brief Send a mag_cal_progress message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param attempt Attempt number + * @param completion_pct Completion percentage + * @param completion_mask Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + * @param direction_x Body frame direction vector for display + * @param direction_y Body frame direction vector for display + * @param direction_z Body frame direction vector for display + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_progress_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN]; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#else + mavlink_mag_cal_progress_t packet; + packet.direction_x = direction_x; + packet.direction_y = direction_y; + packet.direction_z = direction_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.attempt = attempt; + packet.completion_pct = completion_pct; + mav_array_memcpy(packet.completion_mask, completion_mask, sizeof(uint8_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_progress_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t attempt, uint8_t completion_pct, const uint8_t *completion_mask, float direction_x, float direction_y, float direction_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, direction_x); + _mav_put_float(buf, 4, direction_y); + _mav_put_float(buf, 8, direction_z); + _mav_put_uint8_t(buf, 12, compass_id); + _mav_put_uint8_t(buf, 13, cal_mask); + _mav_put_uint8_t(buf, 14, cal_status); + _mav_put_uint8_t(buf, 15, attempt); + _mav_put_uint8_t(buf, 16, completion_pct); + _mav_put_uint8_t_array(buf, 17, completion_mask, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, buf, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#else + mavlink_mag_cal_progress_t *packet = (mavlink_mag_cal_progress_t *)msgbuf; + packet->direction_x = direction_x; + packet->direction_y = direction_y; + packet->direction_z = direction_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->attempt = attempt; + packet->completion_pct = completion_pct; + mav_array_memcpy(packet->completion_mask, completion_mask, sizeof(uint8_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_PROGRESS, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_PROGRESS UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_progress message + * + * @return Compass being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field cal_mask from mag_cal_progress message + * + * @return Bitmask of compasses being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field cal_status from mag_cal_progress message + * + * @return Status (see MAG_CAL_STATUS enum) + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field attempt from mag_cal_progress message + * + * @return Attempt number + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_attempt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field completion_pct from mag_cal_progress message + * + * @return Completion percentage + */ +static inline uint8_t mavlink_msg_mag_cal_progress_get_completion_pct(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field completion_mask from mag_cal_progress message + * + * @return Bitmask of sphere sections (see http://en.wikipedia.org/wiki/Geodesic_grid) + */ +static inline uint16_t mavlink_msg_mag_cal_progress_get_completion_mask(const mavlink_message_t* msg, uint8_t *completion_mask) +{ + return _MAV_RETURN_uint8_t_array(msg, completion_mask, 10, 17); +} + +/** + * @brief Get field direction_x from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field direction_y from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field direction_z from mag_cal_progress message + * + * @return Body frame direction vector for display + */ +static inline float mavlink_msg_mag_cal_progress_get_direction_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Decode a mag_cal_progress message into a struct + * + * @param msg The message to decode + * @param mag_cal_progress C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_progress_decode(const mavlink_message_t* msg, mavlink_mag_cal_progress_t* mag_cal_progress) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_progress->direction_x = mavlink_msg_mag_cal_progress_get_direction_x(msg); + mag_cal_progress->direction_y = mavlink_msg_mag_cal_progress_get_direction_y(msg); + mag_cal_progress->direction_z = mavlink_msg_mag_cal_progress_get_direction_z(msg); + mag_cal_progress->compass_id = mavlink_msg_mag_cal_progress_get_compass_id(msg); + mag_cal_progress->cal_mask = mavlink_msg_mag_cal_progress_get_cal_mask(msg); + mag_cal_progress->cal_status = mavlink_msg_mag_cal_progress_get_cal_status(msg); + mag_cal_progress->attempt = mavlink_msg_mag_cal_progress_get_attempt(msg); + mag_cal_progress->completion_pct = mavlink_msg_mag_cal_progress_get_completion_pct(msg); + mavlink_msg_mag_cal_progress_get_completion_mask(msg, mag_cal_progress->completion_mask); +#else + memcpy(mag_cal_progress, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_PROGRESS_LEN); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h new file mode 100644 index 0000000000000..f8056b7715cbd --- /dev/null +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mag_cal_report.h @@ -0,0 +1,521 @@ +// MESSAGE MAG_CAL_REPORT PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT 192 + +typedef struct __mavlink_mag_cal_report_t +{ + float fitness; ///< RMS milligauss residuals + float ofs_x; ///< X offset + float ofs_y; ///< Y offset + float ofs_z; ///< Z offset + float diag_x; ///< X diagonal (matrix 11) + float diag_y; ///< Y diagonal (matrix 22) + float diag_z; ///< Z diagonal (matrix 33) + float offdiag_x; ///< X off-diagonal (matrix 12 and 21) + float offdiag_y; ///< Y off-diagonal (matrix 13 and 31) + float offdiag_z; ///< Z off-diagonal (matrix 32 and 23) + uint8_t compass_id; ///< Compass being calibrated + uint8_t cal_mask; ///< Bitmask of compasses being calibrated + uint8_t cal_status; ///< Status (see MAG_CAL_STATUS enum) + uint8_t autosaved; ///< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters +} mavlink_mag_cal_report_t; + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 44 +#define MAVLINK_MSG_ID_192_LEN 44 + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 +#define MAVLINK_MSG_ID_192_CRC 36 + + + +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + "MAG_CAL_REPORT", \ + 14, \ + { { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + } \ +} + + +/** + * @brief Pack a mag_cal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + +/** + * @brief Pack a mag_cal_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t autosaved,float fitness,float ofs_x,float ofs_y,float ofs_z,float diag_x,float diag_y,float diag_z,float offdiag_x,float offdiag_y,float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + +/** + * @brief Encode a mag_cal_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack(system_id, component_id, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); +} + +/** + * @brief Encode a mag_cal_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z); +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated + * @param cal_mask Bitmask of compasses being calibrated + * @param cal_status Status (see MAG_CAL_STATUS enum) + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + * @param fitness RMS milligauss residuals + * @param ofs_x X offset + * @param ofs_y Y offset + * @param ofs_z Z offset + * @param diag_x X diagonal (matrix 11) + * @param diag_y Y diagonal (matrix 22) + * @param diag_z Z diagonal (matrix 33) + * @param offdiag_x X off-diagonal (matrix 12 and 21) + * @param offdiag_y Y off-diagonal (matrix 13 and 31) + * @param offdiag_z Z off-diagonal (matrix 32 and 23) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_report_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#else + mavlink_mag_cal_report_t *packet = (mavlink_mag_cal_report_t *)msgbuf; + packet->fitness = fitness; + packet->ofs_x = ofs_x; + packet->ofs_y = ofs_y; + packet->ofs_z = ofs_z; + packet->diag_x = diag_x; + packet->diag_y = diag_y; + packet->diag_z = diag_z; + packet->offdiag_x = offdiag_x; + packet->offdiag_y = offdiag_y; + packet->offdiag_z = offdiag_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->autosaved = autosaved; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_REPORT UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_report message + * + * @return Compass being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field cal_mask from mag_cal_report message + * + * @return Bitmask of compasses being calibrated + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field cal_status from mag_cal_report message + * + * @return Status (see MAG_CAL_STATUS enum) + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field autosaved from mag_cal_report message + * + * @return 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field fitness from mag_cal_report message + * + * @return RMS milligauss residuals + */ +static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field ofs_x from mag_cal_report message + * + * @return X offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field ofs_y from mag_cal_report message + * + * @return Y offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field ofs_z from mag_cal_report message + * + * @return Z offset + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field diag_x from mag_cal_report message + * + * @return X diagonal (matrix 11) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field diag_y from mag_cal_report message + * + * @return Y diagonal (matrix 22) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field diag_z from mag_cal_report message + * + * @return Z diagonal (matrix 33) + */ +static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field offdiag_x from mag_cal_report message + * + * @return X off-diagonal (matrix 12 and 21) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field offdiag_y from mag_cal_report message + * + * @return Y off-diagonal (matrix 13 and 31) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field offdiag_z from mag_cal_report message + * + * @return Z off-diagonal (matrix 32 and 23) + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Decode a mag_cal_report message into a struct + * + * @param msg The message to decode + * @param mag_cal_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* msg, mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP + mag_cal_report->fitness = mavlink_msg_mag_cal_report_get_fitness(msg); + mag_cal_report->ofs_x = mavlink_msg_mag_cal_report_get_ofs_x(msg); + mag_cal_report->ofs_y = mavlink_msg_mag_cal_report_get_ofs_y(msg); + mag_cal_report->ofs_z = mavlink_msg_mag_cal_report_get_ofs_z(msg); + mag_cal_report->diag_x = mavlink_msg_mag_cal_report_get_diag_x(msg); + mag_cal_report->diag_y = mavlink_msg_mag_cal_report_get_diag_y(msg); + mag_cal_report->diag_z = mavlink_msg_mag_cal_report_get_diag_z(msg); + mag_cal_report->offdiag_x = mavlink_msg_mag_cal_report_get_offdiag_x(msg); + mag_cal_report->offdiag_y = mavlink_msg_mag_cal_report_get_offdiag_y(msg); + mag_cal_report->offdiag_z = mavlink_msg_mag_cal_report_get_offdiag_z(msg); + mag_cal_report->compass_id = mavlink_msg_mag_cal_report_get_compass_id(msg); + mag_cal_report->cal_mask = mavlink_msg_mag_cal_report_get_cal_mask(msg); + mag_cal_report->cal_status = mavlink_msg_mag_cal_report_get_cal_status(msg); + mag_cal_report->autosaved = mavlink_msg_mag_cal_report_get_autosaved(msg); +#else + memcpy(mag_cal_report, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 5c9797d065ca8..014efc76e3193 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1764,6 +1764,113 @@ static void mavlink_test_led_control(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_mag_cal_progress(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_mag_cal_progress_t packet_in = { + 17.0,45.0,73.0,41,108,175,242,53,{ 120, 121, 122, 123, 124, 125, 126, 127, 128, 129 } + }; + mavlink_mag_cal_progress_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.direction_x = packet_in.direction_x; + packet1.direction_y = packet_in.direction_y; + packet1.direction_z = packet_in.direction_z; + packet1.compass_id = packet_in.compass_id; + packet1.cal_mask = packet_in.cal_mask; + packet1.cal_status = packet_in.cal_status; + packet1.attempt = packet_in.attempt; + packet1.completion_pct = packet_in.completion_pct; + + mav_array_memcpy(packet1.completion_mask, packet_in.completion_mask, sizeof(uint8_t)*10); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_progress_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_mag_cal_progress_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_progress_pack(system_id, component_id, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.attempt , packet1.completion_pct , packet1.completion_mask , packet1.direction_x , packet1.direction_y , packet1.direction_z ); + mavlink_msg_mag_cal_progress_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_progress_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.attempt , packet1.completion_pct , packet1.completion_mask , packet1.direction_x , packet1.direction_y , packet1.direction_z ); + mavlink_msg_mag_cal_progress_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i Date: Sat, 7 Mar 2015 07:51:53 -0800 Subject: [PATCH 09/39] Copter: Hook up compass calibrator --- ArduCopter/GCS_Mavlink.pde | 74 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b03b829c2c5b7..37fca006b5c9d 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -636,6 +636,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_BATTERY2: break; // just here to prevent a warning + + case MSG_MAG_CAL_PROGRESS: + compass.send_mag_cal_progress(chan); + break; + + case MSG_MAG_CAL_REPORT: + compass.send_mag_cal_report(chan); + break; } return true; @@ -854,6 +862,8 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_MOUNT_STATUS); send_message(MSG_OPTICAL_FLOW); send_message(MSG_GIMBAL_REPORT); + send_message(MSG_MAG_CAL_REPORT); + send_message(MSG_MAG_CAL_PROGRESS); } } @@ -1305,6 +1315,70 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAV_CMD_DO_START_MAG_CAL: { + result = MAV_RESULT_ACCEPTED; + if(packet.param1 < 0 || packet.param1 > 255) { + result = MAV_RESULT_FAILED; + break; + } + + uint8_t mag_mask = packet.param1; + bool retry = packet.param2; + bool autosave = packet.param3; + float delay = packet.param4; + + if (mag_mask == 0) { // 0 means all + compass.start_calibration_all(retry, autosave, delay); + break; + } + + if(!compass.start_calibration_mask(mag_mask, retry, autosave, delay)) { + result = MAV_RESULT_FAILED; + } + + break; + } + + case MAV_CMD_DO_ACCEPT_MAG_CAL: { + result = MAV_RESULT_ACCEPTED; + if(packet.param1 < 0 || packet.param1 > 255) { + result = MAV_RESULT_FAILED; + break; + } + + uint8_t mag_mask = packet.param1; + + if (mag_mask == 0) { // 0 means all + if(!compass.accept_calibration_all()) { + result = MAV_RESULT_FAILED; + } + break; + } + + if(!compass.accept_calibration_mask(mag_mask)) { + result = MAV_RESULT_FAILED; + } + break; + } + + case MAV_CMD_DO_CANCEL_MAG_CAL: { + result = MAV_RESULT_ACCEPTED; + if(packet.param1 < 0 || packet.param1 > 255) { + result = MAV_RESULT_FAILED; + break; + } + + uint8_t mag_mask = packet.param1; + + if (mag_mask == 0) { // 0 means all + compass.cancel_calibration_all(); + break; + } + + compass.cancel_calibration_mask(mag_mask); + break; + } + default: result = MAV_RESULT_UNSUPPORTED; break; From af6b92e7ca72832b9c0d0dc9cc8d4ccaecf801b0 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 8 Mar 2015 23:22:52 +0530 Subject: [PATCH 10/39] Compass: Add Levenberg-Marquadt optimiser for sphere_fit increase iterations to get good results from LM better check for convergence, comparison with initial fitness is a better way to determine if convergence occurs, if fitness has not improved compared to initial fitness it means optimiser has failed. --- libraries/AP_Compass/CompassCalibrator.cpp | 61 ++++++++++++++++++---- libraries/AP_Compass/CompassCalibrator.h | 3 ++ 2 files changed, 54 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index d79747a19bba6..bf919fb9eca6b 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -26,6 +26,7 @@ void CompassCalibrator::start(bool retry, bool autosave, float delay) { _retry = retry; _delay_start_sec = delay; _start_time_ms = hal.scheduler->millis(); + _lambda = 1; set_status(COMPASS_CAL_WAITING_TO_START); } @@ -90,14 +91,16 @@ void CompassCalibrator::run_fit_chunk() { if(_fit_step == 0) { //initialize _fitness before starting a fit _fitness = calc_mean_squared_residuals(_sphere_param,_ellipsoid_param); + _lambda = 1.0f; + _initial_fitness = _fitness; } if(_status == COMPASS_CAL_SAMPLING_STEP_ONE) { - if(_fit_step < 7) { + if(_fit_step < 21) { run_sphere_fit(1); _fit_step++; } else { - if(_fitness > sq(_tolerance*50.0f)) { + if(_fitness == _initial_fitness) { //if true, means that fitness is diverging instead of converging set_status(COMPASS_CAL_FAILED); } @@ -105,13 +108,13 @@ void CompassCalibrator::run_fit_chunk() { } } else if(_status == COMPASS_CAL_SAMPLING_STEP_TWO) { if(_fit_step < 3) { - run_sphere_fit(1); + run_sphere_fit(3); } else if(_fit_step < 6) { - run_ellipsoid_fit(1); + run_ellipsoid_fit(3); } else if(_fit_step%2 == 0 && _fit_step < 35) { - run_sphere_fit(1); + run_sphere_fit(3); } else if(_fit_step%2 == 1 && _fit_step < 35) { - run_ellipsoid_fit(1); + run_ellipsoid_fit(3); } else { if(fit_acceptable()) { set_status(COMPASS_CAL_SUCCESS); @@ -130,6 +133,7 @@ void CompassCalibrator::reset_state() { _samples_collected = 0; _samples_thinned = 0; _fitness = -1.0f; + _lambda = 1.0f; _sphere_param.named.radius = 200; _sphere_param.named.offset.zero(); _ellipsoid_param.named.diag = Vector3f(1.0f,1.0f,1.0f); @@ -286,7 +290,7 @@ float CompassCalibrator::calc_residual(const Vector3f& sample, const sphere_para ep.named.offdiag.y , ep.named.offdiag.z , ep.named.diag.z ); - return fabsf(sp.named.radius) - (softiron*(sample+sp.named.offset)).length(); + return sp.named.radius - (softiron*(sample+sp.named.offset)).length(); } float CompassCalibrator::calc_mean_squared_residuals() const @@ -327,14 +331,16 @@ void CompassCalibrator::run_sphere_fit(uint8_t max_iterations) if(_sample_buffer == NULL) { return; } - float fitness = _fitness; - sphere_param_t fit_param = _sphere_param; + float fitness = _fitness, prevfitness = _fitness, fit1, fit2; + sphere_param_t fit_param = _sphere_param, temp_param; for(uint8_t iterations=0; iterations prevfitness && fit2 > prevfitness){ + _lambda *= 10.0f; + } else if(fit2 < prevfitness && fit2 < fit1) { + _lambda /= 10.0f; + fit_param = temp_param; + fitness = fit2; + } else if(fit1 < prevfitness){ + fitness = fit1; + } + prevfitness = fitness; + //--------------------Levenberg-part-ends-here--------------------------------// if(fitness < _fitness) { _fitness = fitness; _sphere_param = fit_param; diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index e9e38d3245465..6a6f9aa2889c2 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -110,9 +110,12 @@ class CompassCalibrator { // mean squared residuals float _fitness; + float _initial_fitness; float _tolerance; + float _lambda; + sphere_param_t _sphere_param; ellipsoid_param_t _ellipsoid_param; From fcad20fea7da41689bf6ea33930f1de71886b92c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 8 Mar 2015 23:27:52 +0530 Subject: [PATCH 11/39] Compass: Add conditions to check sanity of results --- libraries/AP_Compass/CompassCalibrator.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index bf919fb9eca6b..75f138767f541 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -234,8 +234,18 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { } bool CompassCalibrator::fit_acceptable() { - //TODO check all params - return _fitness <= sq(_tolerance); + if( _sphere_param.named.radius > 200 && //Earth's magnetic field strength range: 250-650mG + _sphere_param.named.radius < 700 && + fabsf(_sphere_param.named.offset.x) < 1000 && + fabsf(_sphere_param.named.offset.y) < 1000 && + fabsf(_sphere_param.named.offset.z) < 1000 && + fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1 + fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f && + fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f ){ + + return _fitness <= sq(_tolerance); + } + return false; } void CompassCalibrator::thin_samples() { From a1b6f1fb86a0f98a3ca51a4d9c7fc189b1f6cd00 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 8 Mar 2015 23:30:02 +0530 Subject: [PATCH 12/39] Compass: Add less complex equations to calculate jacobians --- libraries/AP_Compass/CompassCalibrator.cpp | 46 +++++++++++++++------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 75f138767f541..b27f6ba588204 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -323,17 +323,25 @@ float CompassCalibrator::calc_mean_squared_residuals(const sphere_param_t& sp, c return sum; } -void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const sphere_param_t& sp, sphere_param_t &ret) const -{ +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; + Matrix3f softiron( + diag.x , offdiag.x , offdiag.y, + offdiag.x , diag.y , offdiag.z, + offdiag.y , offdiag.z , diag.z + ); - ret.named.radius = sp.named.radius/fabsf(sp.named.radius); + float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z)); + float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z)); + float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); + float length = (softiron*(sample+offset)).length(); - 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))))); + ret.named.radius = 1; + ret.named.offset.x = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); + ret.named.offset.y = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); + ret.named.offset.z = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); } void CompassCalibrator::run_sphere_fit(uint8_t max_iterations) @@ -425,18 +433,28 @@ void CompassCalibrator::run_sphere_fit(uint8_t max_iterations) } -void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellipsoid_param_t& ep, ellipsoid_param_t &ret) const -{ + +void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellipsoid_param_t& ep, ellipsoid_param_t &ret) const{ const Vector3f &offset = _sphere_param.named.offset; const Vector3f &diag = ep.named.diag; const Vector3f &offdiag = ep.named.offdiag; + Matrix3f softiron( + diag.x , offdiag.x , offdiag.y, + offdiag.x , diag.y , offdiag.z, + offdiag.y , offdiag.z , diag.z + ); - ret.named.diag.x = -(((sample.x+offset.x)*(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z)))/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.diag.y = -(((sample.y+offset.y)*(offdiag.x*(sample.x+offset.x)+diag.y*(sample.y+offset.y)+offdiag.z*(sample.z+offset.z)))/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.diag.z = -(((sample.z+offset.z)*(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z)))/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.offdiag.x = -((2.0f*(sample.y+offset.y)*(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(sample.z+offset.z))+2.0f*(sample.x+offset.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.offdiag.y = -((2.0f*(sample.x+offset.x)*(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+2.0f*(sample.z+offset.z)*(diag.x*(sample.x+offset.x)+offdiag.x*(sample.y+offset.y)+offdiag.y*(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.offdiag.z = -((2.0f*(sample.y+offset.y)*(offdiag.y*(sample.x+offset.x)+offdiag.z*(sample.y+offset.y)+diag.z*(sample.z+offset.z))+2.0f*(sample.z+offset.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))))); + float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z)); + float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z)); + float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); + float length = (softiron*(sample+offset)).length(); + + ret.named.diag.x = -1.0f * ((sample.x + offset.x) * A)/length; + ret.named.diag.y = -1.0f * ((sample.y + offset.y) * B)/length; + ret.named.diag.z = -1.0f * ((sample.z + offset.z) * C)/length; + ret.named.offdiag.x = -1.0f * (((sample.y + offset.y) * A) + ((sample.z + offset.z) * B))/length; + ret.named.offdiag.y = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length; + ret.named.offdiag.z = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length; } void CompassCalibrator::run_ellipsoid_fit(uint8_t max_iterations) From cc7f90d61c54f26562f96f03fe641ab211488f8f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 8 Mar 2015 23:31:40 +0530 Subject: [PATCH 13/39] Compass: decrease sphere coverage to 1/3rd for faster sample collection --- libraries/AP_Compass/CompassCalibrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index b27f6ba588204..1c0d1d67dc529 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -278,7 +278,7 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample) return false; } - float max_distance = fabsf(5.38709f * _sphere_param.named.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 2.0f; + float max_distance = fabsf(5.38709f * _sphere_param.named.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; for (uint16_t i = 0; i<_samples_collected; i++){ float distance = (sample - _sample_buffer[i].get()).length(); From 615a594448d783c4d8eb7dc58acb36a5663ca436 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 9 Mar 2015 20:05:26 +0530 Subject: [PATCH 14/39] Compass: Add math for 9 parameter fitting --- libraries/AP_Compass/CompassCalibrator.cpp | 206 +++++++++++++++++++++ libraries/AP_Compass/CompassCalibrator.h | 5 +- 2 files changed, 210 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 1c0d1d67dc529..08adae800da7f 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -511,6 +511,123 @@ void CompassCalibrator::run_ellipsoid_fit(uint8_t max_iterations) ////////////////////////////////////////////////////////// ////////////////////// MATH HELPERS ////////////////////// ////////////////////////////////////////////////////////// +bool CompassCalibrator::inverse9x9(const float x[81], float y[81]) +{ + if(fabsf(det9x9(x)) < 1.0e-20f) { + return false; + } + float A[81]; + int32_t i0; + int8_t ipiv[9]; + int32_t j; + int32_t c; + int32_t pipk; + int32_t ix; + float smax; + int32_t k; + float s; + int32_t jy; + int32_t ijA; + int8_t p[9]; + for (i0 = 0; i0 < 81; i0++) { + A[i0] = x[i0]; + y[i0] = 0.0; + } + + for (i0 = 0; i0 < 9; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + pipk = 0; + ix = c; + smax = fabs(A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = fabs(A[ix]); + if (s > smax) { + pipk = k - 1; + smax = s; + } + } + + if (A[c + pipk] != 0.0) { + if (pipk != 0) { + ipiv[j] = (int8_t)((j + pipk) + 1); + ix = j; + pipk += j; + for (k = 0; k < 9; k++) { + smax = A[ix]; + A[ix] = A[pipk]; + A[pipk] = smax; + ix += 9; + pipk += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + A[jy] /= A[c]; + } + } + + pipk = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + smax = A[jy]; + if (A[jy] != 0.0) { + ix = c + 1; + i0 = (pipk - j) + 18; + for (ijA = 10 + pipk; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 9; + pipk += 9; + } + } + + for (i0 = 0; i0 < 9; i0++) { + p[i0] = (int8_t)(1 + i0); + } + + for (k = 0; k < 8; k++) { + if (ipiv[k] > 1 + k) { + pipk = p[ipiv[k] - 1]; + p[ipiv[k] - 1] = p[k]; + p[k] = (int8_t)pipk; + } + } + + for (k = 0; k < 9; k++) { + y[k + 9 * (p[k] - 1)] = 1.0; + for (j = k; j + 1 < 10; j++) { + if (y[j + 9 * (p[k] - 1)] != 0.0) { + for (jy = j + 1; jy + 1 < 10; jy++) { + y[jy + 9 * (p[k] - 1)] -= y[j + 9 * (p[k] - 1)] * A[jy + 9 * j]; + } + } + } + } + + for (j = 0; j < 9; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + pipk = 9 * k; + if (y[k + c] != 0.0) { + 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::inverse6x6(const float x[], float y[]) { @@ -892,6 +1009,95 @@ float CompassCalibrator::det6x6(const float C[36]) return f; } +float CompassCalibrator::det9x9(const float C[81]) +{ + float f; + float A[81]; + int8_t ipiv[9]; + 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], 81U * sizeof(float)); + for (i0 = 0; i0 < 9; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + smax = fabs(A[c]); + for (jy = 2; jy <= 9 - j; jy++) { + ix++; + s = fabs(A[ix]); + if (s > smax) { + iy = jy - 1; + smax = s; + } + } + + if (A[c + iy] != 0.0) { + if (iy != 0) { + ipiv[j] = (int8_t)((j + iy) + 1); + ix = j; + iy += j; + for (jy = 0; jy < 9; jy++) { + smax = A[ix]; + A[ix] = A[iy]; + A[iy] = smax; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (iy = c + 1; iy + 1 <= i0; iy++) { + A[iy] /= A[c]; + } + } + + iy = c; + jy = c + 9; + for (b_j = 1; b_j <= 8 - j; b_j++) { + smax = A[jy]; + if (A[jy] != 0.0) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + f = A[0]; + isodd = FALSE; + for (jy = 0; jy < 8; jy++) { + f *= A[(jy + 9 * (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; diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 6a6f9aa2889c2..73fc479557d2a 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -118,8 +118,11 @@ class CompassCalibrator { sphere_param_t _sphere_param; ellipsoid_param_t _ellipsoid_param; - + bool _running_ellipsoid_fit:1; // math helpers + + bool inverse9x9(const float m[],float invOut[]); + float det9x9(const float m[]); bool inverse6x6(const float m[],float invOut[]); float det6x6(const float m[]); bool inverse4x4(float m[],float invOut[]); From dc4f9329e31973b664127f25657ac8e075cd33e9 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 9 Mar 2015 20:07:12 +0530 Subject: [PATCH 15/39] Compass: implement 9 parameter ellipsoid fit --- libraries/AP_Compass/CompassCalibrator.cpp | 32 ++++++++++++++-------- libraries/AP_Compass/CompassCalibrator.h | 3 +- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 08adae800da7f..63c19603b03e3 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -97,6 +97,7 @@ void CompassCalibrator::run_fit_chunk() { if(_status == COMPASS_CAL_SAMPLING_STEP_ONE) { if(_fit_step < 21) { + _running_ellipsoid_fit = false; run_sphere_fit(1); _fit_step++; } else { @@ -105,16 +106,19 @@ void CompassCalibrator::run_fit_chunk() { } set_status(COMPASS_CAL_SAMPLING_STEP_TWO); + _lambda = 1.0f; //reset lambda for ellipsoid fitness + _ellipsoid_param.named.offset = _sphere_param.named.offset; } } else if(_status == COMPASS_CAL_SAMPLING_STEP_TWO) { - if(_fit_step < 3) { - run_sphere_fit(3); - } else if(_fit_step < 6) { - run_ellipsoid_fit(3); - } else if(_fit_step%2 == 0 && _fit_step < 35) { - run_sphere_fit(3); - } else if(_fit_step%2 == 1 && _fit_step < 35) { - run_ellipsoid_fit(3); + if(_fit_step < 21) { + _running_ellipsoid_fit = true; + run_ellipsoid_fit(1); + } else if(_fit_step == 21){ + _lambda = 1.0f; //reset lambda for sphere fitness + _sphere_param.named.offset = _ellipsoid_param.named.offset; + } else if(_fit_step < 42){ + _running_ellipsoid_fit = false; + run_sphere_fit(1); } else { if(fit_acceptable()) { set_status(COMPASS_CAL_SUCCESS); @@ -299,8 +303,11 @@ float CompassCalibrator::calc_residual(const Vector3f& sample, const sphere_para 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 sp.named.radius - (softiron*(sample+sp.named.offset)).length(); + if(_running_ellipsoid_fit){ + return sp.named.radius - (softiron*(sample+ep.named.offset)).length(); + } else{ + return sp.named.radius - (softiron*(sample+sp.named.offset)).length(); + } } float CompassCalibrator::calc_mean_squared_residuals() const @@ -435,7 +442,7 @@ void CompassCalibrator::run_sphere_fit(uint8_t max_iterations) void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellipsoid_param_t& ep, ellipsoid_param_t &ret) const{ - const Vector3f &offset = _sphere_param.named.offset; + const Vector3f &offset = ep.named.offset; const Vector3f &diag = ep.named.diag; const Vector3f &offdiag = ep.named.offdiag; Matrix3f softiron( @@ -449,6 +456,9 @@ void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellip float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); float length = (softiron*(sample+offset)).length(); + ret.named.offset.x = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); + ret.named.offset.y = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); + ret.named.offset.z = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); ret.named.diag.x = -1.0f * ((sample.x + offset.x) * A)/length; ret.named.diag.y = -1.0f * ((sample.y + offset.y) * B)/length; ret.named.diag.z = -1.0f * ((sample.z + offset.z) * C)/length; diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 73fc479557d2a..152a74a5e04ee 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -1,7 +1,7 @@ #include #define COMPASS_CAL_NUM_SPHERE_PARAMS 4 -#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 6 +#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9 #define COMPASS_CAL_NUM_SAMPLES 300 //RMS tolerance @@ -53,6 +53,7 @@ class CompassCalibrator { union ellipsoid_param_t { ellipsoid_param_t(){}; struct { + Vector3f offset; Vector3f diag; Vector3f offdiag; } named; From 0c815da95d3e81f68d9c4757d609501516ccedc6 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 9 Mar 2015 20:08:05 +0530 Subject: [PATCH 16/39] Compass: Add Levenberg-Marquadt for ellipsoid fit --- libraries/AP_Compass/CompassCalibrator.cpp | 48 +++++++++++++++++++--- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 63c19603b03e3..d2c5cfab6dc34 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -472,14 +472,16 @@ void CompassCalibrator::run_ellipsoid_fit(uint8_t max_iterations) if(_sample_buffer == NULL) { return; } - float fitness = _fitness; - ellipsoid_param_t fit_param = _ellipsoid_param; + float fitness = _fitness, prevfitness = _fitness, fit1, fit2; + ellipsoid_param_t fit_param = _ellipsoid_param, temp_param; for(uint8_t iterations=0; iterations prevfitness && fit2 > prevfitness){ + _lambda *= 10.0f; + } else if(fit2 < prevfitness && fit2 < fit1) { + _lambda /= 10.0f; + fit_param = temp_param; + fitness = fit2; + } else if(fit1 < prevfitness){ + fitness = fit1; + } + prevfitness = fitness; + //--------------------Levenberg-part-ends-here--------------------------------// if(fitness < _fitness) { _fitness = fitness; From 1d10df4b786c1a209e735a31734d3e60b26d1d64 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:02:46 -0700 Subject: [PATCH 17/39] AP_Compass: changes and fixes to LMA-based compass calibrate --- libraries/AP_Compass/CompassCalibrator.cpp | 444 ++++++++++----------- libraries/AP_Compass/CompassCalibrator.h | 104 +++-- 2 files changed, 269 insertions(+), 279 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index d2c5cfab6dc34..309a5f2241d17 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -1,5 +1,6 @@ #include "CompassCalibrator.h" #include +#include extern const AP_HAL::HAL& hal; @@ -26,7 +27,6 @@ void CompassCalibrator::start(bool retry, bool autosave, float delay) { _retry = retry; _delay_start_sec = delay; _start_time_ms = hal.scheduler->millis(); - _lambda = 1; set_status(COMPASS_CAL_WAITING_TO_START); } @@ -35,9 +35,9 @@ void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, return; } - offsets = _sphere_param.named.offset; - diagonals = _ellipsoid_param.named.diag; - offdiagonals = _ellipsoid_param.named.offdiag; + offsets = _params.offset; + diagonals = _params.diag; + offdiagonals = _params.offdiag; } float CompassCalibrator::get_completion_percent() const { @@ -47,9 +47,9 @@ float CompassCalibrator::get_completion_percent() const { case COMPASS_CAL_NOT_STARTED: case COMPASS_CAL_WAITING_TO_START: return 0.0f; - case COMPASS_CAL_SAMPLING_STEP_ONE: + case COMPASS_CAL_RUNNING_STEP_ONE: return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES; - case COMPASS_CAL_SAMPLING_STEP_TWO: + case COMPASS_CAL_RUNNING_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; @@ -59,10 +59,6 @@ float CompassCalibrator::get_completion_percent() const { }; } -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) { @@ -74,7 +70,7 @@ 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); + set_status(COMPASS_CAL_RUNNING_STEP_ONE); } if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { @@ -84,41 +80,26 @@ void CompassCalibrator::new_sample(const Vector3f& sample) { } void CompassCalibrator::run_fit_chunk() { - if(!running() || _samples_collected != COMPASS_CAL_NUM_SAMPLES) { + if(!fitting()) { return; } - if(_fit_step == 0) { - //initialize _fitness before starting a fit - _fitness = calc_mean_squared_residuals(_sphere_param,_ellipsoid_param); - _lambda = 1.0f; - _initial_fitness = _fitness; - } - - if(_status == COMPASS_CAL_SAMPLING_STEP_ONE) { - if(_fit_step < 21) { - _running_ellipsoid_fit = false; - run_sphere_fit(1); + if(_status == COMPASS_CAL_RUNNING_STEP_ONE) { + if(_fit_step < 10) { + run_sphere_fit(); _fit_step++; } else { - if(_fitness == _initial_fitness) { //if true, means that fitness is diverging instead of converging + if(_fitness == _initial_fitness || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging set_status(COMPASS_CAL_FAILED); } - set_status(COMPASS_CAL_SAMPLING_STEP_TWO); - _lambda = 1.0f; //reset lambda for ellipsoid fitness - _ellipsoid_param.named.offset = _sphere_param.named.offset; + set_status(COMPASS_CAL_RUNNING_STEP_TWO); } - } else if(_status == COMPASS_CAL_SAMPLING_STEP_TWO) { - if(_fit_step < 21) { - _running_ellipsoid_fit = true; - run_ellipsoid_fit(1); - } else if(_fit_step == 21){ - _lambda = 1.0f; //reset lambda for sphere fitness - _sphere_param.named.offset = _ellipsoid_param.named.offset; - } else if(_fit_step < 42){ - _running_ellipsoid_fit = false; - run_sphere_fit(1); + } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) { + if(_fit_step < 15) { + run_sphere_fit(); + } else if(_fit_step < 35) { + run_ellipsoid_fit(); } else { if(fit_acceptable()) { set_status(COMPASS_CAL_SUCCESS); @@ -133,16 +114,36 @@ void CompassCalibrator::run_fit_chunk() { ///////////////////////////////////////////////////////////// ////////////////////// PRIVATE METHODS ////////////////////// ///////////////////////////////////////////////////////////// +bool CompassCalibrator::running() const { + return _status == COMPASS_CAL_RUNNING_STEP_ONE || _status == COMPASS_CAL_RUNNING_STEP_TWO; +} + +bool CompassCalibrator::fitting() const { + return running() && _samples_collected == COMPASS_CAL_NUM_SAMPLES; +} + +void CompassCalibrator::initialize_fit() { + //initialize _fitness before starting a fit + if (_samples_collected != 0) { + _fitness = calc_mean_squared_residuals(_params); + } else { + _fitness = 1.0e30f; + } + _ellipsoid_lambda = 1.0f; + _sphere_lambda = 1.0f; + _initial_fitness = _fitness; + _fit_step = 0; +} + void CompassCalibrator::reset_state() { _samples_collected = 0; _samples_thinned = 0; - _fitness = -1.0f; - _lambda = 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; + _params.radius = 200; + _params.offset.zero(); + _params.diag = Vector3f(1.0f,1.0f,1.0f); + _params.offdiag.zero(); + + initialize_fit(); } bool CompassCalibrator::set_status(compass_cal_status_t status) { @@ -165,10 +166,10 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { reset_state(); _status = COMPASS_CAL_WAITING_TO_START; - set_status(COMPASS_CAL_SAMPLING_STEP_ONE); + set_status(COMPASS_CAL_RUNNING_STEP_ONE); return true; - case COMPASS_CAL_SAMPLING_STEP_ONE: + case COMPASS_CAL_RUNNING_STEP_ONE: if(_status != COMPASS_CAL_WAITING_TO_START) { return false; } @@ -177,32 +178,33 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { return false; } - if(_sample_buffer != NULL) { - _status = COMPASS_CAL_SAMPLING_STEP_ONE; + initialize_fit(); + _status = COMPASS_CAL_RUNNING_STEP_ONE; return true; } _sample_buffer = (CompassSample*)malloc(sizeof(CompassSample)*COMPASS_CAL_NUM_SAMPLES); if(_sample_buffer != NULL) { - _status = COMPASS_CAL_SAMPLING_STEP_ONE; + initialize_fit(); + _status = COMPASS_CAL_RUNNING_STEP_ONE; return true; } return false; - case COMPASS_CAL_SAMPLING_STEP_TWO: - if(_status != COMPASS_CAL_SAMPLING_STEP_ONE) { + case COMPASS_CAL_RUNNING_STEP_TWO: + if(_status != COMPASS_CAL_RUNNING_STEP_ONE) { return false; } - _fit_step = 0; thin_samples(); - _status = COMPASS_CAL_SAMPLING_STEP_TWO; + initialize_fit(); + _status = COMPASS_CAL_RUNNING_STEP_TWO; return true; case COMPASS_CAL_SUCCESS: - if(_status != COMPASS_CAL_SAMPLING_STEP_TWO) { + if(_status != COMPASS_CAL_RUNNING_STEP_TWO) { return false; } @@ -238,14 +240,15 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { } bool CompassCalibrator::fit_acceptable() { - if( _sphere_param.named.radius > 200 && //Earth's magnetic field strength range: 250-650mG - _sphere_param.named.radius < 700 && - fabsf(_sphere_param.named.offset.x) < 1000 && - fabsf(_sphere_param.named.offset.y) < 1000 && - fabsf(_sphere_param.named.offset.z) < 1000 && - fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1 - fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f && - fabsf(_ellipsoid_param.named.offdiag.x) < 1.0f ){ + if( !isnan(_fitness) && + _params.radius > 200 && //Earth's magnetic field strength range: 250-650mG + _params.radius < 700 && + fabsf(_params.offset.x) < 1000 && + fabsf(_params.offset.y) < 1000 && + fabsf(_params.offset.z) < 1000 && + fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1 + fabsf(_params.offdiag.x) < 1.0f && + fabsf(_params.offdiag.x) < 1.0f ){ return _fitness <= sq(_tolerance); } @@ -282,7 +285,7 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample) return false; } - float max_distance = fabsf(5.38709f * _sphere_param.named.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; + float max_distance = fabsf(5.38709f * _params.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; for (uint16_t i = 0; i<_samples_collected; i++){ float distance = (sample - _sample_buffer[i].get()).length(); @@ -297,43 +300,39 @@ 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 { +float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) 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 + params.diag.x , params.offdiag.x , params.offdiag.y, + params.offdiag.x , params.diag.y , params.offdiag.z, + params.offdiag.y , params.offdiag.z , params.diag.z ); - if(_running_ellipsoid_fit){ - return sp.named.radius - (softiron*(sample+ep.named.offset)).length(); - } else{ - return sp.named.radius - (softiron*(sample+sp.named.offset)).length(); - } + return params.radius - (softiron*(sample+params.offset)).length(); } float CompassCalibrator::calc_mean_squared_residuals() const { - return calc_mean_squared_residuals(_sphere_param,_ellipsoid_param); + return calc_mean_squared_residuals(_params); } -float CompassCalibrator::calc_mean_squared_residuals(const sphere_param_t& sp, const ellipsoid_param_t& ep) const +float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const { - if(_sample_buffer == NULL) { - return -1.0f; + if(_sample_buffer == NULL || _samples_collected == 0) { + return 1.0e30f; } 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); + float resid = calc_residual(sample, params); 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; +void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const{ + const Vector3f &offset = params.offset; + const Vector3f &diag = params.diag; + const Vector3f &offdiag = params.offdiag; Matrix3f softiron( diag.x , offdiag.x , offdiag.y, offdiag.x , diag.y , offdiag.z, @@ -345,106 +344,101 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const sphere_p float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); float length = (softiron*(sample+offset)).length(); - ret.named.radius = 1; - ret.named.offset.x = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); - ret.named.offset.y = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); - ret.named.offset.z = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); + // 0: radius + ret[0] = 1; + // 1-3: offsets + ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); + ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); + ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); } -void CompassCalibrator::run_sphere_fit(uint8_t max_iterations) +void CompassCalibrator::run_sphere_fit() { if(_sample_buffer == NULL) { return; } - float fitness = _fitness, prevfitness = _fitness, fit1, fit2; - sphere_param_t fit_param = _sphere_param, temp_param; - - for(uint8_t iterations=0; iterations _fitness && fit2 > _fitness){ + _sphere_lambda *= lma_damping; + } else if(fit2 < _fitness && fit2 < fit1) { + _sphere_lambda /= lma_damping; + fit1_params = fit2_params; + fitness = fit2; + } else if(fit1 < _fitness){ + fitness = fit1; + } + //--------------------Levenberg-part-ends-here--------------------------------// - if(fit1 > prevfitness && fit2 > prevfitness){ - _lambda *= 10.0f; - } else if(fit2 < prevfitness && fit2 < fit1) { - _lambda /= 10.0f; - fit_param = temp_param; - fitness = fit2; - } else if(fit1 < prevfitness){ - fitness = fit1; - } - prevfitness = fitness; - //--------------------Levenberg-part-ends-here--------------------------------// - if(fitness < _fitness) { - _fitness = fitness; - _sphere_param = fit_param; - } + if(!isnan(fitness) && fitness < _fitness) { + _fitness = fitness; + _params = fit1_params; } } -void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellipsoid_param_t& ep, ellipsoid_param_t &ret) const{ - const Vector3f &offset = ep.named.offset; - const Vector3f &diag = ep.named.diag; - const Vector3f &offdiag = ep.named.offdiag; +void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const{ + const Vector3f &offset = params.offset; + const Vector3f &diag = params.diag; + const Vector3f &offdiag = params.offdiag; Matrix3f softiron( diag.x , offdiag.x , offdiag.y, offdiag.x , diag.y , offdiag.z, @@ -456,103 +450,101 @@ void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const ellip float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z)); float length = (softiron*(sample+offset)).length(); - ret.named.offset.x = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); - ret.named.offset.y = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); - ret.named.offset.z = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); - ret.named.diag.x = -1.0f * ((sample.x + offset.x) * A)/length; - ret.named.diag.y = -1.0f * ((sample.y + offset.y) * B)/length; - ret.named.diag.z = -1.0f * ((sample.z + offset.z) * C)/length; - ret.named.offdiag.x = -1.0f * (((sample.y + offset.y) * A) + ((sample.z + offset.z) * B))/length; - ret.named.offdiag.y = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length; - ret.named.offdiag.z = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length; + // 0-2: offsets + ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length); + ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length); + ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); + // 3-5: diagonals + ret[3] = -1.0f * ((sample.x + offset.x) * A)/length; + ret[4] = -1.0f * ((sample.y + offset.y) * B)/length; + ret[5] = -1.0f * ((sample.z + offset.z) * C)/length; + // 6-8: off-diagonals + ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.z + offset.z) * B))/length; + ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length; + ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length; } -void CompassCalibrator::run_ellipsoid_fit(uint8_t max_iterations) +void CompassCalibrator::run_ellipsoid_fit() { if(_sample_buffer == NULL) { return; } - float fitness = _fitness, prevfitness = _fitness, fit1, fit2; - ellipsoid_param_t fit_param = _ellipsoid_param, temp_param; - for(uint8_t iterations=0; iterations prevfitness && fit2 > prevfitness){ - _lambda *= 10.0f; - } else if(fit2 < prevfitness && fit2 < fit1) { - _lambda /= 10.0f; - fit_param = temp_param; - fitness = fit2; - } else if(fit1 < prevfitness){ - fitness = fit1; + for(uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) { + for(uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) { + fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col]; + fit2_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col]; } - prevfitness = fitness; - //--------------------Levenberg-part-ends-here--------------------------------// + } - if(fitness < _fitness) { - _fitness = fitness; - _ellipsoid_param = fit_param; - } + fit1 = calc_mean_squared_residuals(fit1_params); + fit2 = calc_mean_squared_residuals(fit2_params); + + if(fit1 > _fitness && fit2 > _fitness){ + _ellipsoid_lambda *= lma_damping; + } else if(fit2 < _fitness && fit2 < fit1) { + _ellipsoid_lambda /= lma_damping; + fit1_params = fit2_params; + fitness = fit2; + } else if(fit1 < _fitness){ + fitness = fit1; + } + //--------------------Levenberg-part-ends-here--------------------------------// + + if(fitness < _fitness) { + _fitness = fitness; + _params = fit1_params; } } diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 152a74a5e04ee..775ba57c1e13d 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -10,8 +10,8 @@ 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_RUNNING_STEP_ONE=2, + COMPASS_CAL_RUNNING_STEP_TWO=3, COMPASS_CAL_SUCCESS=4, COMPASS_CAL_FAILED=5 }; @@ -32,7 +32,6 @@ class CompassCalibrator { 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); } @@ -40,25 +39,20 @@ class CompassCalibrator { 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 offset; - Vector3f diag; - Vector3f offdiag; - } named; - - float array[COMPASS_CAL_NUM_ELLIPSOID_PARAMS]; + class param_t { + public: + float* get_sphere_params() { + return &radius; + } + + float* get_ellipsoid_params() { + return &offset.x; + } + + float radius; + Vector3f offset; + Vector3f diag; + Vector3f offdiag; }; class CompassSample { @@ -71,57 +65,61 @@ class CompassCalibrator { int16_t z; }; - bool fit_acceptable(); - bool _autosave; + enum compass_cal_status_t _status; + + // timeout watchdog state + uint32_t _last_sample_ms; + + // behavioral state + float _delay_start_sec; + uint32_t _start_time_ms; + bool _autosave; bool _retry; + float _tolerance; 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; + //fit state + struct param_t _params; + uint16_t _fit_step; + CompassSample *_sample_buffer; + float _fitness; // mean squared residuals + float _initial_fitness; + float _sphere_lambda; + float _ellipsoid_lambda; + uint16_t _samples_collected; + uint16_t _samples_thinned; - 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); + bool set_status(compass_cal_status_t status); - 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); + bool running() const; + bool fitting() const; // returns true if sample should be added to buffer bool accept_sample(const Vector3f &sample); bool accept_sample(const CompassSample &sample); - void thin_samples(); + // returns true if fit is acceptable + bool fit_acceptable(); - bool set_status(compass_cal_status_t status); void reset_state(); + void initialize_fit(); - 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; + // thins out samples between step one and step two + void thin_samples(); - // mean squared residuals - float _fitness; - float _initial_fitness; + float calc_residual(const Vector3f& sample, const param_t& params) const; + float calc_mean_squared_residuals(const param_t& params) const; + float calc_mean_squared_residuals() const; - float _tolerance; + void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const; + void run_sphere_fit(); - float _lambda; + void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const; + void run_ellipsoid_fit(); - sphere_param_t _sphere_param; - ellipsoid_param_t _ellipsoid_param; - bool _running_ellipsoid_fit:1; // math helpers - bool inverse9x9(const float m[],float invOut[]); float det9x9(const float m[]); bool inverse6x6(const float m[],float invOut[]); From 87195b88fad6a2ae0411319c16267c44222c4b47 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:03:31 -0700 Subject: [PATCH 18/39] AP_Compass: update enum values for compass cal status --- libraries/AP_Compass/Compass.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index de649bc491805..7a624ff7f6c83 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -412,8 +412,8 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan) uint8_t cal_status = cal.get_status(); if(cal_status == COMPASS_CAL_WAITING_TO_START || - cal_status == COMPASS_CAL_SAMPLING_STEP_ONE || - cal_status == COMPASS_CAL_SAMPLING_STEP_TWO) { + cal_status == COMPASS_CAL_RUNNING_STEP_ONE || + cal_status == COMPASS_CAL_RUNNING_STEP_TWO) { uint8_t completion_pct = cal.get_completion_percent(); uint8_t completion_mask[10]; Vector3f direction(0.0f,0.0f,0.0f); From fb6e7eeaf6d1c8949b3fcbd9b4383fff4a4a6680 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:06:24 -0700 Subject: [PATCH 19/39] GCS_MAVLink: update MAG_CAL_STATUS enum names --- libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml index 3c1463345dcd7..d38e80e929de9 100644 --- a/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml @@ -158,8 +158,8 @@ - - + + From e54d71cf908cbebee331b9aba358bc082a9671b7 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:07:11 -0700 Subject: [PATCH 20/39] GCS_MAVLink: run generate.sh --- .../include/mavlink/v1.0/ardupilotmega/ardupilotmega.h | 4 ++-- .../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h | 2 +- libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 119e740d0dae3..0fae9d2727f46 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -230,8 +230,8 @@ typedef enum MAG_CAL_STATUS { MAG_CAL_NOT_STARTED=0, /* | */ MAG_CAL_WAITING_TO_START=1, /* | */ - MAG_CAL_SAMPLING_STEP_ONE=2, /* | */ - MAG_CAL_SAMPLING_STEP_TWO=3, /* | */ + MAG_CAL_RUNNING_STEP_ONE=2, /* | */ + MAG_CAL_RUNNING_STEP_TWO=3, /* | */ MAG_CAL_SUCCESS=4, /* | */ MAG_CAL_FAILED=5, /* | */ MAG_CAL_STATUS_ENUM_END=6, /* | */ diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h index c5b33242d8112..7937e0ac2a3b8 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sat Mar 7 01:13:27 2015" +#define MAVLINK_BUILD_DATE "Mon Mar 9 18:07:03 2015" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h index c1caec74319cc..23865608e3ccd 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sat Mar 7 01:13:28 2015" +#define MAVLINK_BUILD_DATE "Mon Mar 9 18:07:04 2015" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 From ef0f519c449a3ea910b856ce86c90b303a36ee99 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:46:24 -0700 Subject: [PATCH 21/39] Compass: add is_calibrating --- libraries/AP_Compass/Compass.cpp | 8 +++++++- libraries/AP_Compass/Compass.h | 6 +++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 7a624ff7f6c83..dca704342ee9f 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -477,8 +477,14 @@ Compass::get_healthy_mask() const return healthy_mask; } +bool +Compass::is_calibrating() const +{ + return get_cal_mask(); +} + uint8_t -Compass::get_cal_mask() +Compass::get_cal_mask() const { uint8_t cal_mask = 0; for(uint8_t i=0; i Date: Mon, 9 Mar 2015 18:43:43 -0700 Subject: [PATCH 22/39] Copter: add arming check for compass calibration running --- ArduCopter/motors.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 083d6ba7083f7..426aa547d440b 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -584,6 +584,13 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs) start_logging(); #endif + if(compass.is_calibrating()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Compass calibration running")); + } + return false; + } + // always check if the current mode allows arming if (!mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { From 81dbfcb6f2f06abd3e1f2bb18f0ff0ac80cad960 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:44:55 -0700 Subject: [PATCH 23/39] Copter: refuse to start mag cal if armed --- ArduCopter/GCS_Mavlink.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 37fca006b5c9d..a973825c5cf87 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1317,7 +1317,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_DO_START_MAG_CAL: { result = MAV_RESULT_ACCEPTED; - if(packet.param1 < 0 || packet.param1 > 255) { + if(motors.armed() || packet.param1 < 0 || packet.param1 > 255) { result = MAV_RESULT_FAILED; break; } From d305d623d85e2e44ac12013cfbd7e11278f271f3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:50:03 -0700 Subject: [PATCH 24/39] AP_Notify: add compass_cal flags --- libraries/AP_Notify/AP_Notify.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 311474073259d..cf5c33405834c 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -72,6 +72,7 @@ class AP_Notify uint32_t parachute_release : 1; // 1 if parachute is being released uint32_t ekf_bad : 1; // 1 if ekf is reporting problems uint32_t autopilot_mode : 1; // 1 if vehicle is in an autopilot flight mode (only used by OreoLEDs) + uint32_t compass_cal_running: 1; // 1 if a compass calibration is running // additional flags uint32_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter) @@ -90,6 +91,9 @@ class AP_Notify uint16_t mission_complete : 1; // 1 when the mission has completed successfully uint16_t waypoint_complete : 1; // 1 as vehicle completes a waypoint uint16_t firmware_update : 1; // 1 just before vehicle firmware is updated + uint16_t initiated_compass_cal : 1; // 1 when user input to begin compass cal was accepted + uint16_t compass_cal_saved : 1; // 1 when compass calibration was just saved + uint16_t compass_cal_failed : 1; // 1 when compass calibration was just saved }; // the notify flags are static to allow direct class access From df686efc886d065bbbde293358f197869ef2bc07 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:50:17 -0700 Subject: [PATCH 25/39] Compass: update AP_Notify compass_cal flags --- libraries/AP_Compass/Compass.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index dca704342ee9f..c9d4e38e5f854 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -294,6 +294,9 @@ bool Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay) { if(healthy(i)) { + if(!is_calibrating() && delay > 0.5f) { + AP_Notify::events.initiated_compass_cal = 1; + } _calibrator[i].start(retry, autosave, delay); return true; } else { @@ -356,13 +359,14 @@ Compass::accept_calibration(uint8_t i) set_and_save_offsets(i, ofs); - - //TODO soft-iron calibrations #if COMPASS_MAX_INSTANCES > 1 _dev_id[i].save(); #endif + if(!is_calibrating()) { + AP_Notify::events.compass_cal_saved = 1; + } return true; } else { return false; @@ -391,6 +395,7 @@ Compass::accept_calibration_mask(uint8_t mask) } } } + return success; } From 2e74a218623aa99c03ed9fbf66453c4412ea9363 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 18:49:05 -0700 Subject: [PATCH 26/39] Compass: add compass_cal_update --- libraries/AP_Compass/Compass.cpp | 21 +++++++++++++++++++-- libraries/AP_Compass/Compass.h | 4 +++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index c9d4e38e5f854..7adba9b7715c5 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include #include "Compass.h" -#include +#include const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // index 0 was used for the old orientation matrix @@ -290,6 +290,24 @@ Compass::init() return true; } +void +Compass::compass_cal_update() +{ + AP_Notify::flags.compass_cal_running = 0; + + for(uint8_t i=0; i Date: Mon, 9 Mar 2015 18:54:05 -0700 Subject: [PATCH 27/39] AP_Compass: make compasscalibrator running() public --- libraries/AP_Compass/CompassCalibrator.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 775ba57c1e13d..70b23c3a76b69 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -28,6 +28,8 @@ class CompassCalibrator { void check_for_timeout(); + bool running() const; + void set_tolerance(float tolerance) { _tolerance = tolerance; } void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); @@ -93,9 +95,6 @@ class CompassCalibrator { bool set_status(compass_cal_status_t status); - bool running() const; - bool fitting() const; - // returns true if sample should be added to buffer bool accept_sample(const Vector3f &sample); bool accept_sample(const CompassSample &sample); @@ -106,6 +105,8 @@ class CompassCalibrator { void reset_state(); void initialize_fit(); + bool fitting() const; + // thins out samples between step one and step two void thin_samples(); From a2741f70f33c69aeb44444e8d7697c7aba46b413 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 19:01:29 -0700 Subject: [PATCH 28/39] AP_Notify: play tones for compass cal --- libraries/AP_Notify/ToneAlarm_PX4.cpp | 34 +++++++++++++++++++++++++++ libraries/AP_Notify/ToneAlarm_PX4.h | 1 + 2 files changed, 35 insertions(+) diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp index 2e2b96311cd51..c96b82e2dc0b6 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp @@ -63,6 +63,8 @@ const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] { { "MBT200>B#1", true }, #define AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS 13 { "MBNT255>B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8", true }, + #define AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS 14 + { "MBNT255 Date: Mon, 9 Mar 2015 19:05:29 -0700 Subject: [PATCH 29/39] Copter: add compass_cal_update --- ArduCopter/ArduCopter.pde | 1 + ArduCopter/compass_cal.pde | 24 ++++++++++++++++++++++++ 2 files changed, 25 insertions(+) create mode 100644 ArduCopter/compass_cal.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 79f2e8076da6f..6370ee85b8704 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -761,6 +761,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { update_thr_average, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 4, 42 }, + { compass_cal_update, 4, 40 }, { barometer_accumulate, 8, 25 }, #if FRAME_CONFIG == HELI_FRAME { check_dynamic_flight, 8, 10 }, diff --git a/ArduCopter/compass_cal.pde b/ArduCopter/compass_cal.pde new file mode 100644 index 0000000000000..4cc87ad610621 --- /dev/null +++ b/ArduCopter/compass_cal.pde @@ -0,0 +1,24 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#define COMPASS_CAL_STICK_DELAY 2.0f // 2 seconds +#define COMPASS_CAL_DELAY 5.0f + +static void compass_cal_update() { + compass.compass_cal_update(); + + if (compass.is_calibrating()) { + return; + } + + bool stick_gesture_detected = !motors.armed() && g.rc_4.control_in > 4000 && g.rc_3.control_in > 900; + static uint32_t stick_gesture_begin = 0; + uint32_t tnow = millis(); + + if(!stick_gesture_detected) { + stick_gesture_begin = tnow; + } else { + if(tnow-stick_gesture_begin > 1000*COMPASS_CAL_STICK_DELAY) { + compass.start_calibration_all(true,true,COMPASS_CAL_DELAY); + } + } +} From b6c5c8ab80f89b31e5450c44637ab35c929f8671 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 19:59:57 -0700 Subject: [PATCH 30/39] CompassCalibrator: update AP_Notify on failure --- libraries/AP_Compass/CompassCalibrator.cpp | 9 +++++++-- libraries/AP_Compass/CompassCalibrator.h | 2 +- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 309a5f2241d17..227863898a59f 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -1,6 +1,6 @@ #include "CompassCalibrator.h" #include -#include +#include extern const AP_HAL::HAL& hal; @@ -59,11 +59,14 @@ float CompassCalibrator::get_completion_percent() const { }; } -void CompassCalibrator::check_for_timeout() { +bool CompassCalibrator::check_for_timeout() { uint32_t tnow = hal.scheduler->millis(); if(running() && tnow - _last_sample_ms > 1000) { + _retry = false; set_status(COMPASS_CAL_FAILED); + return true; } + return false; } void CompassCalibrator::new_sample(const Vector3f& sample) { @@ -221,6 +224,8 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { return false; } + AP_Notify::events.compass_cal_failed = 1; + if(_retry && set_status(COMPASS_CAL_WAITING_TO_START)) { _attempt++; return true; diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 70b23c3a76b69..eb560456de2e3 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -26,7 +26,7 @@ class CompassCalibrator { void new_sample(const Vector3f &sample); void run_fit_chunk(); - void check_for_timeout(); + bool check_for_timeout(); bool running() const; From 68c94fac63b03b2d1bf48e613b25bc17a8dd9ce5 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 22 Mar 2014 20:45:48 -0700 Subject: [PATCH 31/39] AP_Compass: Add soft-iron calibrations --- libraries/AP_Compass/Compass.cpp | 117 +++++++++++++++++++++++++++++-- libraries/AP_Compass/Compass.h | 2 + 2 files changed, 115 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 7adba9b7715c5..d94cff779e17e 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -258,6 +258,106 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { AP_GROUPINFO("EXTERN3",23, Compass, _external[2], 0), #endif + // @Param: DIA_X + // @DisplayName: Compass soft-iron diagonal X component + // @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA_Y + // @DisplayName: Compass soft-iron diagonal Y component + // @Description: DIA_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA_Z + // @DisplayName: Compass soft-iron diagonal Z component + // @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("DIA", 24, Compass, _diagonals[0], 0), + + // @Param: ODI_X + // @DisplayName: Compass soft-iron off-diagonal X component + // @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI_Y + // @DisplayName: Compass soft-iron off-diagonal Y component + // @Description: ODI_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI_Z + // @DisplayName: Compass soft-iron off-diagonal Z component + // @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("ODI", 25, Compass, _offdiagonals[0], 0), + +#if COMPASS_MAX_INSTANCES > 1 + // @Param: DIA2_X + // @DisplayName: Compass2 soft-iron diagonal X component + // @Description: DIA_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA2_Y + // @DisplayName: Compass2 soft-iron diagonal Y component + // @Description: DIA_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA2_Z + // @DisplayName: Compass2 soft-iron diagonal Z component + // @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("DIA2", 26, Compass, _diagonals[1], 0), + + // @Param: ODI2_X + // @DisplayName: Compass2 soft-iron off-diagonal X component + // @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI2_Y + // @DisplayName: Compass2 soft-iron off-diagonal Y component + // @Description: ODI_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI2_Z + // @DisplayName: Compass2 soft-iron off-diagonal Z component + // @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("ODI2", 27, Compass, _offdiagonals[1], 0), +#endif + +#if COMPASS_MAX_INSTANCES > 2 + // @Param: DIA3_X + // @DisplayName: Compass3 soft-iron diagonal X component + // @Description: DIA_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA3_Y + // @DisplayName: Compass3 soft-iron diagonal Y component + // @Description: DIA_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: DIA3_Z + // @DisplayName: Compass3 soft-iron diagonal Z component + // @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("DIA3", 28, Compass, _diagonals[2], 0), + + // @Param: ODI3_X + // @DisplayName: Compass3 soft-iron off-diagonal X component + // @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI3_Y + // @DisplayName: Compass3 soft-iron off-diagonal Y component + // @Description: ODI_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + + // @Param: ODI3_Z + // @DisplayName: Compass3 soft-iron off-diagonal Z component + // @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] + // @User: Advanced + AP_GROUPINFO("ODI3", 29, Compass, _offdiagonals[2], 0), +#endif + AP_GROUPEND }; @@ -699,13 +799,14 @@ void Compass::apply_corrections(Vector3f &mag, uint8_t i) { _calibrator[i].new_sample(mag); + if (_diagonals[i].get().is_zero()) { + _diagonals[i].set(Vector3f(1.0f,1.0f,1.0f)); + } const Vector3f &offsets = _offset[i].get(); + const Vector3f &diagonals = _diagonals[i].get(); + const Vector3f &offdiagonals = _offdiagonals[i].get(); const Vector3f &mot = _motor_compensation[i].get(); - /* - note that _motor_offset[] is kept even if compensation is not - being applied so it can be logged correctly - */ mag += offsets; if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) { _motor_offset[i] = mot * _thr_or_curr; @@ -713,4 +814,12 @@ void Compass::apply_corrections(Vector3f &mag, uint8_t i) } else { _motor_offset[i].zero(); } + + Matrix3f mat( + diagonals.x, offdiagonals.x, offdiagonals.y, + offdiagonals.x, diagonals.y, offdiagonals.z, + offdiagonals.y, offdiagonals.z, diagonals.z + ); + + mag = mag * mat; } diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 842ef3358a8c8..cc8ff43e9d3cf 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -292,6 +292,8 @@ class Compass AP_Int8 _orientation[COMPASS_MAX_INSTANCES]; AP_Vector3f _offset[COMPASS_MAX_INSTANCES]; + AP_Vector3f _diagonals[COMPASS_MAX_INSTANCES]; + AP_Vector3f _offdiagonals[COMPASS_MAX_INSTANCES]; AP_Float _declination; AP_Int8 _use_for_yaw[COMPASS_MAX_INSTANCES];/// Date: Mon, 9 Mar 2015 20:22:35 -0700 Subject: [PATCH 32/39] Compass: save diagonals and off-diagonals --- libraries/AP_Compass/Compass.cpp | 22 ++++++++++++++++++++-- libraries/AP_Compass/Compass.h | 3 +++ 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index d94cff779e17e..62c1d5a872132 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -476,8 +476,8 @@ Compass::accept_calibration(uint8_t i) cal.clear(); set_and_save_offsets(i, ofs); - - //TODO soft-iron calibrations + set_and_save_diagonals(i,diag); + set_and_save_offdiagonals(i,offdiag); #if COMPASS_MAX_INSTANCES > 1 _dev_id[i].save(); @@ -637,6 +637,24 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets) } } +void +Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals) +{ + // sanity check compass instance provided + if (i < COMPASS_MAX_INSTANCES) { + _diagonals[i].set_and_save(diagonals); + } +} + +void +Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals) +{ + // sanity check compass instance provided + if (i < COMPASS_MAX_INSTANCES) { + _offdiagonals[i].set_and_save(offdiagonals); + } +} + void Compass::save_offsets(uint8_t i) { diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index cc8ff43e9d3cf..27db1d2fa52d2 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -128,6 +128,9 @@ class Compass /// void set_and_save_offsets(uint8_t i, const Vector3f &offsets); + void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals); + void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals); + /// Saves the current offset x/y/z values for one or all compasses /// /// @param i compass instance From 1b75a85c5d09c2c80376a9c0e03c98bc0c79ceac Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 25 Feb 2015 19:14:15 -0800 Subject: [PATCH 33/39] Copter: run compass_accumulate at 100hz --- ArduCopter/ArduCopter.pde | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6370ee85b8704..79f2e8076da6f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -761,7 +761,6 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { update_thr_average, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 4, 42 }, - { compass_cal_update, 4, 40 }, { barometer_accumulate, 8, 25 }, #if FRAME_CONFIG == HELI_FRAME { check_dynamic_flight, 8, 10 }, From 0fc11649f25090025e8f99ed3cfaa65c2b1ef5ee Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 9 Mar 2015 19:05:29 -0700 Subject: [PATCH 34/39] Copter: add compass_cal_update --- ArduCopter/ArduCopter.pde | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 79f2e8076da6f..6370ee85b8704 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -761,6 +761,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { update_thr_average, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 4, 42 }, + { compass_cal_update, 4, 40 }, { barometer_accumulate, 8, 25 }, #if FRAME_CONFIG == HELI_FRAME { check_dynamic_flight, 8, 10 }, From 33a3dd16b761da75372f7c0bd8122b0231e80660 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 10 Mar 2015 23:34:58 +0530 Subject: [PATCH 35/39] Compass: change to lower case "false" for assignment --- libraries/AP_Compass/CompassCalibrator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 227863898a59f..f7ba05477b0a3 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -1039,7 +1039,7 @@ float CompassCalibrator::det6x6(const float C[36]) } f = A[0]; - isodd = FALSE; + isodd = false; for (jy = 0; jy < 5; jy++) { f *= A[(jy + 6 * (1 + jy)) + 1]; if (ipiv[jy] > 1 + jy) { @@ -1128,7 +1128,7 @@ float CompassCalibrator::det9x9(const float C[81]) } f = A[0]; - isodd = FALSE; + isodd = false; for (jy = 0; jy < 8; jy++) { f *= A[(jy + 9 * (1 + jy)) + 1]; if (ipiv[jy] > 1 + jy) { From ae80821735bc0ab193c438f969b9399d4f2279f6 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 12 Mar 2015 03:36:04 +0530 Subject: [PATCH 36/39] AP_Math: add inverse function to matrix algebra library --- libraries/AP_Math/AP_Math.h | 4 + libraries/AP_Math/matrix_alg.cpp | 603 +++++++++++++++++++++++++++++++ 2 files changed, 607 insertions(+) create mode 100644 libraries/AP_Math/matrix_alg.cpp diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 19a3d8bbbe178..1270586659915 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -33,6 +33,8 @@ #define DEG_TO_RAD 0.017453292519943295769236907684886f #define RAD_TO_DEG 57.295779513082320876798154814105f + +#define TINY_FLOAT 1e-20 //GPS Specific double precision conversions //The precision here does matter when using the wsg* functions for converting //between LLH and ECEF coordinates. Test code in examlpes/location/location.pde @@ -183,6 +185,8 @@ float sq(float v); float pythagorous2(float a, float b); float pythagorous3(float a, float b, float c); +// matrix algebra +bool inverse(float x[], float y[], uint16_t dim); #ifdef radians #error "Build is including Arduino base headers" #endif diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.cpp new file mode 100644 index 0000000000000..9d14677422ce6 --- /dev/null +++ b/libraries/AP_Math/matrix_alg.cpp @@ -0,0 +1,603 @@ +#include +#include + +extern const AP_HAL::HAL& hal; + +float 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; +} + +float det9x9(const float C[81]) +{ + float f; + float A[81]; + int8_t ipiv[9]; + 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], 81U * sizeof(float)); + for (i0 = 0; i0 < 9; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + smax = fabs(A[c]); + for (jy = 2; jy <= 9 - j; jy++) { + ix++; + s = fabs(A[ix]); + if (s > smax) { + iy = jy - 1; + smax = s; + } + } + + if (A[c + iy] != 0.0) { + if (iy != 0) { + ipiv[j] = (int8_t)((j + iy) + 1); + ix = j; + iy += j; + for (jy = 0; jy < 9; jy++) { + smax = A[ix]; + A[ix] = A[iy]; + A[iy] = smax; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (iy = c + 1; iy + 1 <= i0; iy++) { + A[iy] /= A[c]; + } + } + + iy = c; + jy = c + 9; + for (b_j = 1; b_j <= 8 - j; b_j++) { + smax = A[jy]; + if (A[jy] != 0.0) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + f = A[0]; + isodd = false; + for (jy = 0; jy < 8; jy++) { + f *= A[(jy + 9 * (1 + jy)) + 1]; + if (ipiv[jy] > 1 + jy) { + isodd = !isodd; + } + } + + if (isodd) { + f = -f; + } + + return f; +} + +bool inverse9x9(const float x[81], float y[81]) +{ + if(fabsf(det9x9(x)) < 1.0e-20f) { + return false; + } + float A[81]; + int32_t i0; + int8_t ipiv[9]; + int32_t j; + int32_t c; + int32_t pipk; + int32_t ix; + float smax; + int32_t k; + float s; + int32_t jy; + int32_t ijA; + int8_t p[9]; + for (i0 = 0; i0 < 81; i0++) { + A[i0] = x[i0]; + y[i0] = 0.0; + } + + for (i0 = 0; i0 < 9; i0++) { + ipiv[i0] = (int8_t)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + pipk = 0; + ix = c; + smax = fabs(A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = fabs(A[ix]); + if (s > smax) { + pipk = k - 1; + smax = s; + } + } + + if (A[c + pipk] != 0.0) { + if (pipk != 0) { + ipiv[j] = (int8_t)((j + pipk) + 1); + ix = j; + pipk += j; + for (k = 0; k < 9; k++) { + smax = A[ix]; + A[ix] = A[pipk]; + A[pipk] = smax; + ix += 9; + pipk += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + A[jy] /= A[c]; + } + } + + pipk = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + smax = A[jy]; + if (A[jy] != 0.0) { + ix = c + 1; + i0 = (pipk - j) + 18; + for (ijA = 10 + pipk; ijA + 1 <= i0; ijA++) { + A[ijA] += A[ix] * -smax; + ix++; + } + } + + jy += 9; + pipk += 9; + } + } + + for (i0 = 0; i0 < 9; i0++) { + p[i0] = (int8_t)(1 + i0); + } + + for (k = 0; k < 8; k++) { + if (ipiv[k] > 1 + k) { + pipk = p[ipiv[k] - 1]; + p[ipiv[k] - 1] = p[k]; + p[k] = (int8_t)pipk; + } + } + + for (k = 0; k < 9; k++) { + y[k + 9 * (p[k] - 1)] = 1.0; + for (j = k; j + 1 < 10; j++) { + if (y[j + 9 * (p[k] - 1)] != 0.0) { + for (jy = j + 1; jy + 1 < 10; jy++) { + y[jy + 9 * (p[k] - 1)] -= y[j + 9 * (p[k] - 1)] * A[jy + 9 * j]; + } + } + } + } + + for (j = 0; j < 9; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + pipk = 9 * k; + if (y[k + c] != 0.0) { + 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 inverse6x6(const float x[], float y[]) +{ + if(fabsf(det6x6(x)) < 1.0e-20f) { + return false; + } + + float A[36]; + int32_t i0; + int32_t ipiv[6]; + int32_t j; + int32_t c; + int32_t pipk; + int32_t ix; + float smax; + int32_t k; + float s; + int32_t jy; + int32_t ijA; + int32_t p[6]; + for (i0 = 0; i0 < 36; i0++) { + A[i0] = x[i0]; + y[i0] = 0.0f; + } + + for (i0 = 0; i0 < 6; i0++) { + ipiv[i0] = (int32_t)(1 + i0); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + pipk = 0; + ix = c; + smax = fabsf(A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = fabsf(A[ix]); + if (s > 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 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 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; +} + + +bool inverse(float x[], float y[], uint16_t dim) +{ + switch(dim){ + case 3: return inverse3x3(x,y); + case 4: return inverse4x4(x,y); + case 6: return inverse6x6(x,y); + case 9: return inverse9x9(x,y); + default: return false; + } +} \ No newline at end of file From 60d4e42f43a3187748066f54fa3d5f85552b60a0 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 12 Mar 2015 03:37:26 +0530 Subject: [PATCH 37/39] AP_Math: add optimiser library with LM optimiser --- libraries/AP_Math/AP_Math.h | 1 + libraries/AP_Math/optimiser.cpp | 124 ++++++++++++++++++++++++++++++++ libraries/AP_Math/optimiser.h | 47 ++++++++++++ 3 files changed, 172 insertions(+) create mode 100644 libraries/AP_Math/optimiser.cpp create mode 100644 libraries/AP_Math/optimiser.h diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 1270586659915..fd7db5798996e 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -19,6 +19,7 @@ #include "quaternion.h" #include "polygon.h" #include "edc.h" +#include "optimiser.h" #ifndef M_PI_F #define M_PI_F 3.141592653589793f diff --git a/libraries/AP_Math/optimiser.cpp b/libraries/AP_Math/optimiser.cpp new file mode 100644 index 0000000000000..739d78aaca588 --- /dev/null +++ b/libraries/AP_Math/optimiser.cpp @@ -0,0 +1,124 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include "optimiser.h" + +void Optimiser::set_init_params(float params[], const float const_params[], uint16_t num_params, float& fitness, float& lambda) +{ + _params = params; + _const_params = const_params; + _num_params = num_params; + _fitness = &fitness; + _lambda = λ +} + +float Optimiser::calc_mean_squared_residuals(float* params) +{ + if(_sample_buffer_size == 0) { + return 1.0e30f; + } + float sum = 0.0f; + for(uint16_t i=0; i < _sample_buffer_size; i++){ + Vector3f sample = _process_sample(_sample_buffer, i); + float resid = _calc_residual(sample, params, _const_params); + sum += sq(resid); + } + sum /= _sample_buffer_size; + return sum; +} + +void Optimiser::do_levenberg_marquardt_fit(float lma_damping, uint16_t num_iters) +{ + if(_sample_buffer_size == 0) { + return; + } + + float fitness = *_fitness; + float fit1, fit2; + + + float *JTJ = new float[_num_params*_num_params]; + float *JTJ2 = new float[_num_params*_num_params]; + float *JTFI = new float[_num_params]; + float *jacob = new float[_num_params]; + + float *fit1_params = new float[_num_params]; + float *fit2_params = new float[_num_params]; + if (JTJ == NULL || JTJ2 == NULL || JTFI == NULL || jacob == NULL || fit1_params == NULL || fit2_params == NULL ){ + goto lm_ret; + } + + memcpy(fit1_params,_params,sizeof(float)*_num_params); + memcpy(fit2_params,_params,sizeof(float)*_num_params); + + for(uint16_t iters = 0; iters < num_iters; iters++){ + + memset(JTJ,0,sizeof(float)*_num_params*_num_params); + memset(JTJ2,0,sizeof(float)*_num_params*_num_params); + memset(JTFI,0,sizeof(float)*_num_params); + + for(uint16_t k = 0; k<_sample_buffer_size; k++) { + Vector3f sample = _process_sample(_sample_buffer, k); + + _calc_jacob(sample, fit1_params, jacob, _const_params); + + for(uint8_t i = 0;i < _num_params; i++) { + // compute JTJ + for(uint8_t j = 0; j < _num_params; j++) { + JTJ[i*_num_params+j] += jacob[i] * jacob[j]; + JTJ2[i*_num_params+j] += jacob[i] * jacob[j]; //a backup JTJ for LM + } + // compute JTFI + JTFI[i] += jacob[i] * _calc_residual(sample, fit1_params, _const_params); + } + } + + //------------------------Levenberg-part-starts-here---------------------------------// + for(uint8_t i = 0; i < _num_params; i++) { + JTJ[i*_num_params+i] += (*_lambda); + JTJ2[i*_num_params+i] += (*_lambda)/lma_damping; + } + + if(!inverse(JTJ, JTJ, _num_params)) { + goto lm_ret; + } + + if(!inverse(JTJ2, JTJ2, _num_params)) { + goto lm_ret; + } + + for(uint8_t row=0; row < _num_params; row++) { + for(uint8_t col=0; col < _num_params; col++) { + fit1_params[row] -= JTFI[col] * JTJ[row*_num_params+col]; + fit2_params[row] -= JTFI[col] * JTJ2[row*_num_params+col]; + } + } + + fit1 = calc_mean_squared_residuals(fit1_params); + fit2 = calc_mean_squared_residuals(fit2_params); + + if(fit1 > *_fitness && fit2 > *_fitness){ + *_lambda *= lma_damping; + } else if(fit2 < *_fitness && fit2 < fit1) { + *_lambda /= lma_damping; + memcpy(fit1_params, fit2_params, sizeof(float)*_num_params); + fitness = fit2; + } else if(fit1 < *_fitness){ + fitness = fit1; + } + //--------------------Levenberg-part-ends-here------------------------------------// + + if(!isnan(fitness) && fitness < *_fitness) { + *_fitness = fitness; + memcpy(_params,fit1_params,sizeof(float)*_num_params); + } + } + +lm_ret: + delete[] fit1_params; + delete[] fit2_params; + delete[] JTJ; + delete[] JTJ2; + delete[] JTFI; + delete[] jacob; +} diff --git a/libraries/AP_Math/optimiser.h b/libraries/AP_Math/optimiser.h new file mode 100644 index 0000000000000..1b49009aafb40 --- /dev/null +++ b/libraries/AP_Math/optimiser.h @@ -0,0 +1,47 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef OPTIMISER_H +#define OPTIMISER_H + + +class Optimiser{ +public: + //Functions to be passed for sample preprocessing, fitness tests and Jacobian calculations + typedef float (*residual)(const Vector3f& sample, const float params[], const float const_params[]); + typedef void (*jacob)(const Vector3f& sample, const float params[], float* ret, const float const_params[]); + typedef Vector3f (*process_sample)(const Vector3i _sample_buffer[], uint16_t num); + + //Funtions to Initialise Optimser + void set_fitness_function(residual calc_residual) { _calc_residual = calc_residual; } + void set_jacobian_function(jacob calc_jacob) { _calc_jacob = calc_jacob; } + void set_preprocess_sample_function(process_sample preprocess_sample) { _process_sample = preprocess_sample; } + void set_buffer( Vector3i sample_buffer[], uint16_t sample_buffer_size) { + _sample_buffer = sample_buffer; + _sample_buffer_size = sample_buffer_size; + } + void set_init_params(float params[], const float const_params[], uint16_t num_params, float& fitness, float& lambda); + + //Optimiser Funtions + void do_levenberg_marquardt_fit(float lma_damping, uint16_t num_iters = 1); + void do_gauss_newton_fit(uint16_t iters = 1); + + float calc_mean_squared_residuals(float *params); + +protected: + + jacob _calc_jacob; + residual _calc_residual; + process_sample _process_sample; + + float *_fitness; + float *_lambda; + + Vector3i *_sample_buffer; + float *_params; + const float *_const_params; + + uint16_t _num_params; + uint16_t _sample_buffer_size; +}; + +#endif //OPTIMISER_H \ No newline at end of file From 379cfd3eff5576b01934d83840d86e699ce21549 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 12 Mar 2015 03:45:22 +0530 Subject: [PATCH 38/39] Compass: use optimiser library for compass calibration --- libraries/AP_Compass/CompassCalibrator.cpp | 864 ++------------------- libraries/AP_Compass/CompassCalibrator.h | 37 +- 2 files changed, 73 insertions(+), 828 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index f7ba05477b0a3..3b2fedc60d3aa 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -77,7 +77,7 @@ void CompassCalibrator::new_sample(const Vector3f& sample) { } if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) { - _sample_buffer[_samples_collected].set(sample); + set_sample(sample, _samples_collected); _samples_collected++; } } @@ -87,9 +87,25 @@ void CompassCalibrator::run_fit_chunk() { return; } + if (_fit_step == 0){ //initialise optimiser for sphere fit + optimise.set_fitness_function(&calc_sphere_residual); + optimise.set_jacobian_function(&calc_sphere_jacob); + optimise.set_preprocess_sample_function(&get_sample); + optimise.set_buffer(_sample_buffer, _samples_collected); + optimise.set_init_params(_params.get_sphere_params(), NULL, COMPASS_CAL_NUM_SPHERE_PARAMS, _fitness, _sphere_lambda); + }else if (_fit_step == 15){ //initialise optimiser for ellipsoid fit + optimise.set_fitness_function(&calc_ellipsoid_residual); + optimise.set_jacobian_function(&calc_ellipsoid_jacob); + optimise.set_preprocess_sample_function(&get_sample); + optimise.set_buffer(_sample_buffer, _samples_collected); + optimise.set_init_params(_params.get_ellipsoid_params(), _params.get_sphere_params(), + COMPASS_CAL_NUM_ELLIPSOID_PARAMS, _fitness, _ellipsoid_lambda); + } + if(_status == COMPASS_CAL_RUNNING_STEP_ONE) { if(_fit_step < 10) { - run_sphere_fit(); + optimise.do_levenberg_marquardt_fit(10.0f); + _fit_step++; } else { if(_fitness == _initial_fitness || isnan(_fitness)) { //if true, means that fitness is diverging instead of converging @@ -100,9 +116,9 @@ void CompassCalibrator::run_fit_chunk() { } } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) { if(_fit_step < 15) { - run_sphere_fit(); + optimise.do_levenberg_marquardt_fit(10.0f); } else if(_fit_step < 35) { - run_ellipsoid_fit(); + optimise.do_levenberg_marquardt_fit(10.0f); //ellipsoid fit } else { if(fit_acceptable()) { set_status(COMPASS_CAL_SUCCESS); @@ -128,7 +144,7 @@ bool CompassCalibrator::fitting() const { void CompassCalibrator::initialize_fit() { //initialize _fitness before starting a fit if (_samples_collected != 0) { - _fitness = calc_mean_squared_residuals(_params); + _fitness = calc_mean_squared_residuals(); } else { _fitness = 1.0e30f; } @@ -187,7 +203,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { return true; } - _sample_buffer = (CompassSample*)malloc(sizeof(CompassSample)*COMPASS_CAL_NUM_SAMPLES); + _sample_buffer = (Vector3i*)malloc(sizeof(Vector3i)*COMPASS_CAL_NUM_SAMPLES); if(_sample_buffer != NULL) { initialize_fit(); @@ -270,13 +286,13 @@ void CompassCalibrator::thin_samples() { // 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]; + Vector3i 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])) { + if(!accept_sample(get_sample(_sample_buffer, i))) { _sample_buffer[i] = _sample_buffer[_samples_collected-1]; _samples_collected --; _samples_thinned ++; @@ -293,7 +309,7 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample) float max_distance = fabsf(5.38709f * _params.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; for (uint16_t i = 0; i<_samples_collected; i++){ - float distance = (sample - _sample_buffer[i].get()).length(); + float distance = (sample - get_sample(_sample_buffer, i)).length(); if(distance < max_distance) { return false; } @@ -301,43 +317,38 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample) return true; } -bool CompassCalibrator::accept_sample(const CompassSample& sample) { - return accept_sample(sample.get()); -} -float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const { - Matrix3f softiron( - params.diag.x , params.offdiag.x , params.offdiag.y, - params.offdiag.x , params.diag.y , params.offdiag.z, - params.offdiag.y , params.offdiag.z , params.diag.z - ); - return params.radius - (softiron*(sample+params.offset)).length(); +float CompassCalibrator::calc_mean_squared_residuals() +{ + return optimise.calc_mean_squared_residuals(_params.get_sphere_params()); } -float CompassCalibrator::calc_mean_squared_residuals() const -{ - return calc_mean_squared_residuals(_params); +float CompassCalibrator::calc_sphere_residual(const Vector3f& sample, const float params[], const float const_params[]) { + float radius = params[0]; + Vector3f offset = Vector3f(params[1], params[2], params[3]); + + return radius - ((sample+offset)).length(); } -float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const -{ - if(_sample_buffer == NULL || _samples_collected == 0) { - return 1.0e30f; - } - float sum = 0.0f; - for(uint16_t i=0; i < _samples_collected; i++){ - Vector3f sample = _sample_buffer[i].get(); - float resid = calc_residual(sample, params); - sum += sq(resid); - } - sum /= _samples_collected; - return sum; +float CompassCalibrator::calc_ellipsoid_residual(const Vector3f& sample, const float params[], const float const_params[]) { + + const Vector3f &offset = Vector3f(params[0], params[1], params[2]); + const Vector3f &diag = Vector3f(params[3], params[4], params[5]); + const Vector3f &offdiag = Vector3f(params[6], params[7], params[8]); + + Matrix3f softiron( + diag.x , offdiag.x , offdiag.y, + offdiag.x , diag.y , offdiag.z, + offdiag.y , offdiag.z , diag.z + ); + return const_params[0] - (softiron*(sample+offset)).length(); } -void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const{ - const Vector3f &offset = params.offset; - const Vector3f &diag = params.diag; - const Vector3f &offdiag = params.offdiag; +void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]) { + + const Vector3f &offset = Vector3f(params[1], params[2], params[3]); + const Vector3f &diag = Vector3f(1.0f, 1.0f, 1.0f); + const Vector3f &offdiag = Vector3f(0.0f, 0.0f, 0.0f); Matrix3f softiron( diag.x , offdiag.x , offdiag.y, offdiag.x , diag.y , offdiag.z, @@ -357,93 +368,12 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); } -void CompassCalibrator::run_sphere_fit() -{ - if(_sample_buffer == NULL) { - return; - } - - const float lma_damping = 10.0f; - - float fitness = _fitness; - float fit1, fit2; - param_t fit1_params, fit2_params; - fit1_params = fit2_params = _params; - float JTJ[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS]; - float JTJ2[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS]; - float JTFI[COMPASS_CAL_NUM_SPHERE_PARAMS]; - - memset(&JTJ,0,sizeof(JTJ)); - memset(&JTJ2,0,sizeof(JTJ2)); - memset(&JTFI,0,sizeof(JTFI)); - - for(uint16_t k = 0; k<_samples_collected; k++) { - Vector3f sample = _sample_buffer[k].get(); - - float sphere_jacob[COMPASS_CAL_NUM_SPHERE_PARAMS]; - - calc_sphere_jacob(sample, fit1_params, sphere_jacob); - - for(uint8_t i = 0;i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) { - // compute JTJ - for(uint8_t j = 0; j < COMPASS_CAL_NUM_SPHERE_PARAMS; j++) { - JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j]; - JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM - } - // compute JTFI - JTFI[i] += sphere_jacob[i] * calc_residual(sample, fit1_params); - } - } - - - //------------------------Levenberg-part-starts-here---------------------------------// - for(uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) { - JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda; - JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping; - } - - if(!inverse4x4(JTJ, JTJ)) { - return; - } - - if(!inverse4x4(JTJ2, JTJ2)) { - return; - } - - for(uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) { - for(uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) { - fit1_params.get_sphere_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col]; - fit2_params.get_sphere_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col]; - } - } - - fit1 = calc_mean_squared_residuals(fit1_params); - fit2 = calc_mean_squared_residuals(fit2_params); - - if(fit1 > _fitness && fit2 > _fitness){ - _sphere_lambda *= lma_damping; - } else if(fit2 < _fitness && fit2 < fit1) { - _sphere_lambda /= lma_damping; - fit1_params = fit2_params; - fitness = fit2; - } else if(fit1 < _fitness){ - fitness = fit1; - } - //--------------------Levenberg-part-ends-here--------------------------------// - - if(!isnan(fitness) && fitness < _fitness) { - _fitness = fitness; - _params = fit1_params; - } -} +void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]) { - - -void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const{ - const Vector3f &offset = params.offset; - const Vector3f &diag = params.diag; - const Vector3f &offdiag = params.offdiag; + const Vector3f &offset = Vector3f(params[0], params[1], params[2]); + const Vector3f &diag = Vector3f(params[3], params[4], params[5]); + const Vector3f &offdiag = Vector3f(params[6], params[7], params[8]); Matrix3f softiron( diag.x , offdiag.x , offdiag.y, offdiag.x , diag.y , offdiag.z, @@ -469,679 +399,9 @@ void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length; } -void CompassCalibrator::run_ellipsoid_fit() -{ - if(_sample_buffer == NULL) { - return; - } - - const float lma_damping = 10.0f; - - - float fitness = _fitness; - float fit1, fit2; - param_t fit1_params, fit2_params; - fit1_params = fit2_params = _params; - - - float JTJ[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS]; - float JTJ2[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS]; - float JTFI[COMPASS_CAL_NUM_ELLIPSOID_PARAMS]; - - memset(&JTJ,0,sizeof(JTJ)); - memset(&JTJ2,0,sizeof(JTJ2)); - memset(&JTFI,0,sizeof(JTFI)); - - for(uint16_t k = 0; k<_samples_collected; k++) { - Vector3f sample = _sample_buffer[k].get(); - - float ellipsoid_jacob[COMPASS_CAL_NUM_ELLIPSOID_PARAMS]; - - calc_ellipsoid_jacob(sample, fit1_params, ellipsoid_jacob); - - for(uint8_t i = 0;i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) { - // compute JTJ - for(uint8_t j = 0; j < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; j++) { - JTJ [i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; - JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; - } - // compute JTFI - JTFI[i] += ellipsoid_jacob[i] * calc_residual(sample, fit1_params); - } - } - - - - //------------------------Levenberg-part-starts-here---------------------------------// - for(uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) { - JTJ[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda; - JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda/lma_damping; - } - - if(!inverse9x9(JTJ, JTJ)) { - return; - } - - if(!inverse9x9(JTJ2, JTJ2)) { - return; - } - - for(uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) { - for(uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) { - fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col]; - fit2_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col]; - } - } - - fit1 = calc_mean_squared_residuals(fit1_params); - fit2 = calc_mean_squared_residuals(fit2_params); - - if(fit1 > _fitness && fit2 > _fitness){ - _ellipsoid_lambda *= lma_damping; - } else if(fit2 < _fitness && fit2 < fit1) { - _ellipsoid_lambda /= lma_damping; - fit1_params = fit2_params; - fitness = fit2; - } else if(fit1 < _fitness){ - fitness = fit1; - } - //--------------------Levenberg-part-ends-here--------------------------------// - - if(fitness < _fitness) { - _fitness = fitness; - _params = fit1_params; - } -} - ////////////////////////////////////////////////////////// ////////////////////// MATH HELPERS ////////////////////// ////////////////////////////////////////////////////////// -bool CompassCalibrator::inverse9x9(const float x[81], float y[81]) -{ - if(fabsf(det9x9(x)) < 1.0e-20f) { - return false; - } - float A[81]; - int32_t i0; - int8_t ipiv[9]; - int32_t j; - int32_t c; - int32_t pipk; - int32_t ix; - float smax; - int32_t k; - float s; - int32_t jy; - int32_t ijA; - int8_t p[9]; - for (i0 = 0; i0 < 81; i0++) { - A[i0] = x[i0]; - y[i0] = 0.0; - } - - for (i0 = 0; i0 < 9; i0++) { - ipiv[i0] = (int8_t)(1 + i0); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - pipk = 0; - ix = c; - smax = fabs(A[c]); - for (k = 2; k <= 9 - j; k++) { - ix++; - s = fabs(A[ix]); - if (s > smax) { - pipk = k - 1; - smax = s; - } - } - - if (A[c + pipk] != 0.0) { - if (pipk != 0) { - ipiv[j] = (int8_t)((j + pipk) + 1); - ix = j; - pipk += j; - for (k = 0; k < 9; k++) { - smax = A[ix]; - A[ix] = A[pipk]; - A[pipk] = smax; - ix += 9; - pipk += 9; - } - } - - i0 = (c - j) + 9; - for (jy = c + 1; jy + 1 <= i0; jy++) { - A[jy] /= A[c]; - } - } - - pipk = c; - jy = c + 9; - for (k = 1; k <= 8 - j; k++) { - smax = A[jy]; - if (A[jy] != 0.0) { - ix = c + 1; - i0 = (pipk - j) + 18; - for (ijA = 10 + pipk; ijA + 1 <= i0; ijA++) { - A[ijA] += A[ix] * -smax; - ix++; - } - } - - jy += 9; - pipk += 9; - } - } - - for (i0 = 0; i0 < 9; i0++) { - p[i0] = (int8_t)(1 + i0); - } - - for (k = 0; k < 8; k++) { - if (ipiv[k] > 1 + k) { - pipk = p[ipiv[k] - 1]; - p[ipiv[k] - 1] = p[k]; - p[k] = (int8_t)pipk; - } - } - - for (k = 0; k < 9; k++) { - y[k + 9 * (p[k] - 1)] = 1.0; - for (j = k; j + 1 < 10; j++) { - if (y[j + 9 * (p[k] - 1)] != 0.0) { - for (jy = j + 1; jy + 1 < 10; jy++) { - y[jy + 9 * (p[k] - 1)] -= y[j + 9 * (p[k] - 1)] * A[jy + 9 * j]; - } - } - } - } - - for (j = 0; j < 9; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - pipk = 9 * k; - if (y[k + c] != 0.0) { - 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::inverse6x6(const float x[], float y[]) -{ - if(fabsf(det6x6(x)) < 1.0e-20f) { - return false; - } - - float A[36]; - int32_t i0; - int32_t ipiv[6]; - int32_t j; - int32_t c; - int32_t pipk; - int32_t ix; - float smax; - int32_t k; - float s; - int32_t jy; - int32_t ijA; - int32_t p[6]; - for (i0 = 0; i0 < 36; i0++) { - A[i0] = x[i0]; - y[i0] = 0.0f; - } - - for (i0 = 0; i0 < 6; i0++) { - ipiv[i0] = (int32_t)(1 + i0); - } - - for (j = 0; j < 5; j++) { - c = j * 7; - pipk = 0; - ix = c; - smax = fabsf(A[c]); - for (k = 2; k <= 6 - j; k++) { - ix++; - s = fabsf(A[ix]); - if (s > 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; -} - -float CompassCalibrator::det9x9(const float C[81]) -{ - float f; - float A[81]; - int8_t ipiv[9]; - 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], 81U * sizeof(float)); - for (i0 = 0; i0 < 9; i0++) { - ipiv[i0] = (int8_t)(1 + i0); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - iy = 0; - ix = c; - smax = fabs(A[c]); - for (jy = 2; jy <= 9 - j; jy++) { - ix++; - s = fabs(A[ix]); - if (s > smax) { - iy = jy - 1; - smax = s; - } - } - - if (A[c + iy] != 0.0) { - if (iy != 0) { - ipiv[j] = (int8_t)((j + iy) + 1); - ix = j; - iy += j; - for (jy = 0; jy < 9; jy++) { - smax = A[ix]; - A[ix] = A[iy]; - A[iy] = smax; - ix += 9; - iy += 9; - } - } - - i0 = (c - j) + 9; - for (iy = c + 1; iy + 1 <= i0; iy++) { - A[iy] /= A[c]; - } - } - - iy = c; - jy = c + 9; - for (b_j = 1; b_j <= 8 - j; b_j++) { - smax = A[jy]; - if (A[jy] != 0.0) { - ix = c + 1; - i0 = (iy - j) + 18; - for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { - A[ijA] += A[ix] * -smax; - ix++; - } - } - - jy += 9; - iy += 9; - } - } - - f = A[0]; - isodd = false; - for (jy = 0; jy < 8; jy++) { - f *= A[(jy + 9 * (1 + jy)) + 1]; - if (ipiv[jy] > 1 + jy) { - isodd = !isodd; - } - } - - if (isodd) { - f = -f; - } - - return f; -} uint16_t CompassCalibrator::get_random(void) { @@ -1156,16 +416,16 @@ uint16_t CompassCalibrator::get_random(void) //////////// CompassSample public interface ////////////// ////////////////////////////////////////////////////////// -Vector3f CompassCalibrator::CompassSample::get() const { +Vector3f CompassCalibrator::get_sample(const Vector3i in[], uint16_t num) { 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; + out.x = (float)in[num].x*2048.0f/32700.0f; + out.y = (float)in[num].y*2048.0f/32700.0f; + out.z = (float)in[num].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); +void CompassCalibrator::set_sample(const Vector3f& in, uint16_t num) { + _sample_buffer[num].x = (int16_t)(in.x*32700.0f/2048.0f + 0.5f); + _sample_buffer[num].y = (int16_t)(in.y*32700.0f/2048.0f + 0.5f); + _sample_buffer[num].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 index eb560456de2e3..3ab29aef9faa7 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -41,6 +41,8 @@ class CompassCalibrator { uint8_t get_attempt() const { return _attempt; } private: + Optimiser optimise; + class param_t { public: float* get_sphere_params() { @@ -57,17 +59,8 @@ class CompassCalibrator { Vector3f offdiag; }; - class CompassSample { - public: - Vector3f get() const; - void set(const Vector3f &in); - private: - int16_t x; - int16_t y; - int16_t z; - }; - - + static Vector3f get_sample(const Vector3i in[], uint16_t num); + void set_sample(const Vector3f& in, uint16_t num); enum compass_cal_status_t _status; @@ -85,7 +78,7 @@ class CompassCalibrator { //fit state struct param_t _params; uint16_t _fit_step; - CompassSample *_sample_buffer; + Vector3i *_sample_buffer; float _fitness; // mean squared residuals float _initial_fitness; float _sphere_lambda; @@ -97,7 +90,6 @@ class CompassCalibrator { // returns true if sample should be added to buffer bool accept_sample(const Vector3f &sample); - bool accept_sample(const CompassSample &sample); // returns true if fit is acceptable bool fit_acceptable(); @@ -110,22 +102,15 @@ class CompassCalibrator { // thins out samples between step one and step two void thin_samples(); - float calc_residual(const Vector3f& sample, const param_t& params) const; - float calc_mean_squared_residuals(const param_t& params) const; - float calc_mean_squared_residuals() const; + static float calc_sphere_residual(const Vector3f& sample,const float params[], const float const_params[]); + static float calc_ellipsoid_residual(const Vector3f& sample,const float params[], const float const_params[]); + + float calc_mean_squared_residuals(); - void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const; - void run_sphere_fit(); + static void calc_sphere_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]); - void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const; - void run_ellipsoid_fit(); + static void calc_ellipsoid_jacob(const Vector3f& sample, const float params[], float* ret, const float const_params[]); // math helpers - bool inverse9x9(const float m[],float invOut[]); - float det9x9(const float m[]); - 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(); }; From d309350affe658a428050e815234ad34815c7916 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 12 Mar 2015 04:55:08 +0530 Subject: [PATCH 39/39] Compass: stop fit count to increment unnecessary _fit_count was incremented even when fit is not done after failed/successful completion thus hindering in retrials --- libraries/AP_Compass/CompassCalibrator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 3b2fedc60d3aa..6b486bd556c7e 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -86,8 +86,7 @@ void CompassCalibrator::run_fit_chunk() { if(!fitting()) { return; } - - if (_fit_step == 0){ //initialise optimiser for sphere fit + if (_fit_step == 0){ //initialise optimiser for sphere fit optimise.set_fitness_function(&calc_sphere_residual); optimise.set_jacobian_function(&calc_sphere_jacob); optimise.set_preprocess_sample_function(&get_sample); @@ -117,8 +116,10 @@ void CompassCalibrator::run_fit_chunk() { } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) { if(_fit_step < 15) { optimise.do_levenberg_marquardt_fit(10.0f); + _fit_step++; } else if(_fit_step < 35) { optimise.do_levenberg_marquardt_fit(10.0f); //ellipsoid fit + _fit_step++; } else { if(fit_acceptable()) { set_status(COMPASS_CAL_SUCCESS); @@ -126,7 +127,6 @@ void CompassCalibrator::run_fit_chunk() { set_status(COMPASS_CAL_FAILED); } } - _fit_step++; } }