Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AccelCal: basic sanity check on fit parameters

This commit is contained in:
Jonathan Challinger 2015-10-29 17:57:47 -07:00
parent 5cd0ca851a
commit 660d9e86d5
2 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_AccelCal/AccelCalibrator.cpp
View File

@ -135,7 +135,7 @@ void AccelCalibrator::new_sample(const Vector3f delta_velocity, float dt) {
if (_samples_collected >= _conf_num_samples) { if (_samples_collected >= _conf_num_samples) {
run_fit(MAX_ITERATIONS, _fitness); run_fit(MAX_ITERATIONS, _fitness);
if (_fitness < _conf_tolerance) { if (_fitness < _conf_tolerance && accept_result()) {
set_status(ACCEL_CAL_SUCCESS); set_status(ACCEL_CAL_SUCCESS);
} else { } else {
set_status(ACCEL_CAL_FAILED); set_status(ACCEL_CAL_FAILED);
@ -146,6 +146,20 @@ void AccelCalibrator::new_sample(const Vector3f delta_velocity, float dt) {
} }
} }
// determines if the result is acceptable
bool AccelCalibrator::accept_result() const {
if (fabsf(_param_struct.offset.x) > GRAVITY_MSS ||
fabsf(_param_struct.offset.y) > GRAVITY_MSS ||
fabsf(_param_struct.offset.z) > GRAVITY_MSS ||
_param_struct.diag.x < 0.8f || _param_struct.diag.x > 1.2f ||
_param_struct.diag.y < 0.8f || _param_struct.diag.y > 1.2f ||
_param_struct.diag.z < 0.8f || _param_struct.diag.z > 1.2f) {
return false;
} else {
return true;
}
}
// interface for LSq estimator to read sample buffer sent after conversion from delta velocity // interface for LSq estimator to read sample buffer sent after conversion from delta velocity
// to averaged acc over time // to averaged acc over time
bool AccelCalibrator::get_sample(uint8_t i, Vector3f& s) const { bool AccelCalibrator::get_sample(uint8_t i, Vector3f& s) const {

3
libraries/AP_AccelCal/AccelCalibrator.h
View File

@ -120,6 +120,9 @@ private:
// sets status of calibrator and takes appropriate actions // sets status of calibrator and takes appropriate actions
void set_status(enum accel_cal_status_t); void set_status(enum accel_cal_status_t);
// determines if the result is acceptable
bool accept_result() const;
// returns number of paramters are required for selected Fit type // returns number of paramters are required for selected Fit type
uint8_t get_num_params() const; uint8_t get_num_params() const;