Piteball
/
android_kernel_samsung_msm8976
mirror of https://github.com/team-infusion-developers/android_kernel_samsung_msm8976.git
1
1
Fork 0
Code Issues Projects Releases Wiki Activity
android_kernel_samsung_msm8976/drivers/sensors/yas_mag_drv-yas539.c

828 lines
22 KiB
C
Raw Permalink Blame History

/*
* Copyright (c) 2015 Yamaha Corporation
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "yas.h"
#if YAS_MAG_DRIVER == YAS_MAG_DRIVER_YAS539
#define YAS539_REG_DIDR (0x80)
#define YAS539_REG_CMDR (0x81)
#define YAS539_REG_RCOILR (0x82)
#define YAS539_REG_INTRVLR (0x83)
#define YAS539_REG_RCAUTOR (0x85)
#define YAS539_REG_AVRR (0x87)
#define YAS539_REG_SRSTR (0x90)
#define YAS539_REG_ADCCALR (0x91)
#define YAS539_REG_TRMR (0x9f)
#define YAS539_REG_RCDRVR (0xa0)
#define YAS539_REG_RCCURR (0xaa)
#define YAS539_REG_DATAR (0xb0)
#define YAS539_REG_CALR (0xc0)
#define YAS539_DATA_UNDERFLOW (0)
#define YAS539_DATA_OVERFLOW (32767)
#define YAS539_DEVICE_ID (0x08) /* YAS539 (MS-3S) */
#define YAS_X_OVERFLOW (0x01)
#define YAS_X_UNDERFLOW (0x02)
#define YAS_Y1_OVERFLOW (0x04)
#define YAS_Y1_UNDERFLOW (0x08)
#define YAS_Y2_OVERFLOW (0x10)
#define YAS_Y2_UNDERFLOW (0x20)
#define YAS_OVERFLOW (YAS_X_OVERFLOW|YAS_Y1_OVERFLOW|YAS_Y2_OVERFLOW)
#define YAS_UNDERFLOW (YAS_X_UNDERFLOW|YAS_Y1_UNDERFLOW|YAS_Y2_UNDERFLOW)
#define YAS539_MAG_STATE_NORMAL (0)
#define YAS539_MAG_STATE_INIT_COIL (1)
#define YAS539_MAG_INITCOIL_TIMEOUT (1000) /* msec */
#define YAS539_MAG_POWER_ON_RESET_TIME (4000) /* usec */
#define YAS539_MAG_SOFT_RESET_TIME (150) /* usec */
#define YAS539_MAG_NOTRANS_POSITION (3)
#define YAS539_MAG_AVERAGE_16 (0)
#define YAS539_MAG_AVERAGE_64 (1)
#define YAS539_MAG_AVERAGE_128 (2)
#define YAS539_MAG_AVERAGE_256 (3)
#define YAS539_MAG_RCOIL_TIME0 (65)
#define YAS539_MAG_RCOIL_TIME3 (550)
#define YAS539_MAG_RCOIL_RETRY (2)
#define YAS539_MAG_TEST_TIME (5000) /* usec */
#define set_vector(to, from) \
{int _l; for (_l = 0; _l < 3; _l++) (to)[_l] = (from)[_l]; }
struct yas_cdriver {
int initialized;
struct yas_driver_callback cbk;
int measure_state;
int invalid_data;
uint32_t invalid_data_time;
int position;
int32_t delay;
int enable;
uint8_t dev_id;
const int8_t *transform;
int average;
uint32_t current_time;
uint16_t last_raw[4];
uint16_t last_after_rcoil[3];
int16_t overflow[3], underflow[3];
struct yas_matrix static_matrix;
int noise_rcoil_flag;
int flowflag;
};
static const struct yas_matrix no_conversion
= { {10000, 0, 0, 0, 10000, 0, 0, 0, 10000} };
static const int measure_time_worst[] = {682, 1210, 2387, 4752};
static const int8_t YAS539_TRANSFORMATION[][9] = {
{ 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 },
};
static struct yas_cdriver driver;
static int yas_set_enable_wrap(int enable, int rcoil);
static int yas_set_enable(int enable);
static int single_read(int *busy, uint16_t *t, uint16_t *xy1y2, int *ouflow);
static int yas_open(void)
{
if (driver.cbk.device_open(YAS_TYPE_MAG) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
driver.cbk.usleep(YAS539_MAG_POWER_ON_RESET_TIME);
return YAS_NO_ERROR;
}
#define yas_read(a, b, c) \
(driver.cbk.device_read(YAS_TYPE_MAG, (a), (b), (c)))
static int yas_single_write(uint8_t addr, uint8_t data)
{
return driver.cbk.device_write(YAS_TYPE_MAG, addr, &data, 1);
}
static void apply_matrix(struct yas_vector *xyz, struct yas_matrix *m)
{
int32_t tmp[3];
int i;
if (m == NULL)
return;
for (i = 0; i < 3; i++)
tmp[i] = ((m->m[i*3]/10) * (xyz->v[0]/10)
+ (m->m[i*3+1]/10) * (xyz->v[1]/10)
+ (m->m[i*3+2]/10) * (xyz->v[2]/10)) / 100;
for (i = 0; i < 3; i++)
xyz->v[i] = tmp[i];
}
static uint32_t curtime(void)
{
if (driver.cbk.current_time)
return driver.cbk.current_time();
else
return driver.current_time;
}
static int invalid_magnetic_field(uint16_t *cur, uint16_t *last)
{
int16_t invalid_thresh[] = {2000, 2000, 2000};
int i;
for (i = 0; i < 3; i++)
if (invalid_thresh[i] <= ABS(cur[i] - last[i]))
return 1;
return 0;
}
static int start_yas539(int tcsen, int cont)
{
uint8_t data = 0x81;
data = (uint8_t)(data | (cont<<5));
data = (uint8_t)(data | (tcsen<<6));
if (yas_single_write(YAS539_REG_CMDR, data) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
}
static int cont_start_yas539(void)
{
if (start_yas539(0, 1) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
/* wait for the first measurement */
driver.cbk.usleep(measure_time_worst[driver.average]);
return YAS_NO_ERROR;
}
static void sensitivity_correction(uint16_t *xy1y2, uint16_t t)
{
int32_t h[3];
int i;
for (i = 0; i < 3; i++) {
h[i] = (int32_t) xy1y2[i] - 16384;
h[i] = 100000 * h[i] / (100000 - 11 * (t - 8170));
xy1y2[i] = CLIP(h[i], -16384, 16383) + 16384;
}
}
static int read_yas539(int *busy, uint16_t *t, uint16_t *xy1y2, int *ouflow)
{
uint8_t data[8];
int i;
if (yas_read(YAS539_REG_DATAR, data, 8) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
*t = (data[0]<<8) | data[1];
*busy = data[2]>>7;
xy1y2[0] = (uint16_t)(((data[2]&0x7f)<<8) | data[3]);
xy1y2[1] = (uint16_t)(((data[4]&0x7f)<<8) | data[5]);
xy1y2[2] = (uint16_t)(((data[6]&0x7f)<<8) | data[7]);
*ouflow = 0;
for (i = 0; i < 3; i++) {
if (driver.overflow[i] <= xy1y2[i])
*ouflow |= (1<<(i*2));
if (xy1y2[i] <= driver.underflow[i])
*ouflow |= (1<<(i*2+1));
driver.last_raw[i] = xy1y2[i];
}
driver.last_raw[i] = *t;
return YAS_NO_ERROR;
}
static int update_intrvlr(int32_t delay)
{
int ds = (delay * 1000 - measure_time_worst[driver.average]) / 1000;
int l, h;
if (ds < 1)
ds = 1;
for (l = 0; l < 7; l++) {
if (16 * (1<<l) < ds)
continue;
else
break;
}
for (h = 15; 0 < h; h--) {
if (ds < (h+1) * (1<<l))
continue;
else
break;
}
if (yas_single_write(YAS539_REG_INTRVLR, (h<<3) | l) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
}
static int update_avrr(int average)
{
static const uint8_t avrr[] = {0x05, 0x2d, 0x2e, 0x2f};
if (yas_single_write(YAS539_REG_AVRR, avrr[average]) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
}
static int rcoil(int mode, int rnum)
{
if (yas_single_write(YAS539_REG_RCDRVR, mode == 0 ? 0x00 : 0x01) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (yas_single_write(YAS539_REG_RCOILR, 0x0a | ((rnum&0x3)<<6)) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (rnum == 0)
driver.cbk.usleep(YAS539_MAG_RCOIL_TIME0);
else
driver.cbk.usleep(YAS539_MAG_RCOIL_TIME3);
return YAS_NO_ERROR;
}
static int rcoil_and_record_data(int flowflag, uint16_t *xy1y2, int *ouflow)
{
uint16_t t;
int rt, busy;
if (flowflag) {
if (rcoil(0, 0) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (rcoil(1, 3) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
} else {
if (rcoil(1, 0) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
}
rt = single_read(&busy, &t, xy1y2, ouflow);
if (rt < 0)
return rt;
if (busy)
return YAS_ERROR_BUSY;
sensitivity_correction(xy1y2, t);
set_vector(driver.last_after_rcoil, xy1y2);
return YAS_NO_ERROR;
}
static int enable_rcoil(void)
{
uint16_t xy1y2[3], xy1y2_after[3], t;
int rt, busy, overflow;
rt = single_read(&busy, &t, xy1y2, &overflow);
if (rt < 0)
return rt;
if (busy)
return YAS_ERROR_BUSY;
sensitivity_correction(xy1y2, t);
if (!overflow) {
rt = rcoil_and_record_data(0, xy1y2_after, &overflow);
if (rt < 0)
return rt;
if (!overflow && !invalid_magnetic_field(xy1y2, xy1y2_after))
return 1;
}
rt = rcoil_and_record_data(1, xy1y2, &overflow);
if (rt < 0)
return rt;
if (overflow)
return 0;
return 1;
}
static int reset_yas539(void)
{
if (yas_single_write(YAS539_REG_SRSTR, 0x80) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
driver.cbk.usleep(YAS539_MAG_SOFT_RESET_TIME);
if (yas_single_write(YAS539_REG_ADCCALR, 0x03) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (yas_single_write(YAS539_REG_ADCCALR+1, 0xe0) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (yas_single_write(YAS539_REG_TRMR, 0xff) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (yas_single_write(YAS539_REG_RCAUTOR, 0x8a) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (yas_single_write(YAS539_REG_RCCURR, 0x03) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (update_intrvlr(driver.delay) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (update_avrr(driver.average) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
}
static int single_read(int *busy, uint16_t *t, uint16_t *xy1y2, int *ouflow)
{
if (start_yas539(0, 0) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
driver.cbk.usleep(measure_time_worst[driver.average]);
if (read_yas539(busy, t, xy1y2, ouflow) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
}
static void xy1y2_to_xyz(uint16_t *xy1y2, int32_t *xyz)
{
xyz[0] = ((int32_t)xy1y2[0] - 16384) * 150;
xyz[1] = ((int32_t)xy1y2[1] - xy1y2[2]) * 866 / 10;
xyz[2] = ((int32_t)-xy1y2[1] - xy1y2[2] + 32768) * 150;
}
static int sensitivity_measuremnet(uint16_t *xy1y2)
{
uint16_t t;
int rt, busy, ouflow;
rt = start_yas539(1, 0);
if (rt < 0)
return rt;
driver.cbk.usleep(YAS539_MAG_TEST_TIME);
rt = read_yas539(&busy, &t, xy1y2, &ouflow);
if (rt < 0)
return rt;
if (busy)
return YAS_ERROR_BUSY;
return YAS_NO_ERROR;
}
static int yas_get_position(void)
{
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
return driver.position;
}
static int yas_set_position(int position)
{
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
if (position < 0 || 7 < position)
return YAS_ERROR_ARG;
if (position == YAS539_MAG_NOTRANS_POSITION)
driver.transform = NULL;
else
driver.transform = YAS539_TRANSFORMATION[position];
driver.position = position;
return YAS_NO_ERROR;
}
static int yas_measure(struct yas_data *data, int num, int *ouflow)
{
int32_t xyz_tmp[3];
int i, busy, rt;
uint16_t xy1y2[3], t;
uint32_t tm;
*ouflow = 0;
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
if (data == NULL || num < 0)
return YAS_ERROR_ARG;
if (driver.cbk.current_time == NULL)
driver.current_time += (uint32_t)driver.delay;
if (num == 0)
return 0;
if (!driver.enable)
return 0;
if (driver.measure_state == YAS539_MAG_STATE_INIT_COIL) {
tm = curtime();
if (YAS539_MAG_INITCOIL_TIMEOUT
<= tm - driver.invalid_data_time) {
driver.invalid_data_time = tm;
/* stop the continuous measurement */
if (yas_single_write(YAS539_REG_CMDR, 0x00) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
rt = rcoil_and_record_data(driver.flowflag, xy1y2,
ouflow);
if (rt < 0)
return rt;
if (!*ouflow) {
driver.flowflag = 0;
driver.invalid_data = 0;
driver.measure_state = YAS539_MAG_STATE_NORMAL;
}
if (cont_start_yas539() < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
}
}
if (read_yas539(&busy, &t, xy1y2, ouflow) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
sensitivity_correction(xy1y2, t);
xy1y2_to_xyz(xy1y2, data->xyz.v);
if (driver.transform != NULL) {
for (i = 0; i < 3; i++) {
xyz_tmp[i] = driver.transform[i*3] * data->xyz.v[0]
+ driver.transform[i*3+1] * data->xyz.v[1]
+ driver.transform[i*3+2] * data->xyz.v[2];
}
set_vector(data->xyz.v, xyz_tmp);
}
apply_matrix(&data->xyz, &driver.static_matrix);
for (i = 0; i < 3; i++) {
data->xyz.v[i] -= data->xyz.v[i] % 10;
if (*ouflow & (1<<(i*2)))
data->xyz.v[i] += 1; /* set overflow */
if (*ouflow & (1<<(i*2+1)))
data->xyz.v[i] += 2; /* set underflow */
}
tm = curtime();
data->type = YAS_TYPE_MAG;
if (driver.cbk.current_time)
data->timestamp = tm;
else
data->timestamp = 0;
data->accuracy = 0;
if (busy)
return YAS_ERROR_BUSY;
if (*ouflow || invalid_magnetic_field(xy1y2, driver.last_after_rcoil)) {
if (!driver.invalid_data) {
driver.invalid_data_time = tm;
driver.invalid_data = 1;
}
if (*ouflow)
driver.flowflag = 1;
driver.measure_state = YAS539_MAG_STATE_INIT_COIL;
for (i = 0; i < 3; i++) {
if (!*ouflow)
data->xyz.v[i] += 3;
}
}
return 1;
}
static int yas_measure_wrap(struct yas_data *data, int num)
{
int ouflow;
return yas_measure(data, num, &ouflow);
}
static int yas_get_delay(void)
{
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
return driver.delay;
}
static int yas_set_delay(int delay)
{
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
if (delay < 0)
return YAS_ERROR_ARG;
driver.delay = delay;
if (!driver.enable)
return YAS_NO_ERROR;
/* stop the continuous measurement */
if (yas_single_write(YAS539_REG_CMDR, 0x00) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (update_intrvlr(driver.delay) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (cont_start_yas539() < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
}
static int yas_get_enable(void)
{
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
return driver.enable;
}
static int yas_set_enable_wrap(int enable, int rcoil)
{
int rt;
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
enable = !!enable;
if (driver.enable == enable)
return YAS_NO_ERROR;
if (enable) {
if (yas_open() < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
rt = reset_yas539();
if (rt < 0) {
driver.cbk.device_close(YAS_TYPE_MAG);
return rt;
}
if (rcoil) {
rt = enable_rcoil();
if (rt < 0) {
driver.cbk.device_close(YAS_TYPE_MAG);
return rt;
}
if (rt) {
driver.flowflag = 0;
driver.invalid_data = 0;
driver.measure_state = YAS539_MAG_STATE_NORMAL;
} else {
driver.invalid_data_time = curtime();
driver.flowflag = 1;
driver.invalid_data = 1;
driver.measure_state
= YAS539_MAG_STATE_INIT_COIL;
}
}
if (cont_start_yas539() < 0) {
driver.cbk.device_close(YAS_TYPE_MAG);
return YAS_ERROR_DEVICE_COMMUNICATION;
}
} else {
yas_single_write(YAS539_REG_CMDR, 0x00);
driver.cbk.device_close(YAS_TYPE_MAG);
}
driver.enable = enable;
return YAS_NO_ERROR;
}
static int yas_set_enable(int enable)
{
return yas_set_enable_wrap(enable, 1);
}
static int yas_ext(int32_t cmd, void *p)
{
struct yas539_self_test_result *r;
struct yas_data data;
int32_t *xyz;
int16_t *ouflow, *m;
int8_t average;
int rt, i, enable, overflow, busy, position;
uint16_t xy1y2[3], t;
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
if (p == NULL)
return YAS_ERROR_ARG;
switch (cmd) {
case YAS539_SELF_TEST:
r = (struct yas539_self_test_result *) p;
r->id = driver.dev_id;
enable = driver.enable;
if (!enable) {
if (yas_open() < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
}
rt = reset_yas539();
if (rt < 0)
goto self_test_exit;
rt = enable_rcoil();
if (rt < 0)
goto self_test_exit;
rt = single_read(&busy, &t, xy1y2, &overflow);
if (rt < 0)
goto self_test_exit;
if (busy) {
rt = YAS_ERROR_BUSY;
goto self_test_exit;
}
sensitivity_correction(xy1y2, t);
xy1y2_to_xyz(xy1y2, r->xyz);
for (i = 0; i < 3; i++)
r->xyz[i] = r->xyz[i] / 1000;
if (overflow & YAS_OVERFLOW) {
rt = YAS_ERROR_OVERFLOW;
goto self_test_exit;
}
if (overflow & YAS_UNDERFLOW) {
rt = YAS_ERROR_UNDERFLOW;
goto self_test_exit;
}
if (r->xyz[0] == 0 && r->xyz[1] == 0 && r->xyz[2] == 0) {
rt = YAS_ERROR_DIRCALC;
goto self_test_exit;
}
r->dir = 99;
rt = sensitivity_measuremnet(r->sxy1y2);
if (rt < 0)
goto self_test_exit;
rt = YAS_NO_ERROR;
self_test_exit:
if (enable)
cont_start_yas539();
else
driver.cbk.device_close(YAS_TYPE_MAG);
return rt;
case YAS539_SELF_TEST_NOISE:
xyz = (int32_t *) p;
enable = driver.enable;
if (!enable) {
if (driver.noise_rcoil_flag)
rt = yas_set_enable_wrap(1, 1);
else
rt = yas_set_enable_wrap(1, 0);
if (rt < 0)
return rt;
driver.noise_rcoil_flag = 0;
}
position = yas_get_position();
yas_set_position(YAS539_MAG_NOTRANS_POSITION);
rt = yas_measure(&data, 1, &overflow);
yas_set_position(position);
if (rt < 0) {
if (!enable)
yas_set_enable(0);
return rt;
}
for (i = 0; i < 3; i++)
xyz[i] = data.xyz.v[i] / 150;
if (!enable)
yas_set_enable(0);
return YAS_NO_ERROR;
case YAS539_GET_LAST_RAWDATA:
for (i = 0; i < 4; i++)
((uint16_t *) p)[i] = driver.last_raw[i];
return YAS_NO_ERROR;
case YAS539_GET_AVERAGE_SAMPLE:
*(int8_t *) p = driver.average;
return YAS_NO_ERROR;
case YAS539_SET_AVERAGE_SAMPLE:
average = *(int8_t *) p;
if (average < YAS539_MAG_AVERAGE_16
|| YAS539_MAG_AVERAGE_256 < average)
return YAS_ERROR_ARG;
driver.average = average;
if (!driver.enable)
return YAS_NO_ERROR;
/* stop the continuous measurement */
if (yas_single_write(YAS539_REG_CMDR, 0x00) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (update_avrr(driver.average) < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (cont_start_yas539() < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
return YAS_NO_ERROR;
case YAS539_GET_STATIC_MATRIX:
m = (int16_t *) p;
for (i = 0; i < 9; i++)
m[i] = driver.static_matrix.m[i];
return YAS_NO_ERROR;
case YAS539_SET_STATIC_MATRIX:
m = (int16_t *) p;
for (i = 0; i < 9; i++)
driver.static_matrix.m[i] = m[i];
return YAS_NO_ERROR;
case YAS539_GET_OUFLOW_THRESH:
ouflow = (int16_t *) p;
for (i = 0; i < 3; i++) {
ouflow[i] = driver.overflow[i];
ouflow[i+3] = driver.underflow[i];
}
return YAS_NO_ERROR;
default:
break;
}
return YAS_ERROR_ARG;
}
static int yas_init(void)
{
static const int16_t h[] = {16000, 16256, 16512, 16768};
int i, cal_valid = 0;
uint8_t data[18];
int32_t c[3], a[9], f[3];
int32_t of[3], uf[3], eof[3], euf[3];
uint8_t k;
if (driver.initialized)
return YAS_ERROR_INITIALIZE;
if (yas_open() < 0)
return YAS_ERROR_DEVICE_COMMUNICATION;
if (yas_read(YAS539_REG_DIDR, data, 1) < 0) {
driver.cbk.device_close(YAS_TYPE_MAG);
return YAS_ERROR_DEVICE_COMMUNICATION;
}
driver.dev_id = data[0] & 0x7f;
if (driver.dev_id != YAS539_DEVICE_ID) {
driver.cbk.device_close(YAS_TYPE_MAG);
return YAS_ERROR_CHIP_ID;
}
if (yas_single_write(YAS539_REG_SRSTR, 0x80) < 0) {
driver.cbk.device_close(YAS_TYPE_MAG);
return YAS_ERROR_DEVICE_COMMUNICATION;
}
driver.cbk.usleep(YAS539_MAG_SOFT_RESET_TIME);
if (yas_read(YAS539_REG_CALR, data, 18) < 0) {
driver.cbk.device_close(YAS_TYPE_MAG);
return YAS_ERROR_DEVICE_COMMUNICATION;
}
for (i = 0; i < 13; i++)
if (data[i] != 0)
cal_valid = 1;
if (!cal_valid) {
driver.cbk.device_close(YAS_TYPE_MAG);
return YAS_ERROR_CALREG;
}
driver.cbk.device_close(YAS_TYPE_MAG);
c[0] = ((data[0]<<1) | (data[1]>>7)) - 256;
c[1] = (((data[1]<<2)&0x1fc) | (data[2]>>6)) - 256;
c[2] = (((data[2]<<3)&0x1f8) | (data[3]>>5)) - 256;
a[0] = 128;
a[1] = (((data[3]<<2)&0x7c) | (data[4]>>6)) - 64;
a[2] = (((data[4]<<1)&0x7e) | (data[5]>>7)) - 64;
a[3] = (((data[5]<<1)&0xfe) | (data[6]>>7)) - 128;
a[4] = (((data[6]<<2)&0x1fc) | (data[7]>>6)) - 112;
a[5] = (((data[7]<<1)&0x7e) | (data[8]>>7)) - 64;
a[6] = (((data[8]<<1)&0xfe) | (data[9]>>7)) - 128;
a[7] = (data[9]&0x7f) - 64;
a[8] = (((data[10]<<1)&0x1fe) | (data[11]>>7)) - 112;
k = data[11]&0x7f;
f[0] = (data[12]>>4)&0x03;
f[1] = (data[12]>>2)&0x03;
f[2] = data[12]&0x03;
for (i = 0; i < 3; i++) {
eof[i] = 32768 - h[f[i]] - ABS(c[i]) * 225 / 8;
euf[i] = h[f[i]] - ABS(c[i]) * 225 / 8;
}
of[0] = 16384 + k * (a[0] * eof[0] - ABS(a[1]) * eof[1]
- ABS(a[2]) * eof[2]) / 8192;
of[1] = 16384 + k * (-ABS(a[3]) * eof[0] + a[4] * eof[1]
- ABS(a[5]) * eof[2]) / 8192;
of[2] = 16384 + k * (-ABS(a[6]) * eof[0] - ABS(a[7]) * eof[1]
+ a[8] * eof[2]) / 8192;
uf[0] = 16384 - k * (a[0] * euf[0] - ABS(a[1]) * euf[1]
- ABS(a[2]) * euf[2]) / 8192;
uf[1] = 16384 - k * (-ABS(a[3]) * euf[0] + a[4] * euf[1]
- ABS(a[5]) * euf[2]) / 8192;
uf[2] = 16384 - k * (-ABS(a[6]) * euf[0] - ABS(a[7]) * euf[1]
+ a[8] * euf[2]) / 8192;
for (i = 0; i < 3; i++) {
if (YAS539_DATA_OVERFLOW < of[i])
driver.overflow[i] = YAS539_DATA_OVERFLOW;
else
driver.overflow[i] = (int16_t) of[i];
if (uf[i] < YAS539_DATA_UNDERFLOW)
driver.underflow[i] = YAS539_DATA_UNDERFLOW;
else
driver.underflow[i] = (int16_t) uf[i];
}
driver.measure_state = YAS539_MAG_STATE_NORMAL;
if (driver.cbk.current_time)
driver.current_time = driver.cbk.current_time();
else
driver.current_time = 0;
driver.invalid_data = 0;
driver.invalid_data_time = driver.current_time;
driver.position = YAS539_MAG_NOTRANS_POSITION;
driver.delay = YAS_DEFAULT_SENSOR_DELAY;
driver.enable = 0;
driver.transform = NULL;
driver.average = YAS539_MAG_AVERAGE_64;
driver.noise_rcoil_flag = 1;
driver.flowflag = 0;
for (i = 0; i < 3; i++)
driver.last_after_rcoil[i] = 0;
for (i = 0; i < 4; i++)
driver.last_raw[i] = 0;
driver.static_matrix = no_conversion;
driver.initialized = 1;
return YAS_NO_ERROR;
}
static int yas_term(void)
{
int rt;
if (!driver.initialized)
return YAS_ERROR_INITIALIZE;
rt = yas_set_enable(0);
driver.initialized = 0;
return rt;
}
int yas_mag_driver_init(struct yas_mag_driver *f)
{
if (f == NULL || f->callback.device_open == NULL
|| f->callback.device_close == NULL
|| f->callback.device_read == NULL
|| f->callback.device_write == NULL
|| f->callback.usleep == NULL)
return YAS_ERROR_ARG;
f->init = yas_init;
f->term = yas_term;
f->get_delay = yas_get_delay;
f->set_delay = yas_set_delay;
f->get_enable = yas_get_enable;
f->set_enable = yas_set_enable;
f->get_position = yas_get_position;
f->set_position = yas_set_position;
f->measure = yas_measure_wrap;
f->ext = yas_ext;
driver.cbk = f->callback;
yas_term();
return YAS_NO_ERROR;
}
#endif
Reference in New Issue View Git Blame Copy Permalink