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