mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: switched to a vector based interface
this is more consistent with other APIs and makes multi-device support easier
This commit is contained in:
Andrew Tridgell
parent
df001faf34
commit
cb16733918
|
|
@ -63,9 +63,7 @@ bool AP_Compass_HIL::read()
|
|||
}
|
||||
|
||||
// return last values provided by setHIL function
|
||||
mag_x = _hil_mag.x + ofs.x + _motor_offset.x;
|
||||
mag_y = _hil_mag.y + ofs.y + _motor_offset.y;
|
||||
mag_z = _hil_mag.z + ofs.z + _motor_offset.z;
|
||||
_field = _hil_mag + ofs + _motor_offset;
|
||||
|
||||
// values set by setHIL function
|
||||
last_update = hal.scheduler->micros(); // record time of update
|
||||
|
|
|
|||
|
|
@ -316,16 +316,16 @@ bool AP_Compass_HMC5843::read()
|
|||
}
|
||||
}
|
||||
|
||||
mag_x = _mag_x_accum * calibration[0] / _accum_count;
|
||||
mag_y = _mag_y_accum * calibration[1] / _accum_count;
|
||||
mag_z = _mag_z_accum * calibration[2] / _accum_count;
|
||||
_field.x = _mag_x_accum * calibration[0] / _accum_count;
|
||||
_field.y = _mag_y_accum * calibration[1] / _accum_count;
|
||||
_field.z = _mag_z_accum * calibration[2] / _accum_count;
|
||||
_accum_count = 0;
|
||||
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
|
||||
|
||||
last_update = hal.scheduler->micros(); // record time of update
|
||||
|
||||
// rotate to the desired orientation
|
||||
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
|
||||
Vector3f rot_mag = Vector3f(_field.x,_field.y,_field.z);
|
||||
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
|
||||
rot_mag.rotate(ROTATION_YAW_90);
|
||||
}
|
||||
|
|
@ -354,9 +354,9 @@ bool AP_Compass_HMC5843::read()
|
|||
_motor_offset.z = 0;
|
||||
}
|
||||
|
||||
mag_x = rot_mag.x;
|
||||
mag_y = rot_mag.y;
|
||||
mag_z = rot_mag.z;
|
||||
_field.x = rot_mag.x;
|
||||
_field.y = rot_mag.y;
|
||||
_field.z = rot_mag.z;
|
||||
healthy = true;
|
||||
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -121,14 +121,10 @@ bool AP_Compass_PX4::read(void)
|
|||
_motor_offset = _motor_compensation.get() * _thr_or_curr;
|
||||
_sum += _motor_offset;
|
||||
} else {
|
||||
_motor_offset.x = 0;
|
||||
_motor_offset.y = 0;
|
||||
_motor_offset.z = 0;
|
||||
_motor_offset.zero();
|
||||
}
|
||||
|
||||
mag_x = _sum.x;
|
||||
mag_y = _sum.y;
|
||||
mag_z = _sum.z;
|
||||
_field = _sum;
|
||||
|
||||
_sum.zero();
|
||||
_count = 0;
|
||||
|
|
|
|||
|
|
@ -192,10 +192,10 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
|
|||
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
|
||||
|
||||
// Tilt compensated magnetic field Y component:
|
||||
float headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y;
|
||||
float headY = _field.y * dcm_matrix.c.z - _field.z * dcm_matrix.c.y;
|
||||
|
||||
// Tilt compensated magnetic field X component:
|
||||
float headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z);
|
||||
float headX = _field.x * cos_pitch_sq - dcm_matrix.c.x * (_field.y * dcm_matrix.c.y + _field.z * dcm_matrix.c.z);
|
||||
|
||||
// magnetic heading
|
||||
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
|
||||
|
|
@ -258,7 +258,7 @@ Compass::null_offsets(void)
|
|||
for (uint8_t i=0; i<_mag_history_size; i++) {
|
||||
// fill the history buffer with the current mag vector,
|
||||
// with the offset removed
|
||||
_mag_history[i] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z);
|
||||
_mag_history[i] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.z+0.5f) - ofs.z);
|
||||
}
|
||||
_mag_history_index = 0;
|
||||
return;
|
||||
|
|
@ -275,7 +275,7 @@ Compass::null_offsets(void)
|
|||
b1 += ofs;
|
||||
|
||||
// get the current vector
|
||||
b2 = Vector3f(mag_x, mag_y, mag_z);
|
||||
b2 = Vector3f(_field.x, _field.y, _field.z);
|
||||
|
||||
// calculate the delta for this sample
|
||||
diff = b2 - b1;
|
||||
|
|
@ -293,7 +293,7 @@ Compass::null_offsets(void)
|
|||
}
|
||||
|
||||
// put the vector in the history
|
||||
_mag_history[_mag_history_index] = Vector3i((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z);
|
||||
_mag_history[_mag_history_index] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.z+0.5f) - ofs.z);
|
||||
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
|
||||
|
||||
// equation 6 of Bills paper
|
||||
|
|
|
|||
|
|
@ -41,9 +41,6 @@ class Compass
|
|||
{
|
||||
public:
|
||||
int16_t product_id; /// product id
|
||||
int16_t mag_x; ///< magnetic field strength along the X axis
|
||||
int16_t mag_y; ///< magnetic field strength along the Y axis
|
||||
int16_t mag_z; ///< magnetic field strength along the Z axis
|
||||
uint32_t last_update; ///< micros() time of last update
|
||||
bool healthy; ///< true if last read OK
|
||||
|
||||
|
|
@ -62,6 +59,8 @@ public:
|
|||
///
|
||||
virtual bool read(void) = 0;
|
||||
|
||||
|
||||
|
||||
/// use spare CPU cycles to accumulate values from the compass if
|
||||
/// possible
|
||||
virtual void accumulate(void) = 0;
|
||||
|
|
@ -87,6 +86,12 @@ public:
|
|||
///
|
||||
void save_offsets();
|
||||
|
||||
/// Return the current field as a Vector3f
|
||||
const Vector3f &get_field(void) const { return _field; }
|
||||
|
||||
/// set the current field as a Vector3f
|
||||
void set_field(const Vector3f &field) { _field = field; }
|
||||
|
||||
/// Returns the current offset values
|
||||
///
|
||||
/// @returns The current compass offsets.
|
||||
|
|
@ -194,6 +199,8 @@ public:
|
|||
AP_Int8 _learn; ///<enable calibration learning
|
||||
|
||||
protected:
|
||||
Vector3f _field; ///< magnetic field strength
|
||||
|
||||
AP_Int8 _orientation;
|
||||
AP_Vector3f _offset;
|
||||
AP_Float _declination;
|
||||
|
|
|
|||
|
|
@ -84,20 +84,21 @@ void loop()
|
|||
compass.null_offsets();
|
||||
|
||||
// capture min
|
||||
if( compass.mag_x < min[0] )
|
||||
min[0] = compass.mag_x;
|
||||
if( compass.mag_y < min[1] )
|
||||
min[1] = compass.mag_y;
|
||||
if( compass.mag_z < min[2] )
|
||||
min[2] = compass.mag_z;
|
||||
const Vector3f &mag = compass.get_field();
|
||||
if( mag.x < min[0] )
|
||||
min[0] = mag.x;
|
||||
if( mag.y < min[1] )
|
||||
min[1] = mag.y;
|
||||
if( mag.z < min[2] )
|
||||
min[2] = mag.z;
|
||||
|
||||
// capture max
|
||||
if( compass.mag_x > max[0] )
|
||||
max[0] = compass.mag_x;
|
||||
if( compass.mag_y > max[1] )
|
||||
max[1] = compass.mag_y;
|
||||
if( compass.mag_z > max[2] )
|
||||
max[2] = compass.mag_z;
|
||||
if( mag.x > max[0] )
|
||||
max[0] = mag.x;
|
||||
if( mag.y > max[1] )
|
||||
max[1] = mag.y;
|
||||
if( mag.z > max[2] )
|
||||
max[2] = mag.z;
|
||||
|
||||
// calculate offsets
|
||||
offset[0] = -(max[0]+min[0])/2;
|
||||
|
|
@ -107,9 +108,9 @@ void loop()
|
|||
// display all to user
|
||||
hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
|
||||
ToDeg(heading),
|
||||
(int)compass.mag_x,
|
||||
(int)compass.mag_y,
|
||||
(int)compass.mag_z,
|
||||
(int)mag.x,
|
||||
(int)mag.y,
|
||||
(int)mag.z,
|
||||
(unsigned)hal.i2c->lockup_count());
|
||||
|
||||
// display offsets
|
||||
|
|
|
|||
Loading…
Reference in New Issue