void AP_Compass_PX4::read(void)
{
// try to accumulate one more sample, so we have the latest data
accumulate();
for (uint8_t i=0; i<_num_sensors; i++) {
// avoid division by zero if we haven't received any mag reports
if (_count[i] == 0) continue;
_sum[i] /= _count[i];
_sum[i] *= 1000;
publish_field(_sum[i], _instance[i]);
_sum[i].zero();
_count[i] = 0;
}
}
// Read Sensor data
void AP_Compass_HMC5843::read()
{
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;
}
if (_retry_time != 0) {
if (hal.scheduler->millis() < _retry_time) {
return;
}
if (!re_initialise()) {
_retry_time = hal.scheduler->millis() + 1000;
_bus->set_high_speed(false);
return;
}
}
if (_accum_count == 0) {
accumulate();
if (_retry_time != 0) {
_bus->set_high_speed(false);
return;
}
}
Vector3f field(_mag_x_accum * _scaling[0],
_mag_y_accum * _scaling[1],
_mag_z_accum * _scaling[2]);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
// rotate to the desired orientation
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
field.rotate(ROTATION_YAW_90);
}
publish_field(field, _compass_instance);
_retry_time = 0;
}
void AP_Compass_AK8963::read()
{
if (!_initialised) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
return;
}
/* Update */
Vector3f field(_mag_x_accum * magnetometer_ASA[0],
_mag_y_accum * magnetometer_ASA[1],
_mag_z_accum * magnetometer_ASA[2]);
field /= _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
publish_field(field, _compass_instance);
}
