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; }