1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include <atomic.h> 18 #include <gpio.h> 19 #include <isr.h> 20 #include <nanohubPacket.h> 21 #include <plat/exti.h> 22 #include <plat/gpio.h> 23 #include <platform.h> 24 #include <plat/syscfg.h> 25 #include <plat/rtc.h> 26 #include <sensors.h> 27 #include <seos.h> 28 #include <slab.h> 29 #include <heap.h> 30 #include <i2c.h> 31 #include <timer.h> 32 #include <variant/sensType.h> 33 #include <cpu/cpuMath.h> 34 #include <floatRt.h> 35 36 #include <stdlib.h> 37 #include <string.h> 38 #include <variant/variant.h> 39 40 #define ST_ACC44_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 7) 41 42 /* Sensor registers */ 43 #define ST_ACC44_WAI_REG_ADDR 0x0F 44 #define ST_ACC44_WAI_REG_VAL 0x44 45 46 /* 47 * CTRL1 Register 48 * 49 * CTRL1[7:4] := ODR 50 * CTRL1[3:2] := MODE 51 * CTRL1[1:0] := LP_MODE 52 */ 53 #define ST_ACC44_CTRL1_REG 0x20 54 #define ST_ACC44_ODR_POWER_DOWN 0x00 55 #define ST_ACC44_ODR_12_5_HZ 0x20 56 #define ST_ACC44_ODR_25_HZ 0x30 57 #define ST_ACC44_ODR_50_HZ 0x40 58 #define ST_ACC44_ODR_100_HZ 0x50 59 #define ST_ACC44_ODR_200_HZ 0x60 60 #define ST_ACC44_ODR_400_HZ 0x70 61 #define ST_ACC44_ODR_800_HZ 0x80 62 #define ST_ACC44_ODR_1600_HZ 0x90 63 #define ST_ACC44_HIPERF_MODE 0x04 64 #define ST_ACC44_CTRL1_DEFVAL (ST_ACC44_HIPERF_MODE) 65 66 /* 67 * CTRL2 Register 68 * 69 * CTRL2[7] := BOOT 70 * CTRL2[6] := SOFT_RESET 71 * CTRL2[3] := BDU 72 */ 73 #define ST_ACC44_CTRL2_REG 0x21 74 #define ST_ACC44_CTRL2_BOOT 0x80 75 #define ST_ACC44_CTRL2_SW_RST 0x40 76 #define ST_ACC44_CTRL2_BDU 0x08 77 #define ST_ACC44_CTRL2_IF_ADD_INC 0x04 78 #define ST_ACC44_CTRL2_DEFVAL (ST_ACC44_CTRL2_BDU | ST_ACC44_CTRL2_IF_ADD_INC) 79 80 /* 81 * CTRL3 Register 82 */ 83 #define ST_ACC44_CTRL3_REG 0x22 84 #define ST_ACC44_CTRL3_LIR 0x10 85 86 /* 87 * CTRL4 Register 88 * 89 * CTRL4[7] := INT1_6D 90 * CTRL4[6] := INT1_SINGLE_TAP 91 * CTRL4[5] := INT1_WU 92 * CTRL4[4] := INT1_FF 93 * CTRL4[3] := INT1_TAP 94 * CTRL4[2] := INT1_DIFF5 95 * CTRL4[1] := INT1_FTH 96 * CTRL4[0] := INT1_DRDY 97 */ 98 #define ST_ACC44_CTRL4_REG 0x23 99 #define ST_ACC44_CTRL4_INT1_6D 0x80 100 #define ST_ACC44_CTRL4_INT1_STAP 0x40 101 #define ST_ACC44_CTRL4_INT1_WU 0x20 102 #define ST_ACC44_CTRL4_INT1_FF 0x10 103 #define ST_ACC44_CTRL4_INT1_DTAP 0x08 104 #define ST_ACC44_CTRL4_INT1_DIFF5 0x04 105 #define ST_ACC44_CTRL4_INT1_FTH 0x02 106 #define ST_ACC44_CTRL4_INT1_DRDY 0x01 107 108 /* 109 * CTRL5 Register 110 */ 111 #define ST_ACC44_CTRL5_REG 0x24 112 113 /* 114 * CTRL6 Register 115 * 116 * CTRL6[5:4] := FS 117 */ 118 #define ST_ACC44_CTRL6_REG 0x25 119 #define ST_ACC44_CTRL6_FS_2G 0x00 120 #define ST_ACC44_CTRL6_FS_4G 0x10 121 #define ST_ACC44_CTRL6_FS_8G 0x20 122 #define ST_ACC44_CTRL6_FS_16G 0x30 123 124 /* 125 * STATUS Register 126 */ 127 #define ST_ACC44_STATUS_REG_ADDR 0x27 128 #define ST_ACC44_STATUS_REG_FTH 0x80 129 #define ST_ACC44_STATUS_REG_DRDY 0x01 130 131 /* 132 * OUTXL Register 133 */ 134 #define ST_ACC44_OUTXL_REG_ADDR 0x28 135 136 /* 137 * value in m/s2 per LSB (in high-resolution mode @8g) 138 * Since samples are 14-bit left aligned, the value 139 * must also be right-shifted by 2. 140 * 141 * (9.80665 * 0.976) / (4 * 1000) 142 */ 143 #define ST_ACC44_KSCALE 0.0023928226 144 145 146 /* Enable auto-increment of the I2C subaddress (to allow I2C multiple ops) */ 147 #define ST_ACC44_I2C_AUTO_INCR 0x80 148 149 #define INFO_PRINT(fmt, ...) \ 150 do { \ 151 osLog(LOG_INFO, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \ 152 } while (0); 153 154 #define DEBUG_PRINT(fmt, ...) \ 155 do { \ 156 if (ST_ACC44_DBG_ENABLED) { \ 157 osLog(LOG_DEBUG, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \ 158 } \ 159 } while (0); 160 161 #define ERROR_PRINT(fmt, ...) \ 162 do { \ 163 osLog(LOG_ERROR, "%s " fmt, "[ST_ACC44]", ##__VA_ARGS__); \ 164 } while (0); 165 166 /* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */ 167 #ifndef ST_ACC44_DBG_ENABLED 168 #define ST_ACC44_DBG_ENABLED 0 169 #endif /* ST_ACC44_DBG_ENABLED */ 170 171 enum st_acc44_SensorEvents 172 { 173 EVT_COMM_DONE = EVT_APP_START + 1, 174 EVT_SENSOR_INTERRUPT, 175 }; 176 177 enum st_acc44_SensorState { 178 SENSOR_BOOT, 179 SENSOR_VERIFY_ID, 180 SENSOR_INIT, 181 SENSOR_IDLE, 182 SENSOR_ACCEL_POWER_UP, 183 SENSOR_ACCEL_POWER_DOWN, 184 SENSOR_CHANGE_RATE, 185 SENSOR_READ_SAMPLES, 186 }; 187 188 #ifndef ST_ACC44_I2C_BUS_ID 189 #error "ST_ACC44_I2C_BUS_ID is not defined; please define in variant.h" 190 #endif 191 192 #ifndef ST_ACC44_I2C_SPEED 193 #error "ST_ACC44_I2C_SPEED is not defined; please define in variant.h" 194 #endif 195 196 #ifndef ST_ACC44_I2C_ADDR 197 #error "ST_ACC44_I2C_ADDR is not defined; please define in variant.h" 198 #endif 199 200 #ifndef ST_ACC44_INT_PIN 201 #error "ST_ACC44_INT_PIN is not defined; please define in variant.h" 202 #endif 203 204 #ifndef ST_ACC44_INT_IRQ 205 #error "ST_ACC44_INT_IRQ is not defined; please define in variant.h" 206 #endif 207 208 #ifndef ST_ACC44_TO_ANDROID_COORDINATE 209 #error "ST_ACC44_TO_ANDROID_COORDINATE is not defined; please define in variant.h" 210 #endif 211 212 #define RAW_TO_MS2(raw_axis) ((float)raw_axis * ST_ACC44_KSCALE) 213 214 #define ST_ACC44_MAX_PENDING_I2C_REQUESTS 10 215 #define ST_ACC44_MAX_I2C_TRANSFER_SIZE 6 216 #define ST_ACC44_MAX_ACC_EVENTS 50 217 218 struct I2cTransfer 219 { 220 size_t tx; 221 size_t rx; 222 int err; 223 uint8_t txrxBuf[ST_ACC44_MAX_I2C_TRANSFER_SIZE]; 224 bool last; 225 bool inUse; 226 uint32_t delay; 227 }; 228 229 /* Task structure */ 230 struct st_acc44_Task { 231 uint32_t tid; 232 233 struct SlabAllocator *accDataSlab; 234 235 volatile uint8_t state; //task state, type enum st_mag40_SensorState, do NOT change this directly 236 bool accOn; 237 uint32_t sample_rate_ns; 238 uint32_t irq_rate_ns; 239 uint32_t rate; 240 uint32_t latency; 241 uint8_t currentODR; 242 uint8_t samplesToDiscard; 243 uint64_t Timestamp; 244 uint64_t lastTime; 245 246 bool pendingInt; 247 bool pendingSetPower; 248 bool pendingSetRate; 249 uint32_t pendingRate; 250 uint32_t pendingLatency; 251 bool pendingPower; 252 253 struct I2cTransfer transfers[ST_ACC44_MAX_PENDING_I2C_REQUESTS]; 254 255 /* Communication functions */ 256 bool (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, bool last); 257 bool (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, bool last); 258 259 /* irq */ 260 struct Gpio *Int1; 261 struct ChainedIsr Isr1; 262 uint32_t int_num; 263 264 /* sensors */ 265 uint32_t accHandle; 266 }; 267 268 static struct st_acc44_Task mTask; 269 270 #if DBG_STATE 271 #define PRI_STATE PRIi32 272 static int32_t getStateName(int32_t s) { 273 return s; 274 } 275 #endif 276 277 // Atomic get state 278 #define GET_STATE() (atomicReadByte(&mTask.state)) 279 280 // Atomic set state, this set the state to arbitrary value, use with caution 281 #define SET_STATE(s) do{\ 282 atomicWriteByte(&mTask.state, (s));\ 283 }while(0) 284 285 // Atomic switch state from IDLE to desired state. 286 static bool trySwitchState(enum st_acc44_SensorState newState) { 287 #if DBG_STATE 288 bool ret = atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState); 289 uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE(); 290 DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n", 291 getStateName(prevState), getStateName(newState), ret ? "ok" : "failed"); 292 return ret; 293 #else 294 return atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState); 295 #endif 296 } 297 298 #define DEC_INFO(name, type, axis, inter, samples, rates, raw, scale) \ 299 .sensorName = name, \ 300 .sensorType = type, \ 301 .numAxis = axis, \ 302 .interrupt = inter, \ 303 .minSamples = samples, \ 304 .supportedRates = rates, \ 305 .rawType = raw, \ 306 .rawScale = scale, 307 308 static uint32_t st_acc44_Rates[] = { 309 SENSOR_HZ(25.0f/2.0f), 310 SENSOR_HZ(25.0f), 311 SENSOR_HZ(50.0f), 312 SENSOR_HZ(100.0f), 313 SENSOR_HZ(200.0f), 314 SENSOR_HZ(400.0f), 315 SENSOR_HZ(800.0f), 316 0 317 }; 318 319 static uint32_t st_acc44_Rates_in_ns[] = { 320 80000000, /* 12.5 Hz */ 321 40000000, /* 25 Hz */ 322 20000000, /* 50 Hz */ 323 10000000, /* 100 Hz */ 324 5000000, /* 200 Hz */ 325 2500000, /* 400 Hz */ 326 1250000, /* 800 Hz */ 327 0 328 }; 329 330 static uint32_t st_acc44_regVal[] = { 331 ST_ACC44_ODR_12_5_HZ, 332 ST_ACC44_ODR_25_HZ, 333 ST_ACC44_ODR_50_HZ, 334 ST_ACC44_ODR_100_HZ, 335 ST_ACC44_ODR_200_HZ, 336 ST_ACC44_ODR_400_HZ, 337 ST_ACC44_ODR_800_HZ, 338 ST_ACC44_ODR_1600_HZ, 339 }; 340 341 static uint8_t st_acc44_computeOdr(uint32_t rate) 342 { 343 int i; 344 345 for (i = 0; i < (ARRAY_SIZE(st_acc44_Rates) - 1); i++) { 346 if (st_acc44_Rates[i] == rate) 347 break; 348 } 349 if (i == (ARRAY_SIZE(st_acc44_Rates) -1 )) { 350 ERROR_PRINT("ODR not valid! Choosed smallest ODR available\n"); 351 i = 0; 352 } 353 354 return i; 355 } 356 357 static uint32_t st_acc44_Rate_hz_to_ns(uint32_t rate) 358 { 359 int i; 360 361 if ((i = st_acc44_computeOdr(rate)) >= 0) 362 return st_acc44_Rates_in_ns[i]; 363 364 return 0; 365 } 366 367 static const struct SensorInfo st_acc44_SensorInfo = 368 { 369 DEC_INFO("Accelerometer", SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 370 600, st_acc44_Rates, SENS_TYPE_ACCEL_RAW, 1.0f / ST_ACC44_KSCALE) 371 }; 372 373 static bool st_acc44_Power(bool on, void *cookie) 374 { 375 bool oldMode = mTask.accOn; 376 bool newMode = on; 377 uint32_t state = on ? SENSOR_ACCEL_POWER_UP : SENSOR_ACCEL_POWER_DOWN; 378 bool ret = true; 379 380 INFO_PRINT("Power %s\n", on ? "on" : "off"); 381 382 if (trySwitchState(state)) { 383 if (oldMode != newMode) { 384 if (on) { 385 ret = mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_12_5_HZ | 386 ST_ACC44_CTRL1_DEFVAL, 0, true); 387 } else { 388 ret = mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_POWER_DOWN | 389 ST_ACC44_CTRL1_DEFVAL, 0, true); 390 } 391 } else 392 sensorSignalInternalEvt(mTask.accHandle, 393 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 394 } else { 395 mTask.pendingSetPower = true; 396 mTask.pendingPower = on; 397 } 398 399 return ret; 400 } 401 402 static bool st_acc44_FwUpload(void *cookie) 403 { 404 INFO_PRINT("FwUpload\n"); 405 return sensorSignalInternalEvt(mTask.accHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 406 } 407 408 static bool st_acc44_SetRate(uint32_t rate, uint64_t latency, void *cookie) 409 { 410 uint8_t num = 0; 411 412 INFO_PRINT("SetRate %lu Hz - %llu ns\n", rate, latency); 413 414 if (trySwitchState(SENSOR_CHANGE_RATE)) { 415 num = st_acc44_computeOdr(rate); 416 mTask.currentODR = st_acc44_regVal[num]; 417 mTask.latency = latency; 418 mTask.rate = rate; 419 mTask.sample_rate_ns = st_acc44_Rate_hz_to_ns(rate); 420 mTask.samplesToDiscard = 2; 421 mTask.lastTime = 0; 422 423 /* one interrupt every sample */ 424 mTask.irq_rate_ns = mTask.sample_rate_ns; 425 426 mTask.comm_rx(ST_ACC44_OUTXL_REG_ADDR, 6, 0, false); 427 mTask.comm_tx(ST_ACC44_CTRL4_REG, ST_ACC44_CTRL4_INT1_DRDY, 0, false); 428 mTask.comm_tx(ST_ACC44_CTRL1_REG, mTask.currentODR | ST_ACC44_CTRL1_DEFVAL, 0, true); 429 } else { 430 mTask.pendingSetRate = true; 431 mTask.pendingRate = rate; 432 mTask.pendingLatency = latency; 433 } 434 435 return true; 436 } 437 438 static bool st_acc44_Flush(void *cookie) 439 { 440 INFO_PRINT("Flush\n"); 441 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL); 442 } 443 444 static bool st_acc44_SelfTest(void *cookie) 445 { 446 INFO_PRINT("SelfTest\n"); 447 return true; 448 } 449 450 #define DEC_OPS(power, firmware, rate, flush, test, cal, cfg) \ 451 .sensorPower = power, \ 452 .sensorFirmwareUpload = firmware, \ 453 .sensorSetRate = rate, \ 454 .sensorFlush = flush, \ 455 .sensorCalibrate = cal, \ 456 .sensorSelfTest = test, \ 457 .sensorCfgData = cfg 458 459 static const struct SensorOps st_acc44_SensorOps = 460 { 461 DEC_OPS(st_acc44_Power, st_acc44_FwUpload, st_acc44_SetRate, st_acc44_Flush, st_acc44_SelfTest, NULL, NULL), 462 }; 463 464 static void inline enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 465 { 466 gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE); 467 syscfgSetExtiPort(pin); 468 extiEnableIntGpio(pin, EXTI_TRIGGER_RISING); 469 extiChainIsr(ST_ACC44_INT_IRQ, isr); 470 } 471 472 static void inline disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 473 { 474 extiUnchainIsr(ST_ACC44_INT_IRQ, isr); 475 extiDisableIntGpio(pin); 476 } 477 478 static void st_acc44_calc_timestamp(void) 479 { 480 if (mTask.lastTime == 0) { 481 mTask.Timestamp = sensorGetTime(); 482 } else { 483 uint64_t currTime = sensorGetTime(); 484 uint64_t deltaTime = currTime - mTask.lastTime; 485 486 deltaTime = (deltaTime + 7*mTask.irq_rate_ns)/8; 487 mTask.Timestamp = mTask.lastTime + deltaTime; 488 } 489 mTask.lastTime = mTask.Timestamp; 490 } 491 492 static bool st_acc44_int1_isr(struct ChainedIsr *isr) 493 { 494 if (!extiIsPendingGpio(mTask.Int1)) 495 return false; 496 497 /* Start sampling for a value */ 498 if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT, NULL, NULL, mTask.tid)) 499 ERROR_PRINT("st_acc44_int1_isr: osEnqueuePrivateEvt() failed\n"); 500 501 mTask.int_num++; 502 extiClearPendingGpio(mTask.Int1); 503 return true; 504 } 505 506 static void int2Evt(void) 507 { 508 if (trySwitchState(SENSOR_READ_SAMPLES)) { 509 mTask.comm_rx(ST_ACC44_OUTXL_REG_ADDR, 6, 0, true); 510 } else { 511 mTask.pendingInt = true; 512 } 513 } 514 515 static void processPendingEvt(void) 516 { 517 if (mTask.pendingInt) { 518 mTask.pendingInt = false; 519 int2Evt(); 520 return; 521 } 522 523 if (mTask.pendingSetPower) { 524 mTask.pendingSetPower = false; 525 st_acc44_Power(mTask.pendingPower, NULL); 526 } 527 528 if (mTask.pendingSetRate) { 529 mTask.pendingSetRate = false; 530 st_acc44_SetRate(mTask.pendingRate, mTask.pendingLatency, NULL); 531 } 532 } 533 534 static bool accAllocateEvt(struct TripleAxisDataEvent **evPtr) 535 { 536 struct TripleAxisDataEvent *ev; 537 538 ev = *evPtr = slabAllocatorAlloc(mTask.accDataSlab); 539 if (!ev) { 540 ERROR_PRINT("Failed to allocate acc event memory"); 541 return false; 542 } 543 544 memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); 545 return true; 546 } 547 548 static void accFreeEvt(void *ptr) 549 { 550 slabAllocatorFree(mTask.accDataSlab, ptr); 551 } 552 553 // Allocate a buffer and mark it as in use with the given state, or return NULL 554 // if no buffers available. Must *not* be called from interrupt context. 555 static struct I2cTransfer *allocXfer(void) 556 { 557 size_t i; 558 559 for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) { 560 if (!mTask.transfers[i].inUse) { 561 mTask.transfers[i].inUse = true; 562 return &mTask.transfers[i]; 563 } 564 } 565 566 ERROR_PRINT("Ran out of i2c buffers!"); 567 return NULL; 568 } 569 570 static inline void releaseXfer(struct I2cTransfer *xfer) 571 { 572 xfer->inUse = false; 573 } 574 575 static void st_acc44_i2cCallback(void *cookie, size_t tx, size_t rx, int err) 576 { 577 struct I2cTransfer *xfer = cookie; 578 579 /* Do not run callback if not the last one in a set of i2c transfers */ 580 if (xfer && !xfer->last) { 581 releaseXfer(xfer); 582 return; 583 } 584 585 xfer->tx = tx; 586 xfer->rx = rx; 587 xfer->err = err; 588 589 osEnqueuePrivateEvt(EVT_COMM_DONE, cookie, NULL, mTask.tid); 590 if (err != 0) 591 ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err); 592 } 593 594 static bool st_acc44_i2c_read(uint8_t addr, uint16_t len, uint32_t delay, bool last) 595 { 596 struct I2cTransfer *xfer = allocXfer(); 597 int ret = -1; 598 599 if (xfer != NULL) { 600 xfer->delay = delay; 601 xfer->last = last; 602 xfer->txrxBuf[0] = ST_ACC44_I2C_AUTO_INCR | addr; 603 if ((ret = i2cMasterTxRx(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, st_acc44_i2cCallback, xfer)) < 0) { 604 releaseXfer(xfer); 605 DEBUG_PRINT("st_acc44_i2c_read: i2cMasterTxRx operation failed (ret: %d)\n", ret); 606 return false; 607 } 608 } 609 610 return (ret == -1) ? false : true; 611 } 612 613 static bool st_acc44_i2c_write(uint8_t addr, uint8_t data, uint32_t delay, bool last) 614 { 615 struct I2cTransfer *xfer = allocXfer(); 616 int ret = -1; 617 618 if (xfer != NULL) { 619 xfer->delay = delay; 620 xfer->last = last; 621 xfer->txrxBuf[0] = addr; 622 xfer->txrxBuf[1] = data; 623 if ((ret = i2cMasterTx(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_ADDR, xfer->txrxBuf, 2, st_acc44_i2cCallback, xfer)) < 0) { 624 releaseXfer(xfer); 625 DEBUG_PRINT("st_acc44_i2c_write: i2cMasterTx operation failed (ret: %d)\n", ret); 626 return false; 627 } 628 } 629 630 return (ret == -1) ? false : true; 631 } 632 633 static void parseRawData(uint8_t *raw, uint8_t num_of_smpl, uint64_t sensor_time) 634 { 635 uint8_t i; 636 struct TripleAxisDataEvent *accSample; 637 float x, y, z; 638 int32_t raw_x; 639 int32_t raw_y; 640 int32_t raw_z; 641 642 /* Discard samples generated during sensor turn-on time */ 643 if (mTask.samplesToDiscard > 0) { 644 if (num_of_smpl > mTask.samplesToDiscard) { 645 num_of_smpl -= mTask.samplesToDiscard; 646 mTask.samplesToDiscard = 0; 647 } else{ 648 mTask.samplesToDiscard -= num_of_smpl; 649 return; 650 } 651 } 652 653 if (accAllocateEvt(&accSample) == false) 654 return; 655 656 accSample->referenceTime = sensor_time; 657 658 accSample->samples[0].deltaTime = 0; 659 accSample->samples[0].firstSample.numSamples = num_of_smpl; 660 661 for (i = 0; i < num_of_smpl; i++) { 662 raw_x = (*(int16_t *)&raw[6*i + 0]); 663 raw_y = (*(int16_t *)&raw[6*i + 2]); 664 raw_z = (*(int16_t *)&raw[6*i + 4]); 665 666 /* convert raw data in m/s2 */ 667 x = RAW_TO_MS2(raw_x); 668 y = RAW_TO_MS2(raw_y); 669 z = RAW_TO_MS2(raw_z); 670 671 /* rotate axis */ 672 ST_ACC44_TO_ANDROID_COORDINATE(x, y, z); 673 674 accSample->samples[i].x = x; 675 accSample->samples[i].y = y; 676 accSample->samples[i].z = z; 677 678 if (i > 0) 679 accSample->samples[i].deltaTime = mTask.sample_rate_ns; 680 } 681 682 osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_ACCEL), accSample, accFreeEvt); 683 } 684 685 static int st_acc44_handleCommDoneEvt(const void* evtData) 686 { 687 bool returnIdle = false; 688 struct I2cTransfer *xfer = (struct I2cTransfer *)evtData; 689 690 switch (GET_STATE()) { 691 case SENSOR_BOOT: 692 SET_STATE(SENSOR_VERIFY_ID); 693 if (!mTask.comm_rx(ST_ACC44_WAI_REG_ADDR, 1, 0, true)) { 694 DEBUG_PRINT("Not able to read WAI\n"); 695 return -1; 696 } 697 break; 698 699 case SENSOR_VERIFY_ID: 700 /* Check the sensor ID */ 701 if (xfer->err != 0 || xfer->txrxBuf[0] != ST_ACC44_WAI_REG_VAL) { 702 DEBUG_PRINT("WAI returned is: %02x\n", xfer->txrxBuf[0]); 703 break; 704 } 705 706 INFO_PRINT("Device ID is correct! (%02x)\n", xfer->txrxBuf[0]); 707 708 SET_STATE(SENSOR_INIT); 709 mTask.comm_tx(ST_ACC44_CTRL1_REG, ST_ACC44_ODR_POWER_DOWN | ST_ACC44_CTRL1_DEFVAL, 0, false); 710 mTask.comm_tx(ST_ACC44_CTRL2_REG, ST_ACC44_CTRL2_DEFVAL, 0, false); 711 mTask.comm_tx(ST_ACC44_CTRL3_REG, ST_ACC44_CTRL3_LIR, 0, false); 712 mTask.comm_tx(ST_ACC44_CTRL6_REG, ST_ACC44_CTRL6_FS_8G, 0, false); 713 mTask.comm_tx(ST_ACC44_CTRL4_REG, 0, 0, true); 714 break; 715 716 case SENSOR_INIT: 717 DEBUG_PRINT("SENSOR INIT\n"); 718 returnIdle = true; 719 sensorRegisterInitComplete(mTask.accHandle); 720 break; 721 722 case SENSOR_ACCEL_POWER_UP: 723 DEBUG_PRINT("POWER UP\n"); 724 returnIdle = true; 725 mTask.accOn = true; 726 sensorSignalInternalEvt(mTask.accHandle, 727 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); 728 break; 729 730 case SENSOR_ACCEL_POWER_DOWN: 731 DEBUG_PRINT("POWER DWN\n"); 732 returnIdle = true; 733 mTask.accOn = false; 734 sensorSignalInternalEvt(mTask.accHandle, 735 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0); 736 break; 737 738 case SENSOR_CHANGE_RATE: 739 DEBUG_PRINT("CHANGE RATE\n"); 740 returnIdle = true; 741 DEBUG_PRINT("int_num %ld\n", mTask.int_num); 742 mTask.int_num = 0; 743 sensorSignalInternalEvt(mTask.accHandle, 744 SENSOR_INTERNAL_EVT_RATE_CHG, mTask.rate, mTask.latency); 745 break; 746 747 case SENSOR_READ_SAMPLES: 748 returnIdle = true; 749 750 parseRawData(&xfer->txrxBuf[0], 1, mTask.Timestamp); 751 break; 752 753 case SENSOR_IDLE: 754 default: 755 break; 756 } 757 758 releaseXfer(xfer); 759 760 if (returnIdle) { 761 SET_STATE(SENSOR_IDLE); 762 processPendingEvt(); 763 } 764 765 return (0); 766 } 767 768 static void st_acc44_handleEvent(uint32_t evtType, const void* evtData) 769 { 770 switch (evtType) { 771 case EVT_APP_START: 772 INFO_PRINT("EVT_APP_START\n"); 773 osEventUnsubscribe(mTask.tid, EVT_APP_START); 774 775 SET_STATE(SENSOR_BOOT); 776 mTask.comm_tx(ST_ACC44_CTRL2_REG, ST_ACC44_CTRL2_SW_RST, 0, true); 777 break; 778 779 case EVT_COMM_DONE: 780 st_acc44_handleCommDoneEvt(evtData); 781 break; 782 783 case EVT_SENSOR_INTERRUPT: 784 st_acc44_calc_timestamp(); 785 int2Evt(); 786 break; 787 788 default: 789 break; 790 } 791 } 792 793 static bool st_acc44_startTask(uint32_t task_id) 794 { 795 size_t slabSize; 796 797 mTask.tid = task_id; 798 799 INFO_PRINT("start driver\n"); 800 801 mTask.accOn = false; 802 mTask.pendingInt = false; 803 mTask.pendingSetPower = false; 804 mTask.pendingSetRate = false; 805 806 mTask.currentODR = ST_ACC44_ODR_POWER_DOWN; 807 808 slabSize = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint); 809 810 mTask.accDataSlab = slabAllocatorNew(slabSize, 4, ST_ACC44_MAX_ACC_EVENTS); 811 if (!mTask.accDataSlab) { 812 ERROR_PRINT("Failed to allocate accDataSlab memory\n"); 813 return false; 814 } 815 816 /* Init the communication part */ 817 i2cMasterRequest(ST_ACC44_I2C_BUS_ID, ST_ACC44_I2C_SPEED); 818 819 mTask.comm_tx = st_acc44_i2c_write; 820 mTask.comm_rx = st_acc44_i2c_read; 821 822 /* irq */ 823 mTask.int_num = 0; 824 mTask.Int1 = gpioRequest(ST_ACC44_INT_PIN); 825 gpioConfigInput(mTask.Int1, GPIO_SPEED_LOW, GPIO_PULL_NONE); 826 mTask.Isr1.func = st_acc44_int1_isr; 827 enableInterrupt(mTask.Int1, &mTask.Isr1); 828 829 mTask.accHandle = sensorRegister(&st_acc44_SensorInfo, &st_acc44_SensorOps, NULL, false); 830 831 osEventSubscribe(mTask.tid, EVT_APP_START); 832 833 return true; 834 } 835 836 static void st_acc44_endTask(void) 837 { 838 INFO_PRINT("ended\n"); 839 slabAllocatorDestroy(mTask.accDataSlab); 840 disableInterrupt(mTask.Int1, &mTask.Isr1); 841 } 842 843 INTERNAL_APP_INIT(ST_ACC44_APP_ID, 0, st_acc44_startTask, st_acc44_endTask, st_acc44_handleEvent); 844
