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

Compass: current based compensation added

Browse Source
This commit is contained in:
Randy Mackay 2013-03-03 23:02:12 +09:00
parent 7f1c8fd364
commit b8d492b504
5 changed files with 59 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -21,8 +21,14 @@ bool AP_Compass_HIL::read()
// get offsets
Vector3f ofs = _offset.get();
// get motor compensation
_motor_offset = _motor_compensation.get() * _throttle_pct;
// 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;
}else{
_motor_offset.x = 0;
_motor_offset.y = 0;
_motor_offset.z = 0;
}
// return last values provided by setHIL function
mag_x = _hil_mag.x + ofs.x + _motor_offset.x;

6
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -331,9 +331,9 @@ bool AP_Compass_HMC5843::read()
rot_mag += _offset.get();
// add in motor compensation
if( _throttle_pct != 0 ) {
_motor_offset = _motor_compensation.get() * _throttle_pct;
// 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;
rot_mag += _motor_offset;
}else{
_motor_offset.x = 0;

6
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -67,9 +67,9 @@ bool AP_Compass_PX4::read(void)
_sum.rotate(_board_orientation);
_sum += _offset.get();
// add in motor compensation
if( _throttle_pct != 0 ) {
_motor_offset = _motor_compensation.get() * _throttle_pct;
// 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 += _motor_offset;
}else{
_motor_offset.x = 0;

10
libraries/AP_Compass/Compass.cpp
View File

@ -56,6 +56,13 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1),
#endif
// @Param: MOTCT
// @DisplayName: Motor interference compensation type
// @Description: Set motor interference compensation type to disabled, throttle or current
// @Values: 0:Disabled,1:Use Throttle,2:Use Current
// @Increment: 1
AP_GROUPINFO("MOTCT", 6, Compass, _motor_comp_type, AP_COMPASS_MOT_COMP_DISABLED),
// @Param: MOT_X
// @DisplayName: Motor interference compensation for body frame X axis
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference
@ -73,7 +80,7 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference
// @Range: -1000 1000
// @Increment: 1
AP_GROUPINFO("MOT", 6, Compass, _motor_compensation, 0),
AP_GROUPINFO("MOT", 7, Compass, _motor_compensation, 0),
AP_GROUPEND
};
@ -131,6 +138,7 @@ Compass::set_motor_compensation(const Vector3f &motor_comp_factor)
void
Compass::save_motor_compensation()
{
_motor_comp_type.save();
_motor_compensation.save();
}

39
libraries/AP_Compass/Compass.h
View File

@ -15,6 +15,11 @@
#define AP_COMPASS_TYPE_HMC5883L 0x03
#define AP_COMPASS_TYPE_PX4 0x04
// motor compensation types (for use with motor_comp_enabled)
#define AP_COMPASS_MOT_COMP_DISABLED 0x00
#define AP_COMPASS_MOT_COMP_THROTTLE 0x01
#define AP_COMPASS_MOT_COMP_CURRENT 0x02
class Compass
{
public:
@ -126,6 +131,23 @@ public:
_board_orientation = orientation;
}
/// Set the motor compensation type
///
/// @param comp_type 0 = disabled, 1 = enabled use throttle, 2 = enabled use current
///
void motor_compensation_type(const uint8_t comp_type) {
if( comp_type >= AP_COMPASS_MOT_COMP_DISABLED && _motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) {
_motor_comp_type = (int8_t)comp_type;
_thr_or_curr = 0; // set current current or throttle to zero
set_motor_compensation(Vector3f(0,0,0)); // clear out invalid compensation vector
}
}
/// get the motor compensation value.
uint8_t motor_compensation_type() {
return _motor_comp_type;
}
/// Set the motor compensation factor x/y/z values.
///
/// @param offsets Offsets multiplied by the throttle value and added to the raw mag_ values.
@ -152,7 +174,17 @@ public:
/// 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
void set_throttle(float thr_pct) {
_throttle_pct = thr_pct;
if(_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
_thr_or_curr = thr_pct;
}
}
/// Set the current used by system in amps
/// @param amps current flowing to the motors expressed in amps
void set_current(float amps) {
if(_motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
_thr_or_curr = amps;
}
}
static const struct AP_Param::GroupInfo var_info[];
@ -173,9 +205,10 @@ protected:
Vector3i _mag_history[_mag_history_size];
// 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
float _throttle_pct; // throttle expressed as a percentage from 0.0 ~ 1.0
Vector3f _motor_offset; // 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
enum Rotation _board_orientation;