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

AP_Compass: formatting fixes

Browse Source 
this should be a non-functional change
This commit is contained in:
Randy Mackay 2019-11-15 11:14:24 +09:00 committed by Andrew Tridgell
parent 69a7a52e5f
commit 243cf3b22d
3 changed files with 145 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -8,8 +8,7 @@ extern AP_HAL::HAL& hal;
#if COMPASS_CAL_ENABLED
void
Compass::cal_update()
void Compass::cal_update()
{
if (hal.util->get_soft_armed()) {
return;
@ -47,8 +46,7 @@ Compass::cal_update()
}
}
bool
Compass::_start_calibration(uint8_t i, bool retry, float delay)
bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
{
if (!healthy(i)) {
return false;
@ -82,8 +80,7 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
return true;
}
bool
Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
bool Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
{
_cal_autosave = autosave;
_compass_cal_autoreboot = autoreboot;
@ -99,8 +96,7 @@ Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float
return true;
}
void
Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
void Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
{
_cal_autosave = autosave;
_compass_cal_autoreboot = autoreboot;
@ -112,8 +108,7 @@ Compass::start_calibration_all(bool retry, bool autosave, float delay, bool auto
}
}
void
Compass::_cancel_calibration(uint8_t i)
void Compass::_cancel_calibration(uint8_t i)
{
AP_Notify::events.initiated_compass_cal = 0;
@ -124,8 +119,7 @@ Compass::_cancel_calibration(uint8_t i)
_calibrator[i].clear();
}
void
Compass::_cancel_calibration_mask(uint8_t mask)
void Compass::_cancel_calibration_mask(uint8_t mask)
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
@ -134,14 +128,12 @@ Compass::_cancel_calibration_mask(uint8_t mask)
}
}
void
Compass::cancel_calibration_all()
void Compass::cancel_calibration_all()
{
_cancel_calibration_mask(0xFF);
}
bool
Compass::_accept_calibration(uint8_t i)
bool Compass::_accept_calibration(uint8_t i)
{
CompassCalibrator& cal = _calibrator[i];
uint8_t cal_status = cal.get_status();
@ -172,8 +164,7 @@ Compass::_accept_calibration(uint8_t i)
}
}
bool
Compass::_accept_calibration_mask(uint8_t mask)
bool Compass::_accept_calibration_mask(uint8_t mask)
{
bool success = true;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
@ -263,8 +254,7 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
return true;
}
bool
Compass::is_calibrating() const
bool Compass::is_calibrating() const
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
switch(_calibrator[i].get_status()) {
@ -280,8 +270,7 @@ Compass::is_calibrating() const
return false;
}
uint8_t
Compass::_get_cal_mask() const
uint8_t Compass::_get_cal_mask() const
{
uint8_t cal_mask = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
@ -292,7 +281,6 @@ Compass::_get_cal_mask() const
return cal_mask;
}
/*
handle an incoming MAG_CAL command
*/

81
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -76,7 +76,8 @@ _sample_buffer(nullptr)
clear();
}
void CompassCalibrator::clear() {
void CompassCalibrator::clear()
{
set_status(COMPASS_CAL_NOT_STARTED);
}
@ -94,7 +95,8 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint
set_status(COMPASS_CAL_WAITING_TO_START);
}
void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals) {
void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals)
{
if (_status != COMPASS_CAL_SUCCESS) {
return;
}
@ -104,7 +106,8 @@ void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals,
offdiagonals = _params.offdiag;
}
float CompassCalibrator::get_completion_percent() const {
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) {
@ -147,7 +150,8 @@ void CompassCalibrator::update_completion_mask()
}
}
bool CompassCalibrator::check_for_timeout() {
bool CompassCalibrator::check_for_timeout()
{
uint32_t tnow = AP_HAL::millis();
if (running() && tnow - _last_sample_ms > 1000) {
_retry = false;
@ -157,7 +161,8 @@ bool CompassCalibrator::check_for_timeout() {
return false;
}
void CompassCalibrator::new_sample(const Vector3f& sample) {
void CompassCalibrator::new_sample(const Vector3f& sample)
{
_last_sample_ms = AP_HAL::millis();
if (_status == COMPASS_CAL_WAITING_TO_START) {
@ -172,7 +177,8 @@ void CompassCalibrator::new_sample(const Vector3f& sample) {
}
}
void CompassCalibrator::update(bool &failure) {
void CompassCalibrator::update(bool &failure)
{
failure = false;
if (!fitting()) {
@ -214,15 +220,18 @@ void CompassCalibrator::update(bool &failure) {
/////////////////////////////////////////////////////////////
////////////////////// PRIVATE METHODS //////////////////////
/////////////////////////////////////////////////////////////
bool CompassCalibrator::running() const {
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;
bool CompassCalibrator::fitting() const
{
return running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
}
void CompassCalibrator::initialize_fit() {
void CompassCalibrator::initialize_fit()
{
//initialize _fitness before starting a fit
if (_samples_collected != 0) {
_fitness = calc_mean_squared_residuals(_params);
@ -235,7 +244,8 @@ void CompassCalibrator::initialize_fit() {
_fit_step = 0;
}
void CompassCalibrator::reset_state() {
void CompassCalibrator::reset_state()
{
_samples_collected = 0;
_samples_thinned = 0;
_params.radius = 200;
@ -247,7 +257,8 @@ void CompassCalibrator::reset_state() {
initialize_fit();
}
bool CompassCalibrator::set_status(compass_cal_status_t status) {
bool CompassCalibrator::set_status(compass_cal_status_t status)
{
if (status != COMPASS_CAL_NOT_STARTED && _status == status) {
return true;
}
@ -256,7 +267,6 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
case COMPASS_CAL_NOT_STARTED:
reset_state();
_status = COMPASS_CAL_NOT_STARTED;
if (_sample_buffer != nullptr) {
free(_sample_buffer);
_sample_buffer = nullptr;
@ -266,7 +276,6 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
case COMPASS_CAL_WAITING_TO_START:
reset_state();
_status = COMPASS_CAL_WAITING_TO_START;
set_status(COMPASS_CAL_RUNNING_STEP_ONE);
return true;
@ -280,16 +289,13 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
}
if (_sample_buffer == nullptr) {
_sample_buffer =
(CompassSample*) calloc(COMPASS_CAL_NUM_SAMPLES, sizeof(CompassSample));
_sample_buffer = (CompassSample*)calloc(COMPASS_CAL_NUM_SAMPLES, sizeof(CompassSample));
}
if (_sample_buffer != nullptr) {
initialize_fit();
_status = COMPASS_CAL_RUNNING_STEP_ONE;
return true;
}
return false;
case COMPASS_CAL_RUNNING_STEP_TWO:
@ -344,7 +350,8 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
};
}
bool CompassCalibrator::fit_acceptable() {
bool CompassCalibrator::fit_acceptable()
{
if (!isnan(_fitness) &&
_params.radius > 150 && _params.radius < 950 && //Earth's magnetic field strength range: 250-850mG
fabsf(_params.offset.x) < _offset_max &&
@ -356,13 +363,13 @@ bool CompassCalibrator::fit_acceptable() {
fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1
fabsf(_params.offdiag.y) < 1.0f &&
fabsf(_params.offdiag.z) < 1.0f ) {
return _fitness <= sq(_tolerance);
}
return false;
}
void CompassCalibrator::thin_samples() {
void CompassCalibrator::thin_samples()
{
if (_sample_buffer == nullptr) {
return;
}
@ -425,11 +432,13 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample)
return true;
}
bool CompassCalibrator::accept_sample(const CompassSample& sample) {
bool CompassCalibrator::accept_sample(const CompassSample& sample)
{
return accept_sample(sample.get());
}
float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const {
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,
@ -458,7 +467,8 @@ float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) cons
return sum;
}
void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const{
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;
@ -527,7 +537,6 @@ void CompassCalibrator::run_sphere_fit()
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
@ -571,9 +580,8 @@ void CompassCalibrator::run_sphere_fit()
}
}
void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const{
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;
@ -610,13 +618,11 @@ void CompassCalibrator::run_ellipsoid_fit()
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] = { };
@ -640,8 +646,6 @@ void CompassCalibrator::run_ellipsoid_fit()
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
for (uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
@ -693,20 +697,22 @@ void CompassCalibrator::run_ellipsoid_fit()
#define COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(__X) ((int16_t)constrain_float(roundf(__X*8.0f), INT16_MIN, INT16_MAX))
#define COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(__X) (__X/8.0f)
Vector3f CompassCalibrator::CompassSample::get() const {
Vector3f CompassCalibrator::CompassSample::get() const
{
return Vector3f(COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(x),
COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(y),
COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(z));
}
void CompassCalibrator::CompassSample::set(const Vector3f &in) {
void CompassCalibrator::CompassSample::set(const Vector3f &in)
{
x = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.x);
y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
}
void CompassCalibrator::AttitudeSample::set_from_ahrs(void) {
void CompassCalibrator::AttitudeSample::set_from_ahrs(void)
{
const Matrix3f &dcm = AP::ahrs().get_DCM_rotation_body_to_ned();
float roll_rad, pitch_rad, yaw_rad;
dcm.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
@ -715,7 +721,8 @@ void CompassCalibrator::AttitudeSample::set_from_ahrs(void) {
yaw = constrain_int16(127 * (yaw_rad / M_PI), -127, 127);
}
Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) {
Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void)
{
float roll_rad, pitch_rad, yaw_rad;
roll_rad = roll * (M_PI / 127);
pitch_rad = pitch * (M_PI_2 / 127);