void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag)
{
Vector3f raw_field = mag * 1000.0;
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, _instance);
// correct raw_field for known errors
correct_field(raw_field, _instance);
if (_sem_mag->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
// accumulate into averaging filter
_sum += raw_field;
_count++;
_sem_mag->give();
}
}
/*
* Accumulate a reading from the magnetometer
*
* bus semaphore must not be taken
*/
void AP_Compass_HMC5843::accumulate()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return;
}
uint32_t tnow = AP_HAL::micros();
if (_accum_count != 0 && (tnow - _last_accum_time) < 13333) {
// the compass gets new data at 75Hz
return;
}
if (!_bus->get_semaphore()->take(1)) {
// the bus is busy - try again later
return;
}
bool result = _read_sample();
_bus->get_semaphore()->give();
if (!result) {
return;
}
// the _mag_N values are in the range -2048 to 2047, so we can
// accumulate up to 15 of them in an int16_t. Let's make it 14
// for ease of calculation. We expect to do reads at 10Hz, and
// we get new data at most 75Hz, so we don't expect to
// accumulate more than 8 before a read
// get raw_field - sensor frame, uncorrected
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
raw_field *= _gain_scale;
// rotate to the desired orientation
if (is_external(_compass_instance) &&
_product_id == AP_COMPASS_TYPE_HMC5883L) {
raw_field.rotate(ROTATION_YAW_90);
}
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _compass_instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, tnow, _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 14) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 7;
}
_last_accum_time = tnow;
}
