static int yas_bma250_measure(int *out_data, int *out_raw) { struct yas_bma250_acceleration accel; unsigned char buf[6]; int32_t raw[3], data[3]; int pos = acc_data.position; int i, j; #if 0 int32_t sec; int32_t msec; #endif /* Check initialize */ if (acc_data.initialize == 0) return YAS_ERROR_NOT_INITIALIZED; /* Read acceleration data */ if (yas_bma250_read_reg(YAS_BMA250_ACC_REG, buf, 6) != 0) { for (i = 0; i < 3; i++) raw[i] = 0; } else { if (acc_data.chip_id == YAS_BMA250_CHIP_ID_254) for (i = 0; i < 3; i++) raw[i] = ((int16_t)((buf[i*2+1] << 8)) | (buf[i*2] & 0xfe)) >> 4; else for (i = 0; i < 3; i++) raw[i] = ((int16_t)((buf[i*2+1] << 8)) | (buf[i*2] & 0xfe)) >> 6; }
static int yas_bma250_read_reg_byte(unsigned char adr) { unsigned char buf = 0xff; int err; err = yas_bma250_read_reg(adr, &buf, 1); if (err == 0) return buf; return 0; }
static int yas_bma250_measure(int *out_data, int *out_raw) { struct yas_bma250_acceleration accel; unsigned char buf[6]; int32_t raw[3], data[3]; int pos = acc_data.position; int i,j; /* Check initialize */ if (acc_data.initialize == 0) { return YAS_ERROR_NOT_INITIALIZED; } /* Read acceleration data */ if (yas_bma250_read_reg(YAS_BMA250_ACC_REG, buf, 6) != 0) { for (i = 0; i < 3; i++) raw[i] = 0; } else { for (i = 0; i < 3; i++) raw[i] = ((int16_t)((buf[i*2+1] << 8)) | (buf[i*2] & 0xfe)) >> 6; } /* for X, Y, Z axis */ for (i = 0; i < 3; i++) { /* coordinate transformation */ data[i] = 0; for (j = 0; j < 3; j++) { data[i] += raw[j] * yas_bma250_position_map[pos][i][j]; } /* normalization */ data[i] *= (YAS_BMA250_GRAVITY_EARTH / YAS_BMA250_RESOLUTION); } yas_bma250_data_filter(data, raw, &accel); out_data[0] = accel.x - acc_data.offset.v[0]; out_data[1] = accel.y - acc_data.offset.v[1]; out_data[2] = accel.z - acc_data.offset.v[2]; out_raw[0] = accel.x_raw; out_raw[1] = accel.y_raw; out_raw[2] = accel.z_raw; acc_data.last = accel; return YAS_NO_ERROR; }
static int yas_bma250_measure(int *out_data, int *out_raw) { struct yas_bma250_acceleration accel; unsigned char buf[6]; int32_t raw[3], data[3]; int pos = acc_data.position; int i; int j; /* Transformation matrix for chip mounting position */ static const int yas_bma250_position_map[][3][3] = { {{ 0, -1, 0}, { 1, 0, 0}, { 0, 0, 1} }, {{ 1, 0, 0}, { 0, 1, 0}, { 0, 0, 1} }, {{ 0, 1, 0}, {-1, 0, 0}, { 0, 0, 1} }, {{-1, 0, 0}, { 0, -1, 0}, { 0, 0, 1} }, {{ 0, 1, 0}, { 1, 0, 0}, { 0, 0, -1} }, {{-1, 0, 0}, { 0, 1, 0}, { 0, 0, -1} }, {{ 0, -1, 0}, {-1, 0, 0}, { 0, 0, -1} }, {{ 1, 0, 0}, { 0, -1, 0}, { 0, 0, -1} }, }; /* Check initialize */ if (acc_data.initialize == 0) return YAS_ERROR_NOT_INITIALIZED; /* Read acceleration data */ if (yas_bma250_read_reg(YAS_BMA250_ACC_REG, buf, 6) != 0) for (i = 0; i < 3; i++) raw[i] = 0; else { if (acc_data.chip_id == YAS_BMA250_CHIP_ID) for (i = 0; i < 3; i++) raw[i] = ((int16_t)((buf[i*2+1] << 8)) | (buf[i*2] & 0xfe)) >> 6; else for (i = 0; i < 3; i++) raw[i] = ((int16_t)((buf[i*2+1] << 8)) | (buf[i*2] & 0xfe)) >> 4; }