/*
* Take accumulated reads from the magnetometer or try to read once if no
* valid data
*
* bus semaphore must not be locked
*/
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 (_accum_count == 0) {
accumulate();
if (_retry_time != 0) {
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;
publish_filtered_field(field, _compass_instance);
}
void AP_Compass_UAVCAN::read(void)
{
// avoid division by zero if we haven't received any mag reports
if (_count == 0) {
return;
}
if (_mag_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_sum /= _count;
publish_filtered_field(_sum, _instance);
_sum.zero();
_count = 0;
_mag_baro->give();
}
}
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++) {
uint8_t frontend_instance = _instance[i];
// avoid division by zero if we haven't received any mag reports
if (_count[i] == 0) continue;
_sum[i] /= _count[i];
publish_filtered_field(_sum[i], frontend_instance);
_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 (AP_HAL::millis() < _retry_time) {
return;
}
if (!re_initialise()) {
_retry_time = AP_HAL::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_filtered_field(field, _compass_instance);
_retry_time = 0;
}
