Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: support motor compensation for multiple compasses

Browse Source 
Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2013-12-09 19:45:31 +11:00
parent 0a7a935f63
commit 61f564d7c9
5 changed files with 23 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -55,15 +55,13 @@ bool AP_Compass_HIL::read()
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
_motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
}else{
_motor_offset.x = 0;
_motor_offset.y = 0;
_motor_offset.z = 0;
_motor_offset[0].zero();
}
// return last values provided by setHIL function
_field[0] = _hil_mag + ofs + _motor_offset;
_field[0] = _hil_mag + ofs + _motor_offset[0];
// values set by setHIL function
last_update = hal.scheduler->micros(); // record time of update

8
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -345,12 +345,10 @@ bool AP_Compass_HMC5843::read()
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
_field[0] += _motor_offset;
_motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
_field[0] += _motor_offset[0];
}else{
_motor_offset.x = 0;
_motor_offset.y = 0;
_motor_offset.z = 0;
_motor_offset[0].zero();
}
_healthy[0] = true;

6
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -114,10 +114,10 @@ bool AP_Compass_PX4::read(void)
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
_sum[i] += _motor_offset;
_motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
_sum[i] += _motor_offset[i];
} else {
_motor_offset.zero();
_motor_offset[i].zero();
}
_field[i] = _sum[i];

11
libraries/AP_Compass/Compass.cpp
View File

@ -83,7 +83,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Range: -1000 1000
// @Units: Offset per Amp or at Full Throttle
// @Increment: 1
AP_GROUPINFO("MOT", 7, Compass, _motor_compensation, 0),
AP_GROUPINFO("MOT", 7, Compass, _motor_compensation[0], 0),
// @Param: ORIENT
// @DisplayName: Compass orientation
@ -100,6 +100,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
#if COMPASS_MAX_INSTANCES > 1
AP_GROUPINFO("OFS2", 10, Compass, _offset[1], 0),
AP_GROUPINFO("MOT2", 11, Compass, _motor_compensation[1], 0),
#endif
AP_GROUPEND
@ -139,16 +140,18 @@ Compass::save_offsets()
}
void
Compass::set_motor_compensation(const Vector3f &motor_comp_factor)
Compass::set_motor_compensation(const Vector3f &motor_comp_factor, uint8_t i)
{
_motor_compensation.set(motor_comp_factor);
_motor_compensation[i].set(motor_comp_factor);
}
void
Compass::save_motor_compensation()
{
_motor_comp_type.save();
_motor_compensation.save();
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
_motor_compensation[k].save();
}
}
void

14
libraries/AP_Compass/Compass.h
View File

@ -176,12 +176,11 @@ public:
///
/// @param offsets Offsets multiplied by the throttle value and added to the raw mag_ values.
///
void set_motor_compensation(const Vector3f &motor_comp_factor);
void set_motor_compensation(const Vector3f &motor_comp_factor, uint8_t i=0);
/// get motor compensation factors as a vector
const Vector3f& get_motor_compensation() const {
return _motor_compensation;
}
const Vector3f& get_motor_compensation(uint8_t i) const { return _motor_compensation[i]; }
const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(0); }
/// Saves the current motor compensation x/y/z values.
///
@ -193,7 +192,8 @@ public:
///
/// @returns The current compass offsets.
///
const Vector3f &get_motor_offsets() const { return _motor_offset; }
const Vector3f &get_motor_offsets(uint8_t i) const { return _motor_offset[i]; }
const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(0); }
/// Set the throttle as a percentage from 0.0 to 1.0
/// @param thr_pct throttle expressed as a percentage from 0 to 1.0
@ -236,8 +236,8 @@ protected:
// motor compensation
AP_Int8 _motor_comp_type; // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Vector3f _motor_compensation; // factors multiplied by throttle and added to compass outputs
Vector3f _motor_offset; // latest compensation added to compass
AP_Vector3f _motor_compensation[COMPASS_MAX_INSTANCES]; // factors multiplied by throttle and added to compass outputs
Vector3f _motor_offset[COMPASS_MAX_INSTANCES]; // latest compensation added to compass
float _thr_or_curr; // throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps
// board orientation from AHRS