aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: use apply_correction_function to eliminate duplication

Browse Source
This commit is contained in:
Jonathan Challinger 2014-07-31 18:19:53 -07:00 committed by Andrew Tridgell
parent 677f6cce7f
commit 9054dd3f9a
6 changed files with 23 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -50,18 +50,9 @@ void AP_Compass_HIL::_setup_earth_field(void)
bool AP_Compass_HIL::read()
{
// get offsets
Vector3f ofs = _offset[0].get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
}else{
_motor_offset[0].zero();
}
// return last values provided by setHIL function
_field[0] = _hil_mag + ofs + _motor_offset[0];
_field[0] = _hil_mag;
apply_corrections(_field[0],0);
// values set by setHIL function
last_update = hal.scheduler->micros(); // record time of update

10
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -371,15 +371,7 @@ bool AP_Compass_HMC5843::read()
_field[0].rotate(_board_orientation);
}
_field[0] += _offset[0].get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
_field[0] += _motor_offset[0];
}else{
_motor_offset[0].zero();
}
apply_corrections(_field[0],0);
_healthy[0] = true;

13
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -115,18 +115,9 @@ bool AP_Compass_PX4::read(void)
// add in board orientation from AHRS
_sum[i].rotate(_board_orientation);
}
_sum[i] += _offset[i].get();
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
_sum[i] += _motor_offset[i];
} else {
_motor_offset[i].zero();
}
_field[i] = _sum[i];
apply_corrections(_field[i],i);
_sum[i].zero();
_count[i] = 0;

11
libraries/AP_Compass/AP_Compass_VRBRAIN.cpp
View File

@ -124,18 +124,9 @@ bool AP_Compass_VRBRAIN::read(void)
// add in board orientation from AHRS
_sum[i].rotate(_board_orientation);
}
_sum[i] += _offset[i].get();
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
_sum[i] += _motor_offset[i];
} else {
_motor_offset[i].zero();
}
_field[i] = _sum[i];
apply_corrections(_field[i],i);
_sum[i].zero();
_count[i] = 0;

14
libraries/AP_Compass/Compass.cpp
View File

@ -462,3 +462,17 @@ bool Compass::configured(void)
}
return all_configured;
}
void Compass::apply_corrections(Vector3f &mag, uint8_t i)
{
const Vector3f &offsets = _offset[i].get();
const Vector3f &mot = _motor_compensation[i].get();
mag += offsets;
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[i] = mot * _thr_or_curr;
mag += _motor_offset[i];
}else{
_motor_offset[i].zero();
}
}

2
libraries/AP_Compass/Compass.h
View File

@ -282,5 +282,7 @@ protected:
// board orientation from AHRS
enum Rotation _board_orientation;
void apply_corrections(Vector3f &mag, uint8_t i);
};
#endif