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