void PulseAudioDriver::stream_write_callback(pa_stream* stream, size_t bytes, void* udata) { PulseAudioDriver* self = (PulseAudioDriver*)udata; void* vdata; pa_stream_begin_write(stream, &vdata, &bytes); if (!vdata) return; short* out = (short*)vdata; unsigned num_samples = bytes / 4; while (num_samples) { int n = std::min(self->m_buffer_size, num_samples); self->m_callback(n, 0); for (int i = 0; i < n; ++i) { *out++ = FLOAT_TO_SHORT(self->m_outL[i]); *out++ = FLOAT_TO_SHORT(self->m_outR[i]); } num_samples -= n; } pa_stream_write(stream, vdata, (bytes / 4) * 4, 0, 0, PA_SEEK_RELATIVE); }
/** * Clear the accumulation buffer by mapping the renderbuffer and * writing the clear color to it. Called by the driver's implementation * of the glClear function. */ void _mesa_clear_accum_buffer(struct gl_context *ctx) { GLuint x, y, width, height; GLubyte *accMap; GLint accRowStride; struct gl_renderbuffer *accRb; if (!ctx->DrawBuffer) return; accRb = ctx->DrawBuffer->Attachment[BUFFER_ACCUM].Renderbuffer; if (!accRb) return; /* missing accum buffer, not an error */ /* bounds, with scissor */ x = ctx->DrawBuffer->_Xmin; y = ctx->DrawBuffer->_Ymin; width = ctx->DrawBuffer->_Xmax - ctx->DrawBuffer->_Xmin; height = ctx->DrawBuffer->_Ymax - ctx->DrawBuffer->_Ymin; ctx->Driver.MapRenderbuffer(ctx, accRb, x, y, width, height, GL_MAP_WRITE_BIT, &accMap, &accRowStride); if (!accMap) { _mesa_error(ctx, GL_OUT_OF_MEMORY, "glAccum"); return; } if (accRb->Format == MESA_FORMAT_SIGNED_RGBA_16) { const GLshort clearR = FLOAT_TO_SHORT(ctx->Accum.ClearColor[0]); const GLshort clearG = FLOAT_TO_SHORT(ctx->Accum.ClearColor[1]); const GLshort clearB = FLOAT_TO_SHORT(ctx->Accum.ClearColor[2]); const GLshort clearA = FLOAT_TO_SHORT(ctx->Accum.ClearColor[3]); GLuint i, j; for (j = 0; j < height; j++) { GLshort *row = (GLshort *) accMap; for (i = 0; i < width; i++) { row[i * 4 + 0] = clearR; row[i * 4 + 1] = clearG; row[i * 4 + 2] = clearB; row[i * 4 + 3] = clearA; } accMap += accRowStride; } } else { /* other types someday? */ _mesa_warning(ctx, "unexpected accum buffer type"); } ctx->Driver.UnmapRenderbuffer(ctx, accRb); }
/** For ADD/MULT */ static void accum_mad(GLcontext *ctx, GLfloat scale, GLfloat bias, GLint xpos, GLint ypos, GLint width, GLint height, struct st_renderbuffer *acc_strb) { size_t stride = acc_strb->stride; GLubyte *data = acc_strb->data; switch (acc_strb->format) { case PIPE_FORMAT_R16G16B16A16_SNORM: { int i, j; for (i = 0; i < height; i++) { GLshort *acc = (GLshort *) (data + (ypos + i) * stride + xpos * 8); for (j = 0; j < width * 4; j++) { float val = SHORT_TO_FLOAT(*acc) * scale + bias; *acc++ = FLOAT_TO_SHORT(val); } } } break; default: _mesa_problem(NULL, "unexpected format in st_clear_accum_buffer()"); } }
static int wav_gsm610_write_f (SF_PRIVATE *psf, float *ptr, int len) { GSM610_PRIVATE *pgsm610 ; short *sptr ; int k, bufferlen, writecount = 0, count ; int index = 0, total = 0 ; float normfact ; if (! psf->fdata) return 0 ; pgsm610 = (GSM610_PRIVATE*) psf->fdata ; normfact = (psf->norm_float == SF_TRUE) ? ((float) 0x8000) : 1.0 ; sptr = (short*) psf->buffer ; bufferlen = ((SF_BUFFER_LEN / psf->blockwidth) * psf->blockwidth) / sizeof (short) ; while (len > 0) { writecount = (len >= bufferlen) ? bufferlen : len ; for (k = 0 ; k < writecount ; k++) sptr [k] = FLOAT_TO_SHORT (normfact * ptr [index+k]) ; count = wav_gsm610_write (psf, pgsm610, sptr, writecount) ; index += writecount ; total += count ; len -= writecount ; } ; return total ; } /* wav_gsm610_write_f */
void st_clear_accum_buffer(GLcontext *ctx, struct gl_renderbuffer *rb) { struct st_renderbuffer *acc_strb = st_renderbuffer(rb); const GLint xpos = ctx->DrawBuffer->_Xmin; const GLint ypos = ctx->DrawBuffer->_Ymin; const GLint width = ctx->DrawBuffer->_Xmax - xpos; const GLint height = ctx->DrawBuffer->_Ymax - ypos; size_t stride = acc_strb->stride; GLubyte *data = acc_strb->data; if(!data) return; switch (acc_strb->format) { case PIPE_FORMAT_R16G16B16A16_SNORM: { GLshort r = FLOAT_TO_SHORT(ctx->Accum.ClearColor[0]); GLshort g = FLOAT_TO_SHORT(ctx->Accum.ClearColor[1]); GLshort b = FLOAT_TO_SHORT(ctx->Accum.ClearColor[2]); GLshort a = FLOAT_TO_SHORT(ctx->Accum.ClearColor[3]); int i, j; for (i = 0; i < height; i++) { GLshort *dst = (GLshort *) (data + (ypos + i) * stride + xpos * 8); for (j = 0; j < width; j++) { dst[0] = r; dst[1] = g; dst[2] = b; dst[3] = a; dst += 4; } } } break; default: _mesa_problem(ctx, "unexpected format in st_clear_accum_buffer()"); } }
static void accum_load(struct st_context *st, GLfloat value, GLint xpos, GLint ypos, GLint width, GLint height, struct st_renderbuffer *acc_strb, struct st_renderbuffer *color_strb) { struct pipe_context *pipe = st->pipe; struct pipe_screen *screen = pipe->screen; struct pipe_transfer *color_trans; size_t stride = acc_strb->stride; GLubyte *data = acc_strb->data; GLfloat *buf; if (ST_DEBUG & DEBUG_FALLBACK) debug_printf("%s: fallback processing\n", __FUNCTION__); color_trans = st_cond_flush_get_tex_transfer(st, color_strb->texture, 0, 0, 0, PIPE_TRANSFER_READ, xpos, ypos, width, height); buf = (GLfloat *) _mesa_malloc(width * height * 4 * sizeof(GLfloat)); pipe_get_tile_rgba(color_trans, 0, 0, width, height, buf); switch (acc_strb->format) { case PIPE_FORMAT_R16G16B16A16_SNORM: { const GLfloat *color = buf; int i, j; for (i = 0; i < height; i++) { GLshort *acc = (GLshort *) (data + (ypos + i) * stride + xpos * 8); for (j = 0; j < width * 4; j++) { float val = *color++ * value; *acc++ = FLOAT_TO_SHORT(val); } } } break; default: _mesa_problem(NULL, "unexpected format in st_clear_accum_buffer()"); } _mesa_free(buf); screen->tex_transfer_destroy(color_trans); }
/** * @brief If requested via inv_test_setup_accel(), test the accelerometer * biases and calculate the necessary bias correction. * @param mlsl_handle * serial interface handle to allow serial communication with the * device, both gyro and accelerometer. * @param enable_axis * specify which axis has to be checked and corrected: provides * a switch mode between 3 axis calibration and Z axis only * calibration. * @param bias * output pointer to store the initial bias calculation provided * by the MPU Self Test. Requires 3 elements to store accel X, Y, * and Z axis bias. * @param gravity * The gravity value given the parts' sensitivity: for example * if the accelerometer is set to +/- 2 gee ==> the gravity * value will be 2^14 = 16384. * @param perform_full_test * If 1: * calculates offsets and noise and compare it against set * thresholds. The final exist status will reflect if any of the * value is outside of the expected range. * When 0; * skip the noise calculation and pass/fail assessment; simply * calculates the accel biases. * * @return 0 on success. A non-zero error code on error. */ int test_accel(void *mlsl_handle, int enable_axes, short *bias, long gravity, uint_fast8_t perform_full_test) { short *p_vals; float avg[3] = {0.f, 0.f, 0.f}, zg = 0.f; float rms[3]; float accel_rms_thresh = 1000000.f; /* enourmous to make the test always passes - future deployment */ int accel_error = false; const long sample_period = inv_get_sample_step_size_ms() * 1000; int ii; p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); /* collect the samples */ for(ii = 0; ii < test_setup.accel_samples; ii++) { unsigned result = INV_ERROR_ACCEL_DATA_NOT_READY; int tries = 0; long accel_data[3]; short *vals = &p_vals[3 * ii]; /* ignore data not ready errors but don't try more than 5 times */ while (result == INV_ERROR_ACCEL_DATA_NOT_READY && tries++ < 5) { result = inv_get_accel_data(accel_data); usleep(sample_period); } if (result || tries >= 5) { MPL_LOGV("cannot reliably fetch data from the accelerometer"); accel_error = true; goto accel_early_exit; } vals[X] = (short)accel_data[X]; vals[Y] = (short)accel_data[Y]; vals[Z] = (short)accel_data[Z]; avg[X] += 1.f * vals[X] / test_setup.accel_samples; avg[Y] += 1.f * vals[Y] / test_setup.accel_samples; avg[Z] += 1.f * vals[Z] / test_setup.accel_samples; if (VERBOSE_OUT) MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", vals[X], vals[Y], vals[Z]); } if (((enable_axes << 4) & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL) { MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", avg[X], avg[Y], avg[Z]); if (VERBOSE_OUT) MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", avg[X] / gravity, avg[Y] / gravity, avg[Z] / gravity); bias[X] = FLOAT_TO_SHORT(avg[X]); bias[Y] = FLOAT_TO_SHORT(avg[Y]); zg = avg[Z] - g_z_sign * gravity; bias[Z] = FLOAT_TO_SHORT(zg); MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", bias[X], bias[Y], bias[Z]); if (VERBOSE_OUT) MPL_LOGI("Accel correct.: " "%+13.3f %+13.3f %+13.3f (gee)\n", 1.f * bias[X] / gravity, 1.f * bias[Y] / gravity, 1.f * bias[Z] / gravity); if (perform_full_test) { /* accel RMS - for now the threshold is only indicative */ for (ii = 0, rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f; ii < test_setup.accel_samples; ii++) { short *vals = &p_vals[3 * ii]; rms[X] += (vals[X] - avg[X]) * (vals[X] - avg[X]); rms[Y] += (vals[Y] - avg[Y]) * (vals[Y] - avg[Y]); rms[Z] += (vals[Z] - avg[Z]) * (vals[Z] - avg[Z]); } for (ii = 0; ii < 3; ii++) { if (rms[ii] > accel_rms_thresh * accel_rms_thresh * test_setup.accel_samples) { MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " "(threshold = %.2f)\n", a_name[ii], sqrt(rms[ii] / test_setup.accel_samples), accel_rms_thresh); accel_error = true; goto accel_early_exit; } } MPL_LOGI("Accel RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", sqrt(rms[X] / DEF_N_ACCEL_SAMPLES), sqrt(rms[Y] / DEF_N_ACCEL_SAMPLES), sqrt(rms[Z] / DEF_N_ACCEL_SAMPLES)); } } else { MPL_LOGI("Accel Z bias : %+13.3f (LSB)\n", avg[Z]); if (VERBOSE_OUT) MPL_LOGI("Accel Z bias : %+13.3f (gee)\n", avg[Z] / gravity); zg = avg[Z] - g_z_sign * gravity; bias[Z] = FLOAT_TO_SHORT(zg); MPL_LOGI("Accel Z correct.: %+13d (LSB)\n", bias[Z]); if (VERBOSE_OUT) MPL_LOGI("Accel Z correct.: " "%+13.3f (gee)\n", 1.f * bias[Z] / gravity); } accel_early_exit: if (accel_error) { bias[0] = bias[1] = bias[2] = 0; return (1); /* error */ } inv_free(p_vals); return (0); /* success */ }
/** * @brief Test the gyroscope sensor. * Implements the core logic of the MPU Self Test. * Produces the PASS/FAIL result. Loads the calculated gyro biases * and temperature datum into the corresponding pointers. * @param mlsl_handle * serial interface handle to allow serial communication with the * device, both gyro and accelerometer. * @param gyro_biases * output pointer to store the initial bias calculation provided * by the MPU Self Test. Requires 3 elements for gyro X, Y, * and Z. * @param temp_avg * output pointer to store the initial average temperature as * provided by the MPU Self Test. * @param perform_full_test * If 1: * Complete calibration test: * Calculate offset, drive frequency, and noise and compare it * against set thresholds. * When 0: * Skip the noise and drive frequency calculation, * simply calculate the gyro biases. * * @return 0 on success. * On error, the return value is a bitmask representing: * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively * (decimal values will be 1, 2, 4 respectively). * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively * (decimal values will be 8, 16, 32 respectively). * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively * (decimal values will be 64, 128, 256 respectively). * 9 If any of the RMS noise values is zero, it may be * due to a non-functional gyro or FIFO/register failure. * (decimal value will be 512). */ int test_gyro(void *mlsl_handle, short gyro_biases[3], short *temp_avg, uint_fast8_t perform_full_test) { int ret_val = 0; inv_error_t result; int total_count = 0; int total_count_axis[3] = {0, 0, 0}; int packet_count; short x[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0}; short y[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0}; short z[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0}; int temperature = 0; float avg[3]; float rms[3]; unsigned long test_start = inv_get_tick_count(); int i, j, tmp; char tmpStr[200]; unsigned char regs[7] = {0}; /* make sure the DMP is disabled first */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* reset the gyro offset values */ regs[0] = MPUREG_XG_OFFS_USRH; result = inv_serial_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr, 6, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } /* sample rate */ if (perform_full_test) { /* = 8ms */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_SMPLRT_DIV, 0x07); test_setup.bias_thresh = (int)( DEF_BIAS_THRESH_CAL * test_setup.gyro_sens); } else { /* = 1ms */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_SMPLRT_DIV, 0x00); test_setup.bias_thresh = (int)( DEF_BIAS_THRESH_SELF * test_setup.gyro_sens); } if (result) { LOG_RESULT_LOCATION(result); return result; } regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ switch (test_setup.gyro_fs) { case 2000: regs[0] |= 0x18; break; case 1000: regs[0] |= 0x10; break; case 500: regs[0] |= 0x08; break; case 250: default: regs[0] |= 0x00; break; } result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_CONFIG, regs[0]); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_INT_ENABLE, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* 1st, timing test */ for (j = 0; j < 3; j++) { MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); /* turn on all gyros, use gyro X for clocking Set to Y and Z for 2nd and 3rd iteration */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_PWR_MGMT_1, j + 1); if (result) { LOG_RESULT_LOCATION(result); return result; } /* wait for 2 ms after switching clock source */ usleep(2000); /* enable & reset FIFO */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); if (result) { LOG_RESULT_LOCATION(result); return result; } tmp = test_setup.tests_per_axis; while (tmp-- > 0) { const unsigned char fifo_en_reg = MPUREG_FIFO_EN; /* enable XYZ gyro in FIFO and nothing else */ result = inv_serial_single_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr, fifo_en_reg, BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); if (result) { LOG_RESULT_LOCATION(result); return result; } /* wait one period for data */ if (perform_full_test) usleep(DEF_PERIOD_CAL * 1000); else usleep(DEF_PERIOD_SELF * 1000); /* stop storing gyro in the FIFO */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, fifo_en_reg, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* Getting number of bytes in FIFO */ result = inv_serial_read( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_FIFO_COUNTH, 2, dataout); if (result) { LOG_RESULT_LOCATION(result); return result; } /* number of 6 B packets in the FIFO */ packet_count = inv_big8_to_int16(dataout) / 6; sprintf(tmpStr, "Packet Count: %d - ", packet_count); if (abs(packet_count - test_setup.packet_thresh) <= /* Within total_timing_tol % range, rounded up */ (int)(test_setup.total_timing_tol * test_setup.packet_thresh + 1)) { for (i = 0; i < packet_count; i++) { /* getting FIFO data */ result = inv_serial_read_fifo(mlsl_handle, mldl_cfg->mpu_chip_info->addr, 6, dataout); if (result) { LOG_RESULT_LOCATION(result); return result; } x[total_count + i] = inv_big8_to_int16(&dataout[0]); y[total_count + i] = inv_big8_to_int16(&dataout[2]); z[total_count + i] = inv_big8_to_int16(&dataout[4]); if (VERBOSE_OUT) { MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", total_count + i, x[total_count + i], y[total_count + i], z[total_count + i]); } } total_count += packet_count; total_count_axis[j] += packet_count; sprintf(tmpStr, "%sOK", tmpStr); } else { ret_val |= 1 << j; sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); } MPL_LOGI("%s\n", tmpStr); } /* remove gyros from FIFO */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_FIFO_EN, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* Read Temperature */ result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_TEMP_OUT_H, 2, dataout); if (result) { LOG_RESULT_LOCATION(result); return result; } temperature += (short)inv_big8_to_int16(dataout); } MPL_LOGI("\n"); MPL_LOGI("Total %d samples\n", total_count); MPL_LOGI("\n"); /* 2nd, check bias from X, Y, and Z PLL clock source */ tmp = total_count != 0 ? total_count : 1; for (i = 0, avg[X] = .0f, avg[Y] = .0f, avg[Z] = .0f; i < total_count; i++) { avg[X] += 1.f * x[i] / tmp; avg[Y] += 1.f * y[i] / tmp; avg[Z] += 1.f * z[i] / tmp; } MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", avg[X], avg[Y], avg[Z]); if (VERBOSE_OUT) { MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", avg[X] / adj_gyro_sens, avg[Y] / adj_gyro_sens, avg[Z] / adj_gyro_sens); } for (j = 0; j < 3; j++) { if (fabs(avg[j]) > test_setup.bias_thresh) { MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " "(threshold = %d)\n", a_name[j], avg[j], test_setup.bias_thresh); ret_val |= 1 << (3+j); } } /* 3rd, check RMS for dead gyros If any of the RMS noise value returns zero, then we might have dead gyro or FIFO/register failure, the part is sleeping, or the part is not responsive */ for (i = 0, rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f; i < total_count; i++) { rms[X] += (x[i] - avg[X]) * (x[i] - avg[X]); rms[Y] += (y[i] - avg[Y]) * (y[i] - avg[Y]); rms[Z] += (z[i] - avg[Z]) * (z[i] - avg[Z]); } if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0) { ret_val |= 1 << 9; } /* 4th, temperature average */ temperature /= 3; if (VERBOSE_OUT) MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", (float)inv_decode_temperature(temperature) / (1L << 16), "", ""); /* load into final storage */ *temp_avg = (short)temperature; gyro_biases[X] = FLOAT_TO_SHORT(avg[X]); gyro_biases[Y] = FLOAT_TO_SHORT(avg[Y]); gyro_biases[Z] = FLOAT_TO_SHORT(avg[Z]); MPL_LOGI("\n"); MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - test_start); return ret_val; }