Example #1
0
void AP_Compass_PX4::accumulate(void)
{
    struct mag_report mag_report;
    for (uint8_t i=0; i<_num_sensors; i++) {
        uint8_t frontend_instance = _instance[i];
        while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
               mag_report.timestamp != _last_timestamp[i]) {

            uint32_t time_us = (uint32_t)mag_report.timestamp;
            // get raw_field - sensor frame, uncorrected
            Vector3f raw_field = Vector3f(mag_report.x, mag_report.y, mag_report.z)*1.0e3f;

            // rotate raw_field from sensor frame to body frame
            rotate_field(raw_field, frontend_instance);

            // publish raw_field (uncorrected point sample) for calibration use
            publish_raw_field(raw_field, time_us, frontend_instance);

            // correct raw_field for known errors
            correct_field(raw_field, frontend_instance);

            // publish raw_field (corrected point sample) for EKF use
            publish_unfiltered_field(raw_field, time_us, frontend_instance);

            // accumulate into averaging filter
            _sum[i] += raw_field;
            _count[i]++;

            _last_timestamp[i] = mag_report.timestamp;
        }
    }
}
// accumulate a reading from the magnetometer
void AP_Compass_HMC5843::accumulate(void)
{
    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_sem->take(1)) {
       // the bus is busy - try again later
       return;
   }
   bool result = read_raw();
   _bus_sem->give();

   if (result) {
	  // 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_multiple;

       // 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);

       // publish raw_field (corrected point sample) for EKF use
       publish_unfiltered_field(raw_field, tnow, _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;
   }
}
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 (_mag_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        // accumulate into averaging filter
        _sum += raw_field;
        _count++;
        _mag_baro->give();
    }
}