/* * Copyright (c) 2010-2011 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 struct utimeval { int32_t tv_sec; int32_t tv_msec; }; struct utimer { struct utimeval prev_time; struct utimeval total_time; struct utimeval delay_ms; }; static int utimeval_init(struct utimeval *val); static int utimeval_is_initial(struct utimeval *val); static int utimeval_is_overflow(struct utimeval *val); static struct utimeval utimeval_plus(struct utimeval *first, struct utimeval *second); static struct utimeval utimeval_minus(struct utimeval *first, struct utimeval *second); static int utimeval_greater_than(struct utimeval *first, struct utimeval *second); static int utimeval_greater_or_equal(struct utimeval *first, struct utimeval *second); static int utimeval_greater_than_zero(struct utimeval *val); static int utimeval_less_than_zero(struct utimeval *val); static struct utimeval *msec_to_utimeval(struct utimeval *result, uint32_t msec); static uint32_t utimeval_to_msec(struct utimeval *val); static struct utimeval utimer_calc_next_time(struct utimer *ut, struct utimeval *cur); static struct utimeval utimer_current_time(void); static int utimer_is_timeout(struct utimer *ut); static int utimer_clear_timeout(struct utimer *ut); static uint32_t utimer_get_total_time(struct utimer *ut); static uint32_t utimer_get_delay(struct utimer *ut); static int utimer_set_delay(struct utimer *ut, uint32_t delay_ms); static int utimer_update(struct utimer *ut); static int utimer_update_with_curtime(struct utimer *ut, struct utimeval *cur); static uint32_t utimer_sleep_time(struct utimer *ut); static uint32_t utimer_sleep_time_with_curtime(struct utimer *ut, struct utimeval *cur); static int utimer_init(struct utimer *ut, uint32_t delay_ms); static int utimer_clear(struct utimer *ut); static void utimer_lib_init(void (*func) (int *sec, int *msec)); #define YAS_REGADDR_DEVICE_ID (0x80) #define YAS_REGADDR_ACTUATE_INIT_COIL (0x81) #define YAS_REGADDR_MEASURE_COMMAND (0x82) #define YAS_REGADDR_CONFIG (0x83) #define YAS_REGADDR_MEASURE_INTERVAL (0x84) #define YAS_REGADDR_OFFSET_X (0x85) #define YAS_REGADDR_OFFSET_Y1 (0x86) #define YAS_REGADDR_OFFSET_Y2 (0x87) #define YAS_REGADDR_TEST1 (0x88) #define YAS_REGADDR_TEST2 (0x89) #define YAS_REGADDR_CAL (0x90) #define YAS_REGADDR_MEASURE_DATA (0xb0) #define YAS_YAS530_DEVICE_ID (0x01) /* YAS530 (MS-3E) */ #define YAS_YAS530_VERSION_A (0) /* YAS530 (MS-3E Aver) */ #define YAS_YAS530_VERSION_B (1) /* YAS530B (MS-3E Bver) */ #define YAS_YAS530_VERSION_A_COEF (380) #define YAS_YAS530_VERSION_B_COEF (550) #define YAS_YAS530_DATA_CENTER (2048) #define YAS_YAS530_DATA_OVERFLOW (4095) #define YAS_YAS532_DEVICE_ID (0x02) /* YAS532 (MS-3R) */ #define YAS_YAS532_VERSION_AB (0) /* YAS532AB (MS-3R ABver) */ #define YAS_YAS532_VERSION_AC (1) /* YAS532AC (MS-3R ACver) */ #define YAS_YAS532_VERSION_AB_COEF (1800) #define YAS_YAS532_VERSION_AC_COEF (900) #define YAS_YAS532_DATA_CENTER (4096) #define YAS_YAS532_DATA_OVERFLOW (8190) #undef YAS_YAS530_CAL_SINGLE_READ struct yas_machdep_func { int (*device_open) (void); int (*device_close) (void); int (*device_write) (uint8_t addr, const uint8_t *buf, int len); int (*device_read) (uint8_t addr, uint8_t *buf, int len); void (*msleep) (int msec); }; static int yas_cdrv_actuate_initcoil(void); static int yas_cdrv_set_offset(const int8_t *offset); static int yas_cdrv_recalc_calib_offset(int32_t *prev_calib_offset, int32_t *new_calib_offset, int8_t *prev_offset, int8_t *new_offset); static int yas_cdrv_set_transformatiom_matrix(const int8_t *transform); static int yas_cdrv_measure_and_set_offset(int8_t *offset); static int yas_cdrv_measure(int32_t *msens, int32_t *raw, int16_t *t); static int yas_cdrv_init(const int8_t *transform, struct yas_machdep_func *func); static int yas_cdrv_term(void); static void (*current_time) (int *sec, int *msec) = { 0}; static int utimeval_init(struct utimeval *val) { if (unlikely(!val)) return -1; val->tv_sec = val->tv_msec = 0; return 0; } static int utimeval_is_initial(struct utimeval *val) { if (unlikely(!val)) return 0; return val->tv_sec == 0 && val->tv_msec == 0; } static int utimeval_is_overflow(struct utimeval *val) { int32_t max; if (unlikely(!val)) return 0; max = (int32_t) (0xffffffff / 1000); if (val->tv_sec > max) { return 1; /* overflow */ } else if (val->tv_sec == max) { if (val->tv_msec > (int32_t) (0xffffffff % 1000)) { return 1; /* overflow */ } } return 0; } static struct utimeval utimeval_plus(struct utimeval *first, struct utimeval *second) { struct utimeval result = { 0, 0 }; int32_t tmp; if (unlikely(!first || !second)) return result; tmp = first->tv_sec + second->tv_sec; if (first->tv_sec >= 0 && second->tv_sec >= 0 && tmp < 0) goto overflow; if (first->tv_sec < 0 && second->tv_sec < 0 && tmp >= 0) goto underflow; result.tv_sec = tmp; result.tv_msec = first->tv_msec + second->tv_msec; if (1000 <= result.tv_msec) { tmp = result.tv_sec + result.tv_msec / 1000; if (result.tv_sec >= 0 && result.tv_msec >= 0 && tmp < 0) goto overflow; result.tv_sec = tmp; result.tv_msec = result.tv_msec % 1000; } if (result.tv_msec < 0) { tmp = result.tv_sec + result.tv_msec / 1000 - 1; if (result.tv_sec < 0 && result.tv_msec < 0 && tmp >= 0) goto underflow; result.tv_sec = tmp; result.tv_msec = result.tv_msec % 1000 + 1000; } return result; overflow: result.tv_sec = 0x7fffffff; result.tv_msec = 999; return result; underflow: result.tv_sec = 0x80000000; result.tv_msec = 0; return result; } static struct utimeval utimeval_minus(struct utimeval *first, struct utimeval *second) { struct utimeval result = { 0, 0 }, tmp; if (first == NULL || second == NULL || second->tv_sec == (int)0x80000000) return result; tmp.tv_sec = -second->tv_sec; tmp.tv_msec = -second->tv_msec; return utimeval_plus(first, &tmp); } static int utimeval_less_than(struct utimeval *first, struct utimeval *second) { if (unlikely(!first || !second)) return 0; if (first->tv_sec > second->tv_sec) return 1; else if (first->tv_sec < second->tv_sec) return 0; else if (first->tv_msec > second->tv_msec) return 1; else return 0; } static int utimeval_greater_than(struct utimeval *first, struct utimeval *second) { if (unlikely(!first || !second)) return 0; if (first->tv_sec < second->tv_sec) return 1; else if (first->tv_sec > second->tv_sec) return 0; else if (first->tv_msec < second->tv_msec) return 1; else return 0; } static int utimeval_greater_or_equal(struct utimeval *first, struct utimeval *second) { return !utimeval_less_than(first, second); } static int utimeval_greater_than_zero(struct utimeval *val) { struct utimeval zero = { 0, 0 }; return utimeval_greater_than(&zero, val); } static int utimeval_less_than_zero(struct utimeval *val) { struct utimeval zero = { 0, 0 }; return utimeval_less_than(&zero, val); } static struct utimeval *msec_to_utimeval(struct utimeval *result, uint32_t msec) { if (unlikely(!result)) return result; result->tv_sec = msec / 1000; result->tv_msec = msec % 1000; return result; } static uint32_t utimeval_to_msec(struct utimeval *val) { if (unlikely(!val)) return 0; if (utimeval_less_than_zero(val)) return 0; if (utimeval_is_overflow(val)) return 0xffffffff; return val->tv_sec * 1000 + val->tv_msec; } static struct utimeval utimer_calc_next_time(struct utimer *ut, struct utimeval *cur) { struct utimeval result = { 0, 0 }, delay; if (ut == NULL || cur == NULL) return result; utimer_update_with_curtime(ut, cur); if (utimer_is_timeout(ut)) { result = *cur; } else { delay = utimeval_minus(&ut->delay_ms, &ut->total_time); result = utimeval_plus(cur, &delay); } return result; } static struct utimeval utimer_current_time(void) { struct utimeval tv; int sec, msec; if (current_time != NULL) current_time(&sec, &msec); else sec = 0, msec = 0; tv.tv_sec = sec; tv.tv_msec = msec; return tv; } static int utimer_clear(struct utimer *ut) { if (unlikely(!ut)) return -1; utimeval_init(&ut->prev_time); utimeval_init(&ut->total_time); return 0; } static int utimer_update_with_curtime(struct utimer *ut, struct utimeval *cur) { struct utimeval tmp; if (unlikely(!ut || !cur)) return -1; if (utimeval_is_initial(&ut->prev_time)) ut->prev_time = *cur; if (utimeval_greater_than_zero(&ut->delay_ms)) { tmp = utimeval_minus(cur, &ut->prev_time); if (utimeval_less_than_zero(&tmp)) utimeval_init(&ut->total_time); else { ut->total_time = utimeval_plus(&tmp, &ut->total_time); if (utimeval_is_overflow(&ut->total_time)) utimeval_init(&ut->total_time); } ut->prev_time = *cur; } return 0; } static int utimer_update(struct utimer *ut) { struct utimeval cur; if (unlikely(!ut)) return -1; cur = utimer_current_time(); utimer_update_with_curtime(ut, &cur); return 0; } static int utimer_is_timeout(struct utimer *ut) { if (unlikely(!ut)) return 0; if (utimeval_greater_than_zero(&ut->delay_ms)) return utimeval_greater_or_equal(&ut->delay_ms, &ut->total_time); else return 1; } static int utimer_clear_timeout(struct utimer *ut) { uint32_t delay, total; if (unlikely(!ut)) return -1; delay = utimeval_to_msec(&ut->delay_ms); if (delay == 0 || utimeval_is_overflow(&ut->total_time)) total = 0; else if (utimeval_is_overflow(&ut->total_time)) total = 0; else { total = utimeval_to_msec(&ut->total_time); total = total % delay; } msec_to_utimeval(&ut->total_time, total); return 0; } static uint32_t utimer_sleep_time_with_curtime(struct utimer *ut, struct utimeval *cur) { struct utimeval tv; if (unlikely(!ut || !cur)) return 0; tv = utimer_calc_next_time(ut, cur); tv = utimeval_minus(&tv, cur); if (utimeval_less_than_zero(&tv)) return 0; return utimeval_to_msec(&tv); } static uint32_t utimer_sleep_time(struct utimer *ut) { struct utimeval cur; if (unlikely(!ut)) return 0; cur = utimer_current_time(); return utimer_sleep_time_with_curtime(ut, &cur); } static int utimer_init(struct utimer *ut, uint32_t delay_ms) { if (unlikely(!ut)) return -1; utimer_clear(ut); msec_to_utimeval(&ut->delay_ms, delay_ms); return 0; } static uint32_t utimer_get_total_time(struct utimer *ut) { return utimeval_to_msec(&ut->total_time); } static uint32_t utimer_get_delay(struct utimer *ut) { if (unlikely(!ut)) return -1; return utimeval_to_msec(&ut->delay_ms); } static int utimer_set_delay(struct utimer *ut, uint32_t delay_ms) { return utimer_init(ut, delay_ms); } static void utimer_lib_init(void (*func) (int *sec, int *msec)) { current_time = func; } struct yas_cal_data { uint8_t dx, dy1, dy2; uint8_t d2, d3, d4, d5, d6, d7, d8, d9, d0; uint8_t dck; uint8_t ver; }; struct yas_correction_data { int32_t Cx, Cy1, Cy2; int32_t a2, a3, a4, a5, a6, a7, a8, a9, k; }; struct yas_cdriver { struct yas_cal_data cal; struct yas_correction_data correct; struct yas_machdep_func func; int8_t transform[9]; int16_t temperature; uint8_t dev_id; int32_t coef; int16_t center; int16_t overflow; }; static struct yas_cdriver cdriver; static int device_open(void) { if (cdriver.func.device_open == NULL) return -1; return cdriver.func.device_open(); } static int device_close(void) { if (cdriver.func.device_close == NULL) return -1; return cdriver.func.device_close(); } static int device_write(uint8_t addr, const uint8_t *buf, int len) { if (cdriver.func.device_write == NULL) return -1; return cdriver.func.device_write(addr, buf, len); } static int device_read(uint8_t addr, uint8_t *buf, int len) { if (cdriver.func.device_read == NULL) return -1; return cdriver.func.device_read(addr, buf, len); } static void sleep(int millisec) { if (cdriver.func.msleep == NULL) return; cdriver.func.msleep(millisec); } static int init_test_register(void) { uint8_t data; data = 0x00; if (device_write(YAS_REGADDR_TEST1, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; data = 0x00; if (device_write(YAS_REGADDR_TEST2, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int get_device_id(uint8_t *id) { uint8_t data = 0; if (device_read(YAS_REGADDR_DEVICE_ID, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; *id = data; return YAS_NO_ERROR; } static int get_cal_data_yas530(struct yas_cal_data *cal) { uint8_t data[16]; #ifdef YAS_YAS530_CAL_SINGLE_READ int i; for (i = 0; i < 16; i++) { /* dummy read */ if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } for (i = 0; i < 16; i++) { if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } #else if (device_read(YAS_REGADDR_CAL, data, 16) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (device_read(YAS_REGADDR_CAL, data, 16) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; #endif cal->dx = data[0]; cal->dy1 = data[1]; cal->dy2 = data[2]; cal->d2 = (data[3] >> 2) & 0x03f; cal->d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); cal->d4 = data[4] & 0x3f; cal->d5 = (data[5] >> 2) & 0x3f; cal->d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); cal->d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); cal->d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); cal->d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); cal->d0 = (data[9] >> 2) & 0x1f; cal->dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); cal->ver = (data[15]) & 0x03; return YAS_NO_ERROR; } static void get_correction_value_yas530(struct yas_cal_data *cal, struct yas_correction_data *correct) { correct->Cx = cal->dx * 6 - 768; correct->Cy1 = cal->dy1 * 6 - 768; correct->Cy2 = cal->dy2 * 6 - 768; correct->a2 = cal->d2 - 32; correct->a3 = cal->d3 - 8; correct->a4 = cal->d4 - 32; correct->a5 = cal->d5 + 38; correct->a6 = cal->d6 - 32; correct->a7 = cal->d7 - 64; correct->a8 = cal->d8 - 32; correct->a9 = cal->d9; correct->k = cal->d0 + 10; } static int get_cal_data_yas532(struct yas_cal_data *cal) { uint8_t data[14]; #ifdef YAS_YAS530_CAL_SINGLE_READ int i; for (i = 0; i < 14; i++) { /* dummy read */ if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } for (i = 0; i < 14; i++) { if (device_read(YAS_REGADDR_CAL + i, &data[i], 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } #else if (device_read(YAS_REGADDR_CAL, data, 14) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (device_read(YAS_REGADDR_CAL, data, 14) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; #endif cal->dx = data[0]; cal->dy1 = data[1]; cal->dy2 = data[2]; cal->d2 = (data[3] >> 2) & 0x03f; cal->d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); cal->d4 = data[4] & 0x3f; cal->d5 = (data[5] >> 2) & 0x3f; cal->d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); cal->d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); cal->d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); cal->d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); cal->d0 = (data[9] >> 2) & 0x1f; cal->dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); cal->ver = (data[13]) & 0x01; return YAS_NO_ERROR; } static void get_correction_value_yas532(struct yas_cal_data *cal, struct yas_correction_data *correct) { correct->Cx = cal->dx * 10 - 1280; correct->Cy1 = cal->dy1 * 10 - 1280; correct->Cy2 = cal->dy2 * 10 - 1280; correct->a2 = cal->d2 - 32; correct->a3 = cal->d3 - 8; correct->a4 = cal->d4 - 32; correct->a5 = cal->d5 + 38; correct->a6 = cal->d6 - 32; correct->a7 = cal->d7 - 64; correct->a8 = cal->d8 - 32; correct->a9 = cal->d9; correct->k = cal->d0; } static int set_configuration(int inton, int inthact, int cck) { uint8_t data = 0; data |= (!!inton) & 0x01; data |= ((!!inthact) << 1) & 0x02; data |= (cck << 2) & 0x1c; if (device_write(YAS_REGADDR_CONFIG, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int get_measure_interval(int32_t *msec) { uint8_t data; int mult = 7; if (device_read(YAS_REGADDR_MEASURE_INTERVAL, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; switch (cdriver.dev_id) { case YAS_YAS532_DEVICE_ID: mult = 4; break; case YAS_YAS530_DEVICE_ID: default: mult = 7; break; } *msec = data * mult; return YAS_NO_ERROR; } static int set_measure_interval(int32_t msec) { uint8_t data = 0; int mult = 7; switch (cdriver.dev_id) { case YAS_YAS532_DEVICE_ID: mult = 4; break; case YAS_YAS530_DEVICE_ID: default: mult = 7; break; } if (msec > mult*0xff) data = 0xff; else if (msec % mult == 0) data = (uint8_t)(msec / mult); else data = (uint8_t)(msec / mult + 1); if (device_write(YAS_REGADDR_MEASURE_INTERVAL, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int set_measure_command(int ldtc, int fors, int dlymes) { uint8_t data = 0; data |= 0x01; /* bit 0 must be 1 */ data |= ((!(!ldtc)) << 1) & 0x02; data |= ((!(!fors)) << 2) & 0x04; data |= ((!(!dlymes)) << 4) & 0x10; if (device_write(YAS_REGADDR_MEASURE_COMMAND, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int measure_normal_yas530(int *busy, int16_t *t, int16_t *x, int16_t *y1, int16_t *y2) { uint8_t data[8]; if (set_measure_command(0, 0, 0) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; sleep(2); if (device_read(YAS_REGADDR_MEASURE_DATA, data, 8) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; *busy = (data[0] >> 7) & 0x01; *t = (((int32_t) data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03); *x = (((int32_t) data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f); *y1 = (((int32_t) data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f); *y2 = (((int32_t) data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f); /*YLOGD(("f[%d] t[%d] x[%d] y1[%d] y2[%d]\n", *busy, *t, *x, *y1, *y2)); */ return YAS_NO_ERROR; } static int measure_normal_yas532(int *busy, int16_t *t, int16_t *x, int16_t *y1, int16_t *y2) { uint8_t data[8]; if (set_measure_command(0, 0, 0) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; sleep(2); if (device_read(YAS_REGADDR_MEASURE_DATA, data, 8) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; *busy = (data[0] >> 7) & 0x01; *t = (((int32_t) data[0] << 3) & 0x3f8) | ((data[1] >> 5) & 0x07); *x = (((int32_t) data[2] << 6) & 0x1fc0) | ((data[3] >> 2) & 0x3f); *y1 = (((int32_t) data[4] << 6) & 0x1fc0) | ((data[5] >> 2) & 0x3f); *y2 = (((int32_t) data[6] << 6) & 0x1fc0) | ((data[7] >> 2) & 0x3f); /*YLOGD(("f[%d] t[%d] x[%d] y1[%d] y2[%d]\n", *busy, *t, *x, *y1, *y2)); */ return YAS_NO_ERROR; } static int measure_normal(int *busy, int16_t *t, int16_t *x, int16_t *y1, int16_t *y2) { int result; switch (cdriver.dev_id) { case YAS_YAS532_DEVICE_ID: result = measure_normal_yas532(busy, t, x, y1, y2); break; case YAS_YAS530_DEVICE_ID: default: result = measure_normal_yas530(busy, t, x, y1, y2); break; } return result; } static int coordinate_conversion(int32_t x, int32_t y1, int32_t y2, int16_t t, int32_t *xo, int32_t *yo, int32_t *zo, struct yas_correction_data *c) { int32_t sx, sy1, sy2, sy, sz; int32_t hx, hy, hz; sx = x - (c->Cx * t) / 100; sy1 = y1 - (c->Cy1 * t) / 100; sy2 = y2 - (c->Cy2 * t) / 100; sy = sy1 - sy2; sz = -sy1 - sy2; hx = c->k * ((100 * sx + c->a2 * sy + c->a3 * sz) / 10); hy = c->k * ((c->a4 * sx + c->a5 * sy + c->a6 * sz) / 10); hz = c->k * ((c->a7 * sx + c->a8 * sy + c->a9 * sz) / 10); *xo = cdriver.transform[0] * hx + cdriver.transform[1] * hy + cdriver.transform[2] * hz; *yo = cdriver.transform[3] * hx + cdriver.transform[4] * hy + cdriver.transform[5] * hz; *zo = cdriver.transform[6] * hx + cdriver.transform[7] * hy + cdriver.transform[8] * hz; return YAS_NO_ERROR; } static int set_hardware_offset(int8_t offset_x, int8_t offset_y1, int8_t offset_y2) { uint8_t data; data = offset_x & 0x3f; if (device_write(YAS_REGADDR_OFFSET_X, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; data = offset_y1 & 0x3f; if (device_write(YAS_REGADDR_OFFSET_Y1, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; data = offset_y2 & 0x3f; if (device_write(YAS_REGADDR_OFFSET_Y2, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int yas_cdrv_actuate_initcoil(void) { uint8_t data = 0; if (device_write(YAS_REGADDR_ACTUATE_INIT_COIL, &data, 1) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int check_offset(int8_t offset_x, int8_t offset_y1, int8_t offset_y2, int *flag_x, int *flag_y1, int *flag_y2) { int busy; int16_t t, x, y1, y2; if (set_hardware_offset(offset_x, offset_y1, offset_y2) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (measure_normal(&busy, &t, &x, &y1, &y2) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; *flag_x = *flag_y1 = *flag_y2 = 0; if (x > cdriver.center) *flag_x = 1; if (y1 > cdriver.center) *flag_y1 = 1; if (y2 > cdriver.center) *flag_y2 = 1; if (x < cdriver.center) *flag_x = -1; if (y1 < cdriver.center) *flag_y1 = -1; if (y2 < cdriver.center) *flag_y2 = -1; return YAS_NO_ERROR; } static int yas_cdrv_measure_and_set_offset(int8_t *offset) { int i; int8_t offset_x = 0, offset_y1 = 0, offset_y2 = 0; int flag_x = 0, flag_y1 = 0, flag_y2 = 0; static const int correct[5] = { 16, 8, 4, 2, 1 }; for (i = 0; i < 5; i++) { if (check_offset(offset_x, offset_y1, offset_y2, &flag_x, &flag_y1, &flag_y2) < 0) { return YAS_ERROR_DEVICE_COMMUNICATION; } YLOGD(("offset[%d][%d][%d] flag[%d][%d][%d]\n", offset_x, offset_y1, offset_y2, flag_x, flag_y1, flag_y2)); if (flag_x) offset_x += flag_x * correct[i]; if (flag_y1) offset_y1 += flag_y1 * correct[i]; if (flag_y2) offset_y2 += flag_y2 * correct[i]; } if (set_hardware_offset(offset_x, offset_y1, offset_y2) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; offset[0] = offset_x; offset[1] = offset_y1; offset[2] = offset_y2; return YAS_NO_ERROR; } static int yas_cdrv_set_offset(const int8_t *offset) { if (set_hardware_offset(offset[0], offset[1], offset[2]) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; return YAS_NO_ERROR; } static int yas_cdrv_measure(int32_t *data, int32_t *raw, int16_t *temperature) { int busy; int16_t x, y1, y2, t; int32_t xx, yy1, yy2; int result = 0; if (measure_normal(&busy, &t, &x, &y1, &y2) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (x == 0) result |= 0x01; if (x == cdriver.overflow) result |= 0x02; if (y1 == 0) result |= 0x04; if (y1 == cdriver.overflow) result |= 0x08; if (y2 == 0) result |= 0x10; if (y2 == cdriver.overflow) result |= 0x20; xx = x; yy1 = y1; yy2 = y2; if (coordinate_conversion(xx, yy1, yy2, t, &data[0], &data[1], &data[2], &cdriver.correct) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; cdriver.temperature = t; if (raw != NULL) raw[0] = xx, raw[1] = yy1, raw[2] = yy2; if (temperature != NULL) *temperature = t; return result; } static int yas_cdrv_recalc_calib_offset(int32_t *prev_calib_offset, int32_t *new_calib_offset, int8_t *prev_offset, int8_t *new_offset) { int32_t tmp[3], resolution[9], base[3]; int32_t raw[3]; int32_t diff, i; if (prev_calib_offset == NULL || new_calib_offset == NULL || prev_offset == NULL || new_offset == NULL) return YAS_ERROR_ARG; raw[0] = raw[1] = raw[2] = 0; if (coordinate_conversion(raw[0], raw[1], raw[2], cdriver.temperature, &base[0], &base[1], &base[2], &cdriver.correct) < 0) return YAS_ERROR_ERROR; for (i = 0; i < 3; i++) { raw[0] = raw[1] = raw[2] = 0; raw[i] = cdriver.coef; if (coordinate_conversion(raw[0], raw[1], raw[2], cdriver.temperature, &resolution[i * 3 + 0], &resolution[i * 3 + 1], &resolution[i * 3 + 2], &cdriver.correct) < 0) return YAS_ERROR_ERROR; resolution[i * 3 + 0] -= base[0]; resolution[i * 3 + 1] -= base[1]; resolution[i * 3 + 2] -= base[2]; } for (i = 0; i < 3; i++) tmp[i] = prev_calib_offset[i]; for (i = 0; i < 3; i++) { diff = (int32_t) new_offset[i] - (int32_t) prev_offset[i]; while (diff > 0) { tmp[0] -= resolution[i * 3 + 0]; tmp[1] -= resolution[i * 3 + 1]; tmp[2] -= resolution[i * 3 + 2]; diff--; } while (diff < 0) { tmp[0] += resolution[i * 3 + 0]; tmp[1] += resolution[i * 3 + 1]; tmp[2] += resolution[i * 3 + 2]; diff++; } } for (i = 0; i < 3; i++) new_calib_offset[i] = tmp[i]; return YAS_NO_ERROR; } static int yas_cdrv_set_transformatiom_matrix(const int8_t *transform) { int i; if (transform == NULL) return YAS_ERROR_ARG; for (i = 0; i < 9; i++) cdriver.transform[i] = transform[i]; return YAS_NO_ERROR; } static int yas_cdrv_init(const int8_t *transform, struct yas_machdep_func *func) { int interval, i; uint8_t id; if (transform == NULL || func == NULL) return YAS_ERROR_ARG; for (i = 0; i < 9; i++) cdriver.transform[i] = transform[i]; cdriver.func = *func; if (device_open() < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (init_test_register() < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (get_device_id(&id) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; YLOGD(("device id:%02x\n", id)); switch (id) { case YAS_YAS530_DEVICE_ID: if (get_cal_data_yas530(&cdriver.cal) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; get_correction_value_yas530(&cdriver.cal, &cdriver.correct); cdriver.center = YAS_YAS530_DATA_CENTER; cdriver.overflow = YAS_YAS530_DATA_OVERFLOW; switch (cdriver.cal.ver) { case YAS_YAS530_VERSION_B: cdriver.coef = YAS_YAS530_VERSION_B_COEF; break; case YAS_YAS530_VERSION_A: default: cdriver.coef = YAS_YAS530_VERSION_A_COEF; break; } break; case YAS_YAS532_DEVICE_ID: if (get_cal_data_yas532(&cdriver.cal) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; get_correction_value_yas532(&cdriver.cal, &cdriver.correct); cdriver.center = YAS_YAS532_DATA_CENTER; cdriver.overflow = YAS_YAS532_DATA_OVERFLOW; switch (cdriver.cal.ver) { case YAS_YAS532_VERSION_AC: cdriver.coef = YAS_YAS532_VERSION_AC_COEF; break; case YAS_YAS532_VERSION_AB: default: cdriver.coef = YAS_YAS532_VERSION_AB_COEF; break; } break; default: return YAS_ERROR_DEVICE_COMMUNICATION; } cdriver.dev_id = id; if (set_configuration(0, 0, cdriver.cal.dck) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (set_measure_interval(0) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (get_measure_interval(&interval) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; YLOGD(("interval[%d]\n", interval)); return YAS_NO_ERROR; } static int yas_cdrv_term(void) { device_close(); return YAS_NO_ERROR; } #define YAS_DEFAULT_CALIB_INTERVAL (50) /* 50 msecs */ #define YAS_DEFAULT_DATA_INTERVAL (200) /* 200 msecs */ #define YAS_INITCOIL_INTERVAL (200) /* 200 msec */ #define YAS_INITCOIL_GIVEUP_INTERVAL (180000) /* 180 seconds */ #define YAS_DETECT_OVERFLOW_INTERVAL (0) /* 0 second */ #define YAS_MAG_ERROR_DELAY (200) #define YAS_MAG_STATE_NORMAL (0) #define YAS_MAG_STATE_INIT_COIL (1) #define YAS_MAG_STATE_MEASURE_OFFSET (2) static const int8_t YAS_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 const int supported_data_interval[] = { 10, 20, 50, 60, 100, 200, 1000 }; static const int supported_calib_interval[] = { 60, 60, 50, 60, 50, 50, 50 }; static const int32_t INVALID_CALIB_OFFSET[] = { 0x7fffffff, 0x7fffffff, 0x7fffffff }; static const int8_t INVALID_OFFSET[] = { 0x7f, 0x7f, 0x7f }; struct yas_adaptive_filter { int num; int index; int filter_len; int filter_noise; int32_t sequence[YAS_MAG_MAX_FILTER_LEN]; }; struct yas_thresh_filter { int32_t threshold; int32_t last; }; struct yas_driver { int initialized; struct yas_mag_driver_callback callback; struct utimer data_timer; struct utimer initcoil_timer; struct utimer initcoil_giveup_timer; struct utimer detect_overflow_timer; int32_t prev_mag[3]; int32_t prev_xy1y2[3]; int32_t prev_mag_w_offset[3]; int16_t prev_temperature; int measure_state; int active; int overflow; int initcoil_gaveup; int position; int delay_timer_use_data; int delay_timer_interval; int delay_timer_counter; int filter_enable; int filter_len; int filter_thresh; int filter_noise[3]; struct yas_adaptive_filter adap_filter[3]; struct yas_thresh_filter thresh_filter[3]; struct yas_mag_offset offset; #ifdef YAS_MAG_MANUAL_OFFSET struct yas_vector manual_offset; #endif struct yas_matrix static_matrix; struct yas_matrix dynamic_matrix; }; static struct yas_driver this_driver; static int lock(void) { if (this_driver.callback.lock != NULL) { if (this_driver.callback.lock() < 0) return YAS_ERROR_RESTARTSYS; } return 0; } static int unlock(void) { if (this_driver.callback.unlock != NULL) { if (this_driver.callback.unlock() < 0) return YAS_ERROR_RESTARTSYS; } return 0; } static int32_t square(int32_t data) { return data * data; } static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, int noise) { int i; adap_filter->num = 0; adap_filter->index = 0; adap_filter->filter_noise = noise; adap_filter->filter_len = len; for (i = 0; i < adap_filter->filter_len; ++i) adap_filter->sequence[i] = 0; } static int32_t adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, int32_t in) { int32_t avg, sum; int i; if (adap_filter->filter_len == 0) return in; if (adap_filter->num < adap_filter->filter_len) { adap_filter->sequence[adap_filter->index++] = in / 100; adap_filter->num++; return in; } if (adap_filter->filter_len <= adap_filter->index) adap_filter->index = 0; adap_filter->sequence[adap_filter->index++] = in / 100; avg = 0; for (i = 0; i < adap_filter->filter_len; i++) avg += adap_filter->sequence[i]; avg /= adap_filter->filter_len; sum = 0; for (i = 0; i < adap_filter->filter_len; i++) sum += square(avg - adap_filter->sequence[i]); sum /= adap_filter->filter_len; if (sum <= adap_filter->filter_noise) return avg * 100; return ((in / 100 - avg) * (sum - adap_filter->filter_noise) / sum + avg) * 100; } static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, int threshold) { thresh_filter->threshold = threshold; thresh_filter->last = 0; } static int32_t thresh_filter_filter(struct yas_thresh_filter *thresh_filter, int32_t in) { if (in < thresh_filter->last - thresh_filter->threshold || thresh_filter->last + thresh_filter->threshold < in) { thresh_filter->last = in; return in; } else return thresh_filter->last; } static void filter_init(struct yas_driver *d) { int i; for (i = 0; i < 3; i++) { adaptive_filter_init(&d->adap_filter[i], d->filter_len, d->filter_noise[i]); thresh_filter_init(&d->thresh_filter[i], d->filter_thresh); } } static void filter_filter(struct yas_driver *d, int32_t *orig, int32_t *filtered) { int i; for (i = 0; i < 3; i++) { filtered[i] = adaptive_filter_filter(&d->adap_filter[i], orig[i]); filtered[i] = thresh_filter_filter(&d->thresh_filter[i], filtered[i]); } } static int is_valid_offset(const int8_t *p) { return (p != NULL && (p[0] <= 31) && (p[1] <= 31) && (p[2] <= 31) && (-31 <= p[0]) && (-31 <= p[1]) && (-31 <= p[2])); } static int is_valid_calib_offset(const int32_t *p) { int i; for (i = 0; i < 3; i++) { if (p[i] != INVALID_CALIB_OFFSET[i]) return 1; } return 0; } static int is_offset_differ(const int8_t *p0, const int8_t *p1) { if (p0[0] != p1[0] || p0[1] != p1[1] || p0[2] != p1[2]) return 1; else return 0; } static int is_calib_offset_differ(const int32_t *p0, const int32_t *p1) { if (p0[0] != p1[0] || p0[1] != p1[1] || p0[2] != p1[2]) return 1; else return 0; } static int get_overflow(struct yas_driver *d) { return d->overflow; } static void set_overflow(struct yas_driver *d, const int overflow) { if (d->overflow != overflow) d->overflow = overflow; } static int get_initcoil_gaveup(struct yas_driver *d) { return d->initcoil_gaveup; } static void set_initcoil_gaveup(struct yas_driver *d, const int initcoil_gaveup) { d->initcoil_gaveup = initcoil_gaveup; } static int32_t *get_calib_offset(struct yas_driver *d) { return d->offset.calib_offset.v; } static void set_calib_offset(struct yas_driver *d, const int32_t *offset) { int i; if (is_calib_offset_differ(d->offset.calib_offset.v, offset)) { for (i = 0; i < 3; i++) d->offset.calib_offset.v[i] = offset[i]; } } #ifdef YAS_MAG_MANUAL_OFFSET static int32_t *get_manual_offset(struct yas_driver *d) { return d->manual_offset.v; } static void set_manual_offset(struct yas_driver *d, const int32_t *offset) { int i; for (i = 0; i < 3; i++) d->manual_offset.v[i] = offset[i]; } #endif static int32_t *get_static_matrix(struct yas_driver *d) { return d->static_matrix.matrix; } static void set_static_matrix(struct yas_driver *d, const int32_t *matrix) { int i; for (i = 0; i < 9; i++) d->static_matrix.matrix[i] = matrix[i]; } static int32_t *get_dynamic_matrix(struct yas_driver *d) { return d->dynamic_matrix.matrix; } static void set_dynamic_matrix(struct yas_driver *d, const int32_t *matrix) { int i; for (i = 0; i < 9; i++) d->dynamic_matrix.matrix[i] = matrix[i]; } static int8_t *get_offset(struct yas_driver *d) { return d->offset.hard_offset; } static void set_offset(struct yas_driver *d, const int8_t *offset) { int i; if (is_offset_differ(d->offset.hard_offset, offset)) { for (i = 0; i < 3; i++) d->offset.hard_offset[i] = offset[i]; } } static int get_active(struct yas_driver *d) { return d->active; } static void set_active(struct yas_driver *d, const int active) { d->active = active; } static int get_position(struct yas_driver *d) { return d->position; } static void set_position(struct yas_driver *d, const int position) { d->position = position; } static int get_measure_state(struct yas_driver *d) { return d->measure_state; } static void set_measure_state(struct yas_driver *d, const int state) { d->measure_state = state; } static struct utimer *get_data_timer(struct yas_driver *d) { return &d->data_timer; } static struct utimer *get_initcoil_timer(struct yas_driver *d) { return &d->initcoil_timer; } static struct utimer *get_initcoil_giveup_timer(struct yas_driver *d) { return &d->initcoil_giveup_timer; } static struct utimer *get_detect_overflow_timer(struct yas_driver *d) { return &d->detect_overflow_timer; } static int get_delay_timer_use_data(struct yas_driver *d) { return d->delay_timer_use_data; } static void set_delay_timer_use_data(struct yas_driver *d, int flag) { d->delay_timer_use_data = !(!flag); } static int get_delay_timer_interval(struct yas_driver *d) { return d->delay_timer_interval; } static void set_delay_timer_interval(struct yas_driver *d, int interval) { d->delay_timer_interval = interval; } static int get_delay_timer_counter(struct yas_driver *d) { return d->delay_timer_counter; } static void set_delay_timer_counter(struct yas_driver *d, int counter) { d->delay_timer_counter = counter; } static int get_filter_enable(struct yas_driver *d) { return d->filter_enable; } static void set_filter_enable(struct yas_driver *d, int enable) { if (enable) filter_init(d); d->filter_enable = !(!enable); } static int get_filter_len(struct yas_driver *d) { return d->filter_len; } static void set_filter_len(struct yas_driver *d, int len) { if (len < 0) return; if (len > YAS_MAG_MAX_FILTER_LEN) return; d->filter_len = len; filter_init(d); } static int get_filter_noise(struct yas_driver *d, int *noise) { int i; for (i = 0; i < 3; i++) noise[i] = d->filter_noise[i]; return 0; } static void set_filter_noise(struct yas_driver *d, int *noise) { int i; if (noise == NULL) return; for (i = 0; i < 3; i++) { if (noise[i] < 0) noise[i] = 0; d->filter_noise[i] = noise[i]; } filter_init(d); } static int get_filter_thresh(struct yas_driver *d) { return d->filter_thresh; } static void set_filter_thresh(struct yas_driver *d, int threshold) { if (threshold < 0) return; d->filter_thresh = threshold; filter_init(d); } static int32_t *get_previous_mag(struct yas_driver *d) { return d->prev_mag; } static void set_previous_mag(struct yas_driver *d, int32_t *data) { int i; for (i = 0; i < 3; i++) d->prev_mag[i] = data[i]; } static int32_t *get_previous_xy1y2(struct yas_driver *d) { return d->prev_xy1y2; } static void set_previous_xy1y2(struct yas_driver *d, int32_t *data) { int i; for (i = 0; i < 3; i++) d->prev_xy1y2[i] = data[i]; } static int32_t *get_previous_mag_w_offset(struct yas_driver *d) { return d->prev_mag_w_offset; } static void set_previous_mag_w_offset(struct yas_driver *d, int32_t *data) { int i; for (i = 0; i < 3; i++) d->prev_mag_w_offset[i] = data[i]; } static int16_t get_previous_temperature(struct yas_driver *d) { return d->prev_temperature; } static void set_previous_temperature(struct yas_driver *d, int16_t temperature) { d->prev_temperature = temperature; } static int init_coil(struct yas_driver *d) { int rt; YLOGD(("init_coil IN\n")); utimer_update(get_initcoil_timer(d)); if (!get_initcoil_gaveup(d)) { utimer_update(get_initcoil_giveup_timer(d)); if (utimer_is_timeout(get_initcoil_giveup_timer(d))) { utimer_clear_timeout(get_initcoil_giveup_timer(d)); set_initcoil_gaveup(d, TRUE); } } if (utimer_is_timeout(get_initcoil_timer(d)) && !get_initcoil_gaveup(d)) { utimer_clear_timeout(get_initcoil_timer(d)); YLOGI(("init_coil!\n")); rt = yas_cdrv_actuate_initcoil(); if (rt < 0) { YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); return rt; } if (get_overflow(d) || !is_valid_offset(get_offset(d))) set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); else set_measure_state(d, YAS_MAG_STATE_NORMAL); } YLOGD(("init_coil OUT\n")); return 0; } static int measure_offset(struct yas_driver *d) { int8_t offset[3]; int32_t moffset[3]; int rt, result = 0, i; YLOGI(("measure_offset IN\n")); rt = yas_cdrv_measure_and_set_offset(offset); if (rt < 0) { YLOGE(("yas_cdrv_measure_offset failed[%d]\n", rt)); return rt; } YLOGI(("offset[%d][%d][%d]\n", offset[0], offset[1], offset[2])); for (i = 0; i < 3; i++) moffset[i] = get_calib_offset(d)[i]; if (is_offset_differ(get_offset(d), offset)) { if (is_valid_offset(get_offset(d)) && is_valid_calib_offset(get_calib_offset(d))) { yas_cdrv_recalc_calib_offset(get_calib_offset(d), moffset, get_offset(d), offset); result |= YAS_REPORT_CALIB_OFFSET_CHANGED; } } result |= YAS_REPORT_HARD_OFFSET_CHANGED; set_offset(d, offset); if (is_valid_calib_offset(moffset)) set_calib_offset(d, moffset); set_measure_state(d, YAS_MAG_STATE_NORMAL); YLOGI(("measure_offset OUT\n")); return result; } static int measure_msensor_normal(struct yas_driver *d, int32_t *magnetic, int32_t *mag_w_offset, int32_t * xy1y2, int16_t *temperature) { int rt = 0, result, i; int32_t tmp[3]; YLOGD(("measure_msensor_normal IN\n")); result = 0; rt = yas_cdrv_measure(mag_w_offset, tmp, temperature); if (rt < 0) { YLOGE(("yas_cdrv_measure failed[%d]\n", rt)); return rt; } for (i = 0; i < 3; i++) xy1y2[i] = tmp[i]; #ifdef YAS_MAG_MANUAL_OFFSET for (i = 0; i < 3; i++) mag_w_offset[i] -= get_manual_offset(d)[i]; #endif if (rt > 0) { utimer_update(get_detect_overflow_timer(d)); set_overflow(d, TRUE); if (utimer_is_timeout(get_detect_overflow_timer(d))) { utimer_clear_timeout(get_detect_overflow_timer(d)); result |= YAS_REPORT_OVERFLOW_OCCURED; } if (get_measure_state(d) == YAS_MAG_STATE_NORMAL) set_measure_state(d, YAS_MAG_STATE_INIT_COIL); } else { utimer_clear(get_detect_overflow_timer(d)); set_overflow(d, FALSE); if (get_measure_state(d) == YAS_MAG_STATE_NORMAL) { utimer_clear(get_initcoil_timer(d)); utimer_clear(get_initcoil_giveup_timer(d)); } } for (i = 0; i < 3; i++) { tmp[i] = (get_static_matrix(d)[i * 3 + 0] / 10 * (mag_w_offset[0] / 10)) / 100 + (get_static_matrix(d)[i * 3 + 1] / 10 * (mag_w_offset[1] / 10)) / 100 + (get_static_matrix(d)[i * 3 + 2] / 10 * (mag_w_offset[2] / 10)) / 100; } for (i = 0; i < 3; i++) magnetic[i] = mag_w_offset[i] = tmp[i]; if (is_valid_calib_offset(get_calib_offset(d))) { for (i = 0; i < 3; i++) magnetic[i] -= get_calib_offset(d)[i]; } for (i = 0; i < 3; i++) { tmp[i] = (get_dynamic_matrix(d)[i * 3 + 0] / 10 * (magnetic[0] / 10)) / 100 + (get_dynamic_matrix(d)[i * 3 + 1] / 10 * (magnetic[1] / 10)) / 100 + (get_dynamic_matrix(d)[i * 3 + 2] / 10 * (magnetic[2] / 10)) / 100; } for (i = 0; i < 3; i++) magnetic[i] = tmp[i]; if (get_filter_enable(d)) filter_filter(d, magnetic, magnetic); YLOGD(("measure_msensor_normal OUT\n")); return result; } static int measure_msensor(struct yas_driver *d, int32_t *magnetic, int32_t *mag_w_offset, int32_t *xy1y2, int16_t *temperature) { int result, i; YLOGD(("measure_msensor IN\n")); for (i = 0; i < 3; i++) { magnetic[i] = get_previous_mag(d)[i]; mag_w_offset[i] = get_previous_mag_w_offset(d)[i]; xy1y2[i] = get_previous_xy1y2(d)[i]; *temperature = get_previous_temperature(d); } result = 0; switch (get_measure_state(d)) { case YAS_MAG_STATE_INIT_COIL: result = init_coil(d); break; case YAS_MAG_STATE_MEASURE_OFFSET: result = measure_offset(d); break; case YAS_MAG_STATE_NORMAL: result = 0; break; default: result = -1; break; } if (result < 0) return result; if (!(result & YAS_REPORT_OVERFLOW_OCCURED)) result |= measure_msensor_normal(d, magnetic, mag_w_offset, xy1y2, temperature); set_previous_mag(d, magnetic); set_previous_xy1y2(d, xy1y2); set_previous_mag_w_offset(d, mag_w_offset); set_previous_temperature(d, *temperature); YLOGD(("measure_msensor OUT\n")); return result; } static int measure(struct yas_driver *d, int32_t *magnetic, int32_t *mag_w_offset, int32_t *xy1y2, int16_t *temperature, uint32_t *time_delay) { int result; int counter; uint32_t total = 0; YLOGD(("measure IN\n")); utimer_update(get_data_timer(d)); result = measure_msensor(d, magnetic, mag_w_offset, xy1y2, temperature); if (result < 0) return result; counter = get_delay_timer_counter(d); total = utimer_get_total_time(get_data_timer(d)); if (utimer_get_delay(get_data_timer(d)) > 0) counter -= total / utimer_get_delay(get_data_timer(d)); else counter = 0; if (utimer_is_timeout(get_data_timer(d))) { utimer_clear_timeout(get_data_timer(d)); if (get_delay_timer_use_data(d)) { result |= YAS_REPORT_DATA; if (counter <= 0) result |= YAS_REPORT_CALIB; } else { result |= YAS_REPORT_CALIB; if (counter <= 0) result |= YAS_REPORT_DATA; } } if (counter <= 0) set_delay_timer_counter(d, get_delay_timer_interval(d)); else set_delay_timer_counter(d, counter); *time_delay = utimer_sleep_time(get_data_timer(d)); YLOGD(("measure OUT [%d]\n", result)); return result; } static int resume(struct yas_driver *d) { int32_t zero[] = { 0, 0, 0 }; struct yas_machdep_func func; int rt; YLOGI(("resume IN\n")); func.device_open = d->callback.device_open; func.device_close = d->callback.device_close; func.device_write = d->callback.device_write; func.device_read = d->callback.device_read; func.msleep = d->callback.msleep; rt = yas_cdrv_init(YAS_TRANSFORMATION[get_position(d)], &func); if (rt < 0) { YLOGE(("yas_cdrv_init failed[%d]\n", rt)); return rt; } utimer_clear(get_data_timer(d)); utimer_clear(get_initcoil_giveup_timer(d)); utimer_clear(get_initcoil_timer(d)); utimer_clear(get_detect_overflow_timer(d)); set_previous_mag(d, zero); set_previous_xy1y2(d, zero); set_previous_mag_w_offset(d, zero); set_previous_temperature(d, 0); set_overflow(d, FALSE); set_initcoil_gaveup(d, FALSE); filter_init(d); if (is_valid_offset(d->offset.hard_offset)) { yas_cdrv_set_offset(d->offset.hard_offset); rt = yas_cdrv_actuate_initcoil(); if (rt < 0) { YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); set_measure_state(d, YAS_MAG_STATE_INIT_COIL); } else set_measure_state(d, YAS_MAG_STATE_NORMAL); } else { rt = yas_cdrv_actuate_initcoil(); if (rt < 0) { YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); set_measure_state(d, YAS_MAG_STATE_INIT_COIL); } else set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); } YLOGI(("resume OUT\n")); return 0; } static int suspend(struct yas_driver *d) { YLOGI(("suspend IN\n")); (void)d; yas_cdrv_term(); YLOGI(("suspend OUT\n")); return 0; } static int check_interval(int ms) { int index = -1; if (ms <= supported_data_interval[0]) ms = supported_data_interval[0]; for (index = 0; index < NELEMS(supported_data_interval); index++) { if (ms == supported_data_interval[index]) goto done; else if (ms < supported_data_interval[index]) { if (index != 0) index -= 1; goto done; } } done: return index; } static int yas_get_delay_nolock(struct yas_driver *d, int *ms) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (get_delay_timer_use_data(d)) *ms = utimer_get_delay(get_data_timer(d)); else { *ms = utimer_get_delay(get_data_timer(d)) * get_delay_timer_interval(d); } return YAS_NO_ERROR; } static int yas_set_delay_nolock(struct yas_driver *d, int ms) { int index; uint32_t delay_data, delay_calib; if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; index = check_interval(ms); if (index < 0) return YAS_ERROR_ARG; delay_data = supported_data_interval[index]; delay_calib = supported_calib_interval[index]; set_delay_timer_use_data(d, delay_data < delay_calib); if (delay_data < delay_calib) { set_delay_timer_interval(d, delay_calib / delay_data); set_delay_timer_counter(d, delay_calib / delay_data); utimer_set_delay(get_data_timer(d), supported_data_interval[index]); } else { set_delay_timer_interval(d, delay_data / delay_calib); set_delay_timer_counter(d, delay_data / delay_calib); utimer_set_delay(get_data_timer(d), supported_calib_interval[index]); } return YAS_NO_ERROR; } static int yas_get_offset_nolock(struct yas_driver *d, struct yas_mag_offset *offset) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; *offset = d->offset; return YAS_NO_ERROR; } static int yas_set_offset_nolock(struct yas_driver *d, struct yas_mag_offset *offset) { int32_t zero[] = { 0, 0, 0 }; int rt; if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (!get_active(d)) { d->offset = *offset; return YAS_NO_ERROR; } if (!is_valid_offset(offset->hard_offset) || is_offset_differ(offset->hard_offset, d->offset.hard_offset)) { filter_init(d); utimer_clear(get_data_timer(d)); utimer_clear(get_initcoil_giveup_timer(d)); utimer_clear(get_initcoil_timer(d)); utimer_clear(get_detect_overflow_timer(d)); set_previous_mag(d, zero); set_previous_xy1y2(d, zero); set_previous_mag_w_offset(d, zero); set_previous_temperature(d, 0); set_overflow(d, FALSE); set_initcoil_gaveup(d, FALSE); } d->offset = *offset; if (is_valid_offset(d->offset.hard_offset)) yas_cdrv_set_offset(d->offset.hard_offset); else { rt = yas_cdrv_actuate_initcoil(); if (rt < 0) { YLOGE(("yas_cdrv_actuate_initcoil failed[%d]\n", rt)); set_measure_state(d, YAS_MAG_STATE_INIT_COIL); } else set_measure_state(d, YAS_MAG_STATE_MEASURE_OFFSET); } return YAS_NO_ERROR; } #ifdef YAS_MAG_MANUAL_OFFSET static int yas_get_manual_offset_nolock(struct yas_driver *d, struct yas_vector *offset) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; *offset = d->manual_offset; return YAS_NO_ERROR; } static int yas_set_manual_offset_nolock(struct yas_driver *d, struct yas_vector *offset) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; set_manual_offset(d, offset->v); return YAS_NO_ERROR; } #endif static int yas_get_static_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) { int i; if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; for (i = 0; i < 9; i++) matrix->matrix[i] = get_static_matrix(d)[i]; return YAS_NO_ERROR; } static int yas_set_static_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; set_static_matrix(d, matrix->matrix); return YAS_NO_ERROR; } static int yas_get_dynamic_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) { int i; if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; for (i = 0; i < 9; i++) matrix->matrix[i] = get_dynamic_matrix(d)[i]; return YAS_NO_ERROR; } static int yas_set_dynamic_matrix_nolock(struct yas_driver *d, struct yas_matrix *matrix) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; set_dynamic_matrix(d, matrix->matrix); return YAS_NO_ERROR; } static int yas_get_enable_nolock(struct yas_driver *d) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; return get_active(d); } static int yas_set_enable_nolock(struct yas_driver *d, int active) { int rt; if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (active) { if (get_active(d)) return YAS_NO_ERROR; rt = resume(d); if (rt < 0) return rt; set_active(d, TRUE); } else { if (!get_active(d)) return YAS_NO_ERROR; rt = suspend(d); if (rt < 0) return rt; set_active(d, FALSE); } return YAS_NO_ERROR; } static int yas_get_filter_nolock(struct yas_driver *d, struct yas_mag_filter *filter) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; filter->len = get_filter_len(d); get_filter_noise(d, filter->noise); filter->threshold = get_filter_thresh(d); return YAS_NO_ERROR; } static int yas_set_filter_nolock(struct yas_driver *d, struct yas_mag_filter *filter) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; set_filter_len(d, filter->len); set_filter_noise(d, filter->noise); set_filter_thresh(d, filter->threshold); return YAS_NO_ERROR; } static int yas_get_filter_enable_nolock(struct yas_driver *d) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; return get_filter_enable(d); } static int yas_set_filter_enable_nolock(struct yas_driver *d, int enable) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; set_filter_enable(d, enable); return YAS_NO_ERROR; } static int yas_get_position_nolock(struct yas_driver *d, int *position) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; *position = get_position(d); return YAS_NO_ERROR; } static int yas_set_position_nolock(struct yas_driver *d, int position) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (get_active(d)) yas_cdrv_set_transformatiom_matrix(YAS_TRANSFORMATION [position]); set_position(d, position); filter_init(d); return YAS_NO_ERROR; } static int yas_read_reg_nolock(struct yas_driver *d, uint8_t addr, uint8_t * buf, int len) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (!get_active(d)) { if (d->callback.device_open() < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } if (d->callback.device_read(addr, buf, len) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (!get_active(d)) { if (d->callback.device_close() < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } return YAS_NO_ERROR; } static int yas_write_reg_nolock(struct yas_driver *d, uint8_t addr, const uint8_t * buf, int len) { if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (!get_active(d)) { if (d->callback.device_open() < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } if (d->callback.device_write(addr, buf, len) < 0) return YAS_ERROR_DEVICE_COMMUNICATION; if (!get_active(d)) { if (d->callback.device_close() < 0) return YAS_ERROR_DEVICE_COMMUNICATION; } return YAS_NO_ERROR; } static int yas_measure_nolock(struct yas_driver *d, struct yas_mag_data *data, int *time_delay_ms) { uint32_t time_delay = YAS_MAG_ERROR_DELAY; int rt, i; if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; *time_delay_ms = YAS_MAG_ERROR_DELAY; if (!get_active(d)) { for (i = 0; i < 3; i++) { data->xyz.v[i] = get_previous_mag(d)[i]; data->raw.v[i] = get_previous_mag_w_offset(d)[i]; data->xy1y2.v[i] = get_previous_xy1y2(d)[i]; } data->temperature = get_previous_temperature(d); return YAS_NO_ERROR; } rt = measure(d, data->xyz.v, data->raw.v, data->xy1y2.v, &data->temperature, &time_delay); if (rt >= 0) { *time_delay_ms = time_delay; if (*time_delay_ms > 0) *time_delay_ms += 1; } return rt; } static int yas_init_nolock(struct yas_driver *d) { #ifdef YAS_MAG_MANUAL_OFFSET int32_t zero[] = { 0, 0, 0 }; #endif int32_t notransform[] = { 10000, 0, 0, 0, 10000, 0, 0, 0, 10000 }; int noise[] = { YAS_MAG_DEFAULT_FILTER_NOISE_X, YAS_MAG_DEFAULT_FILTER_NOISE_Y, YAS_MAG_DEFAULT_FILTER_NOISE_Z }; YLOGI(("yas_init_nolock IN\n")); utimer_lib_init(this_driver.callback.current_time); utimer_init(get_data_timer(d), 50); utimer_init(get_initcoil_timer(d), YAS_INITCOIL_INTERVAL); utimer_init(get_initcoil_giveup_timer(d), YAS_INITCOIL_GIVEUP_INTERVAL); utimer_init(get_detect_overflow_timer(d), YAS_DETECT_OVERFLOW_INTERVAL); set_delay_timer_use_data(d, 0); set_delay_timer_interval(d, YAS_DEFAULT_DATA_INTERVAL / YAS_DEFAULT_CALIB_INTERVAL); set_delay_timer_counter(d, YAS_DEFAULT_DATA_INTERVAL / YAS_DEFAULT_CALIB_INTERVAL); set_filter_enable(d, FALSE); set_filter_len(d, YAS_MAG_DEFAULT_FILTER_LEN); set_filter_thresh(d, YAS_MAG_DEFAULT_FILTER_THRESH); set_filter_noise(d, noise); filter_init(d); set_calib_offset(d, INVALID_CALIB_OFFSET); #ifdef YAS_MAG_MANUAL_OFFSET set_manual_offset(d, zero); #endif set_static_matrix(d, notransform); set_dynamic_matrix(d, notransform); set_offset(d, INVALID_OFFSET); set_active(d, FALSE); set_position(d, 0); d->initialized = 1; YLOGI(("yas_init_nolock OUT\n")); return YAS_NO_ERROR; } static int yas_term_nolock(struct yas_driver *d) { YLOGI(("yas_term_nolock\n")); if (unlikely(!d->initialized)) return YAS_ERROR_NOT_INITIALIZED; if (get_active(d)) suspend(d); d->initialized = 0; YLOGI(("yas_term_nolock out\n")); return YAS_NO_ERROR; } static int yas_get_delay(void) { int ms = 0, rt; YLOGI(("yas_get_delay\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_delay_nolock(&this_driver, &ms); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_delay[%d] OUT\n", ms)); return (rt < 0 ? rt : ms); } static int yas_set_delay(int delay) { int rt; YLOGI(("yas_set_delay\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_delay_nolock(&this_driver, delay); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_delay OUT\n")); return rt; } static int yas_get_offset(struct yas_mag_offset *offset) { int rt; YLOGI(("yas_get_offset\n")); if (offset == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_offset_nolock(&this_driver, offset); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_offset[%d] OUT\n", rt)); return rt; } static int yas_set_offset(struct yas_mag_offset *offset) { int rt; YLOGI(("yas_set_offset IN\n")); if (offset == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_offset_nolock(&this_driver, offset); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_offset OUT\n")); return rt; } #ifdef YAS_MAG_MANUAL_OFFSET static int yas_get_manual_offset(struct yas_vector *offset) { int rt; YLOGI(("yas_get_manual_offset\n")); if (offset == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_manual_offset_nolock(&this_driver, offset); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_manual_offset[%d] OUT\n", rt)); return rt; } static int yas_set_manual_offset(struct yas_vector *offset) { int rt; YLOGI(("yas_set_manual_offset IN\n")); if (offset == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_manual_offset_nolock(&this_driver, offset); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_manual_offset OUT\n")); return rt; } #endif static int yas_get_static_matrix(struct yas_matrix *matrix) { int rt; YLOGI(("yas_get_static_matrix\n")); if (matrix == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_static_matrix_nolock(&this_driver, matrix); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_static_matrix[%d] OUT\n", rt)); return rt; } static int yas_set_static_matrix(struct yas_matrix *matrix) { int rt; YLOGI(("yas_set_static_matrix IN\n")); if (matrix == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_static_matrix_nolock(&this_driver, matrix); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_static_matrix OUT\n")); return rt; } static int yas_get_dynamic_matrix(struct yas_matrix *matrix) { int rt; YLOGI(("yas_get_dynamic_matrix\n")); if (matrix == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_dynamic_matrix_nolock(&this_driver, matrix); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_dynamic_matrix[%d] OUT\n", rt)); return rt; } static int yas_set_dynamic_matrix(struct yas_matrix *matrix) { int rt; YLOGI(("yas_set_dynamic_matrix IN\n")); if (matrix == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_dynamic_matrix_nolock(&this_driver, matrix); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_dynamic_matrix OUT\n")); return rt; } static int yas_get_enable(void) { int rt; YLOGI(("yas_get_enable\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_enable_nolock(&this_driver); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_enable OUT[%d]\n", rt)); return rt; } static int yas_set_enable(int enable) { int rt; YLOGI(("yas_set_enable IN\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_enable_nolock(&this_driver, enable); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_enable OUT\n")); return rt; } static int yas_get_filter(struct yas_mag_filter *filter) { int rt; YLOGI(("yas_get_filter\n")); if (filter == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_filter_nolock(&this_driver, filter); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_filter[%d] OUT\n", rt)); return rt; } static int yas_set_filter(struct yas_mag_filter *filter) { int rt, i; YLOGI(("yas_set_filter IN\n")); if (filter == NULL || filter->len < 0 || YAS_MAG_MAX_FILTER_LEN < filter->len || filter->threshold < 0) { return YAS_ERROR_ARG; } for (i = 0; i < 3; i++) { if (filter->noise[i] < 0) return YAS_ERROR_ARG; } if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_filter_nolock(&this_driver, filter); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_filter OUT\n")); return rt; } static int yas_get_filter_enable(void) { int rt; YLOGI(("yas_get_filter_enable\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_filter_enable_nolock(&this_driver); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_filter_enable OUT[%d]\n", rt)); return rt; } static int yas_set_filter_enable(int enable) { int rt; YLOGI(("yas_set_filter_enable IN\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_filter_enable_nolock(&this_driver, enable); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_filter_enable OUT\n")); return rt; } static int yas_get_position(void) { int position = 0; int rt; YLOGI(("yas_get_position\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_get_position_nolock(&this_driver, &position); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_get_position[%d] OUT\n", position)); return (rt < 0 ? rt : position); } static int yas_set_position(int position) { int rt; YLOGI(("yas_set_position\n")); if (position < 0 || 7 < position) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_set_position_nolock(&this_driver, position); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_set_position[%d] OUT\n", position)); return rt; } static int yas_read_reg(uint8_t addr, uint8_t *buf, int len) { int rt; YLOGI(("yas_read_reg\n")); if (buf == NULL || len <= 0) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_read_reg_nolock(&this_driver, addr, buf, len); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_read_reg[%d] OUT\n", rt)); return rt; } static int yas_write_reg(uint8_t addr, const uint8_t *buf, int len) { int rt; YLOGI(("yas_write_reg\n")); if (buf == NULL || len <= 0) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_write_reg_nolock(&this_driver, addr, buf, len); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGI(("yas_write_reg[%d] OUT\n", rt)); return rt; } static int yas_measure(struct yas_mag_data *data, int *time_delay_ms) { int rt; YLOGD(("yas_measure IN\n")); if (data == NULL || time_delay_ms == NULL) return YAS_ERROR_ARG; if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_measure_nolock(&this_driver, data, time_delay_ms); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; YLOGD(("yas_measure OUT[%d]\n", rt)); return rt; } static int yas_init(void) { int rt; YLOGI(("yas_init\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_init_nolock(&this_driver); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; return rt; } static int yas_term(void) { int rt; YLOGI(("yas_term\n")); if (lock() < 0) return YAS_ERROR_RESTARTSYS; rt = yas_term_nolock(&this_driver); if (unlock() < 0) return YAS_ERROR_RESTARTSYS; return rt; } int yas_mag_driver_init(struct yas_mag_driver *f) { if (f == NULL) return YAS_ERROR_ARG; if (f->callback.device_open == NULL || f->callback.device_close == NULL || f->callback.device_read == NULL || f->callback.device_write == NULL || f->callback.msleep == NULL || f->callback.current_time == 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_offset = yas_get_offset; f->set_offset = yas_set_offset; #ifdef YAS_MAG_MANUAL_OFFSET f->get_manual_offset = yas_get_manual_offset; f->set_manual_offset = yas_set_manual_offset; #endif f->get_static_matrix = yas_get_static_matrix; f->set_static_matrix = yas_set_static_matrix; f->get_dynamic_matrix = yas_get_dynamic_matrix; f->set_dynamic_matrix = yas_set_dynamic_matrix; f->get_enable = yas_get_enable; f->set_enable = yas_set_enable; f->get_filter = yas_get_filter; f->set_filter = yas_set_filter; f->get_filter_enable = yas_get_filter_enable; f->set_filter_enable = yas_set_filter_enable; f->get_position = yas_get_position; f->set_position = yas_set_position; f->read_reg = yas_read_reg; f->write_reg = yas_write_reg; f->measure = yas_measure; if ((f->callback.lock == NULL && f->callback.unlock != NULL) || (f->callback.lock != NULL && f->callback.unlock == NULL)) { this_driver.callback.lock = NULL; this_driver.callback.unlock = NULL; } else { this_driver.callback.lock = f->callback.lock; this_driver.callback.unlock = f->callback.unlock; } this_driver.callback.device_open = f->callback.device_open; this_driver.callback.device_close = f->callback.device_close; this_driver.callback.device_write = f->callback.device_write; this_driver.callback.device_read = f->callback.device_read; this_driver.callback.msleep = f->callback.msleep; this_driver.callback.current_time = f->callback.current_time; yas_term(); return YAS_NO_ERROR; }