bool AP_Compass_AK8963::read()
{
if (!_initialised) {
return false;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
return true;
}
/* Update */
_field[0].x = _mag_x_accum * magnetometer_ASA[0] / _accum_count;
_field[0].y = _mag_y_accum * magnetometer_ASA[1] / _accum_count;
_field[0].z = _mag_z_accum * magnetometer_ASA[2] / _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
// apply default board orientation for this compass type. This is
// a noop on most boards
_field[0].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_field[0].rotate((enum Rotation)_orientation[0].get());
if (!_external[0]) {
// and add in AHRS_ORIENTATION setting if not an external compass
_field[0].rotate(_board_orientation);
}
apply_corrections(_field[0],0);
#if 0
float x = _field[0].x;
float y = _field[0].y;
float z = _field[0].z;
error("%f %f %f\n", x, y, z);
#endif
last_update = hal.scheduler->micros(); // record time of update
return true;
}
bool AP_Compass_PX4::read(void)
{
// try to accumulate one more sample, so we have the latest data
accumulate();
// consider the compass healthy if we got a reading in the last 0.2s
for (uint8_t i=0; i<_num_instances; i++) {
_healthy[i] = (hal.scheduler->micros64() - _last_timestamp[i] < 200000);
}
for (uint8_t i=0; i<_num_instances; 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;
// apply default board orientation for this compass type. This is
// a noop on most boards
_sum[i].rotate(MAG_BOARD_ORIENTATION);
if (_external[i]) {
// add user selectable orientation
_sum[i].rotate((enum Rotation)_orientation[i].get());
} else {
// add in board orientation from AHRS
_sum[i].rotate(_board_orientation);
}
_field[i] = _sum[i];
apply_corrections(_field[i],i);
_sum[i].zero();
_count[i] = 0;
}
last_update = _last_timestamp[get_primary()];
return _healthy[get_primary()];
}
/*
copy latest data to the frontend from a backend
*/
void AP_Compass_Backend::publish_field(const Vector3f &mag, uint8_t instance)
{
Compass::mag_state &state = _compass._state[instance];
state.field = mag;
// apply default board orientation for this compass type. This is
// a noop on most boards
state.field.rotate(MAG_BOARD_ORIENTATION);
if (!state.external) {
// and add in AHRS_ORIENTATION setting if not an external compass
state.field.rotate(_compass._board_orientation);
} else {
// add user selectable orientation
state.field.rotate((enum Rotation)state.orientation.get());
}
apply_corrections(state.field, instance);
state.last_update_ms = hal.scheduler->millis();
_compass._last_update_usec = hal.scheduler->micros();
}
void test_sparse_apply_corrections() {
Hyperloglog *hll = mock_sparse_hll(8);
uint cardinality = hyperloglog_cardinality(hll, compute_alpha(hll->m));
sassert(3 == apply_corrections(hll, cardinality));
free(hll);
}
