1 /* 2 * Copyright (C) 2016 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 <halIntf.h> 29 #include <slab.h> 30 #include <heap.h> 31 #include <i2c.h> 32 #include <timer.h> 33 #include <variant/sensType.h> 34 #include <cpu/cpuMath.h> 35 #include <calibration/magnetometer/mag_cal.h> 36 #include <floatRt.h> 37 38 #include <stdlib.h> 39 #include <string.h> 40 #include <variant/variant.h> 41 42 #define ST_MAG40_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 3) 43 44 /* Sensor registers */ 45 #define ST_MAG40_WAI_REG_ADDR 0x4F 46 #define ST_MAG40_WAI_REG_VAL 0x40 47 48 #define ST_MAG40_CFG_A_REG_ADDR 0x60 49 #define ST_MAG40_TEMP_COMP_EN 0x80 50 #define ST_MAG40_SOFT_RESET_BIT 0x20 51 #define ST_MAG40_ODR_10_HZ 0x00 52 #define ST_MAG40_ODR_20_HZ 0x04 53 #define ST_MAG40_ODR_50_HZ 0x08 54 #define ST_MAG40_ODR_100_HZ 0x0C 55 #define ST_MAG40_POWER_ON 0x00 56 #define ST_MAG40_POWER_IDLE 0x03 57 58 #define ST_MAG40_CFG_B_REG_ADDR 0x61 59 #define ST_MAG40_OFF_CANC 0x02 60 61 #define ST_MAG40_CFG_C_REG_ADDR 0x62 62 #define ST_MAG40_I2C_DIS 0x20 63 #define ST_MAG40_BDU_ON 0x10 64 #define ST_MAG40_SELFTEST_EN 0x02 65 #define ST_MAG40_INT_MAG 0x01 66 67 #define ST_MAG40_OUTXL_REG_ADDR 0x68 68 69 /* Enable auto-increment of the I2C subaddress (to allow I2C multiple ops) */ 70 #define ST_MAG40_I2C_AUTO_INCR 0x80 71 72 enum st_mag40_SensorEvents 73 { 74 EVT_COMM_DONE = EVT_APP_START + 1, 75 EVT_SENSOR_INTERRUPT, 76 }; 77 78 enum st_mag40_TestState { 79 MAG_SELFTEST_INIT, 80 MAG_SELFTEST_RUN_ST_OFF, 81 MAG_SELFTEST_INIT_ST_EN, 82 MAG_SELFTEST_RUN_ST_ON, 83 MAG_SELFTEST_VERIFY, 84 MAG_SELFTEST_DONE, 85 }; 86 87 enum st_mag40_SensorState { 88 SENSOR_BOOT, 89 SENSOR_VERIFY_ID, 90 SENSOR_INITIALIZATION, 91 SENSOR_IDLE, 92 SENSOR_MAG_CONFIGURATION, 93 SENSOR_READ_SAMPLES, 94 SENSOR_SELF_TEST, 95 }; 96 97 enum st_mag40_subState { 98 NO_SUBSTATE = 0, 99 100 INIT_START, 101 INIT_ENABLE_DRDY, 102 INIT_I2C_DISABLE_ACCEL, 103 INIT_DONE, 104 105 CONFIG_POWER_UP, 106 CONFIG_POWER_UP_2, 107 108 CONFIG_POWER_DOWN, 109 CONFIG_POWER_DOWN_2, 110 111 CONFIG_SET_RATE, 112 CONFIG_SET_RATE_2, 113 114 CONFIG_DONE, 115 }; 116 117 struct TestResultData { 118 struct HostHubRawPacket header; 119 struct SensorAppEventHeader data_header; 120 } __attribute__((packed)); 121 122 #ifndef ST_MAG40_I2C_BUS_ID 123 #error "ST_MAG40_I2C_BUS_ID is not defined; please define in variant.h" 124 #endif 125 126 #ifndef ST_MAG40_I2C_SPEED 127 #error "ST_MAG40_I2C_SPEED is not defined; please define in variant.h" 128 #endif 129 130 #ifndef ST_MAG40_I2C_ADDR 131 #error "ST_MAG40_I2C_ADDR is not defined; please define in variant.h" 132 #endif 133 134 #ifndef ST_MAG40_INT_PIN 135 #error "ST_MAG40_INT_PIN is not defined; please define in variant.h" 136 #endif 137 138 #ifndef ST_MAG40_INT_IRQ 139 #error "ST_MAG40_INT_IRQ is not defined; please define in variant.h" 140 #endif 141 142 #ifndef ST_MAG40_ROT_MATRIX 143 #error "ST_MAG40_ROT_MATRIX is not defined; please define in variant.h" 144 #endif 145 146 #define ST_MAG40_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \ 147 ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \ 148 (r12 == 1 ? y : (r12 == -1 ? -y : 0)) + \ 149 (r13 == 1 ? z : (r13 == -1 ? -z : 0))) 150 151 #define ST_MAG40_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \ 152 ((r21 == 1 ? x : (r21 == -1 ? -x : 0)) + \ 153 (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \ 154 (r23 == 1 ? z : (r23 == -1 ? -z : 0))) 155 156 #define ST_MAG40_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \ 157 ((r31 == 1 ? x : (r31 == -1 ? -x : 0)) + \ 158 (r32 == 1 ? y : (r32 == -1 ? -y : 0)) + \ 159 (r33 == 1 ? z : (r33 == -1 ? -z : 0))) 160 161 #define ST_MAG40_REMAP_X_DATA(...) ST_MAG40_X_MAP(__VA_ARGS__) 162 #define ST_MAG40_REMAP_Y_DATA(...) ST_MAG40_Y_MAP(__VA_ARGS__) 163 #define ST_MAG40_REMAP_Z_DATA(...) ST_MAG40_Z_MAP(__VA_ARGS__) 164 165 /* Self Test macros */ 166 #define ST_MAG40_ST_NUM_OF_SAMPLES 50 167 #define ST_MAG40_ST_MIN_THRESHOLD 10 /* 15 mGa */ 168 #define ST_MAG40_ST_MAX_THRESHOLD 333 /* 500 mGa */ 169 170 #define INFO_PRINT(fmt, ...) \ 171 do { \ 172 osLog(LOG_INFO, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \ 173 } while (0); 174 175 #define DEBUG_PRINT(fmt, ...) \ 176 do { \ 177 if (ST_MAG40_DBG_ENABLED) { \ 178 osLog(LOG_DEBUG, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \ 179 } \ 180 } while (0); 181 182 #define ERROR_PRINT(fmt, ...) \ 183 do { \ 184 osLog(LOG_ERROR, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \ 185 } while (0); 186 187 /* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */ 188 #ifndef ST_MAG40_DBG_ENABLED 189 #define ST_MAG40_DBG_ENABLED 0 190 #endif /* ST_MAG40_DBG_ENABLED */ 191 192 #define ST_MAG40_MAX_PENDING_I2C_REQUESTS 4 193 #define ST_MAG40_MAX_I2C_TRANSFER_SIZE 6 194 #define ST_MAG40_MAX_MAG_EVENTS 20 195 196 struct I2cTransfer 197 { 198 size_t tx; 199 size_t rx; 200 int err; 201 uint8_t txrxBuf[ST_MAG40_MAX_I2C_TRANSFER_SIZE]; 202 bool last; 203 bool inUse; 204 uint32_t delay; 205 }; 206 207 /* Task structure */ 208 struct st_mag40_Task { 209 uint32_t tid; 210 211 struct SlabAllocator *magDataSlab; 212 213 uint64_t timestampInt; 214 215 volatile uint8_t state; //task state, type enum st_mag40_SensorState, do NOT change this directly 216 uint8_t subState; 217 218 /* sensor flags */ 219 uint8_t samplesToDiscard; 220 uint32_t rate; 221 uint64_t latency; 222 bool magOn; 223 bool pendingInt; 224 uint8_t pendingSubState; 225 226 uint8_t currentODR; 227 228 #if defined(ST_MAG40_CAL_ENABLED) 229 struct MagCal moc; 230 #endif 231 232 unsigned char sens_buf[7]; 233 234 struct I2cTransfer transfers[ST_MAG40_MAX_PENDING_I2C_REQUESTS]; 235 236 /* Communication functions */ 237 void (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, bool last); 238 void (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, bool last); 239 240 /* irq */ 241 struct Gpio *Int1; 242 struct ChainedIsr Isr1; 243 244 /* Self Test */ 245 enum st_mag40_TestState mag_test_state; 246 uint32_t mag_selftest_num; 247 int32_t dataST[3]; 248 int32_t dataNOST[3]; 249 250 /* sensors */ 251 uint32_t magHandle; 252 }; 253 254 static struct st_mag40_Task mTask; 255 256 static void sensorMagConfig(void); 257 258 #define PRI_STATE PRIi32 259 static int32_t getStateName(int32_t s) { 260 return s; 261 } 262 263 // Atomic get state 264 #define GET_STATE() (atomicReadByte(&mTask.state)) 265 266 // Atomic set state, this set the state to arbitrary value, use with caution 267 #define SET_STATE(s) do{\ 268 DEBUG_PRINT("set state %" PRI_STATE "\n", getStateName(s));\ 269 atomicWriteByte(&mTask.state, (s));\ 270 }while(0) 271 272 // Atomic switch state from IDLE to desired state. 273 static bool trySwitchState(enum st_mag40_SensorState newState) { 274 #if DBG_STATE 275 bool ret = atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState); 276 uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE(); 277 DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n", 278 getStateName(prevState), getStateName(newState), ret ? "ok" : "failed"); 279 return ret; 280 #else 281 return atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState); 282 #endif 283 } 284 285 static bool magAllocateEvt(struct TripleAxisDataEvent **evPtr) 286 { 287 struct TripleAxisDataEvent *ev; 288 289 ev = *evPtr = slabAllocatorAlloc(mTask.magDataSlab); 290 if (!ev) { 291 ERROR_PRINT("Failed to allocate mag event memory"); 292 return false; 293 } 294 295 memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); 296 return true; 297 } 298 299 static void magFreeEvt(void *ptr) 300 { 301 slabAllocatorFree(mTask.magDataSlab, ptr); 302 } 303 304 // Allocate a buffer and mark it as in use with the given state, or return NULL 305 // if no buffers available. Must *not* be called from interrupt context. 306 static struct I2cTransfer *allocXfer(void) 307 { 308 size_t i; 309 310 for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) { 311 if (!mTask.transfers[i].inUse) { 312 mTask.transfers[i].inUse = true; 313 return &mTask.transfers[i]; 314 } 315 } 316 317 ERROR_PRINT("Ran out of i2c buffers!"); 318 return NULL; 319 } 320 321 static inline void releaseXfer(struct I2cTransfer *xfer) 322 { 323 xfer->inUse = false; 324 } 325 326 static void i2cCallback(void *cookie, size_t tx, size_t rx, int err); 327 328 /* delayed callback */ 329 static void i2cDelayCallback(uint32_t timerId, void *data) 330 { 331 struct I2cTransfer *xfer = data; 332 333 i2cCallback((void *)xfer, xfer->tx, xfer->rx, xfer->err); 334 } 335 336 static void i2cCallback(void *cookie, size_t tx, size_t rx, int err) 337 { 338 struct I2cTransfer *xfer = cookie; 339 340 /* Do not run callback if not the last one in a set of i2c transfers */ 341 if (xfer && !xfer->last) { 342 releaseXfer(xfer); 343 return; 344 } 345 346 /* delay callback if it is the case */ 347 if (xfer->delay > 0) { 348 xfer->tx = tx; 349 xfer->rx = rx; 350 xfer->err = err; 351 352 if (!timTimerSet(xfer->delay * 1000, 0, 50, i2cDelayCallback, xfer, true)) { 353 ERROR_PRINT("Cannot do delayed i2cCallback\n"); 354 goto handle_now; 355 } 356 357 xfer->delay = 0; 358 return; 359 } 360 361 handle_now: 362 xfer->tx = tx; 363 xfer->rx = rx; 364 xfer->err = err; 365 366 osEnqueuePrivateEvt(EVT_COMM_DONE, cookie, NULL, mTask.tid); 367 if (err != 0) 368 ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err); 369 } 370 371 static void i2c_read(uint8_t addr, uint16_t len, uint32_t delay, bool last) 372 { 373 struct I2cTransfer *xfer = allocXfer(); 374 375 if (xfer != NULL) { 376 xfer->delay = delay; 377 xfer->last = last; 378 xfer->txrxBuf[0] = ST_MAG40_I2C_AUTO_INCR | addr; 379 i2cMasterTxRx(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, i2cCallback, xfer); 380 } 381 } 382 383 static void i2c_write(uint8_t addr, uint8_t data, uint32_t delay, bool last) 384 { 385 struct I2cTransfer *xfer = allocXfer(); 386 387 if (xfer != NULL) { 388 xfer->delay = delay; 389 xfer->last = last; 390 xfer->txrxBuf[0] = addr; 391 xfer->txrxBuf[1] = data; 392 i2cMasterTx(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer); 393 } 394 } 395 396 #define DEC_INFO_BIAS(name, type, axis, inter, samples, rates, raw, scale, bias) \ 397 .sensorName = name, \ 398 .sensorType = type, \ 399 .numAxis = axis, \ 400 .interrupt = inter, \ 401 .minSamples = samples, \ 402 .supportedRates = rates, \ 403 .rawType = raw, \ 404 .rawScale = scale, \ 405 .biasType = bias 406 407 #define DEC_INFO(name, type, axis, inter, samples, rates, raw, scale) \ 408 .sensorName = name, \ 409 .sensorType = type, \ 410 .numAxis = axis, \ 411 .interrupt = inter, \ 412 .minSamples = samples, \ 413 .supportedRates = rates, \ 414 .rawType = raw, \ 415 .rawScale = scale, 416 417 static uint32_t st_mag40_Rates[] = { 418 SENSOR_HZ(10.0f), 419 SENSOR_HZ(20.0f), 420 SENSOR_HZ(50.0f), 421 SENSOR_HZ(100.0f), 422 0 423 }; 424 425 static uint32_t st_mag40_regVal[] = { 426 ST_MAG40_ODR_10_HZ, 427 ST_MAG40_ODR_20_HZ, 428 ST_MAG40_ODR_50_HZ, 429 ST_MAG40_ODR_100_HZ, 430 }; 431 432 static uint8_t st_mag40_computeOdr(uint32_t rate) 433 { 434 int i; 435 436 for (i = 0; i < (ARRAY_SIZE(st_mag40_Rates) - 1); i++) { 437 if (st_mag40_Rates[i] == rate) 438 break; 439 } 440 if (i == (ARRAY_SIZE(st_mag40_Rates) -1 )) { 441 ERROR_PRINT("ODR not valid! Choosed smallest ODR available\n"); 442 i = 0; 443 } 444 445 return i; 446 } 447 448 449 static const struct SensorInfo st_mag40_SensorInfo = 450 { 451 #if defined(ST_MAG40_CAL_ENABLED) 452 DEC_INFO_BIAS("Magnetometer", SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 453 600, st_mag40_Rates, 0, 0, SENS_TYPE_MAG_BIAS) 454 #else 455 DEC_INFO("Magnetometer", SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 456 600, st_mag40_Rates, 0, 0) 457 #endif 458 }; 459 460 /* Sensor Operations */ 461 static bool magPower(bool on, void *cookie) 462 { 463 INFO_PRINT("magPower %s\n", on ? "on" : "off"); 464 if (trySwitchState(SENSOR_MAG_CONFIGURATION)) { 465 mTask.subState = on ? CONFIG_POWER_UP : CONFIG_POWER_DOWN; 466 sensorMagConfig(); 467 } else { 468 mTask.pendingSubState = on ? CONFIG_POWER_UP : CONFIG_POWER_DOWN; 469 } 470 471 return true; 472 } 473 474 static bool magFwUpload(void *cookie) 475 { 476 return sensorSignalInternalEvt(mTask.magHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 477 } 478 479 static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie) 480 { 481 uint8_t num = 0; 482 483 INFO_PRINT("magSetRate %lu Hz - %llu ns\n", rate, latency); 484 485 num = st_mag40_computeOdr(rate); 486 mTask.currentODR = st_mag40_regVal[num]; 487 mTask.rate = rate; 488 mTask.latency = latency; 489 mTask.samplesToDiscard = 2; 490 491 if (trySwitchState(SENSOR_MAG_CONFIGURATION)) { 492 mTask.subState = CONFIG_SET_RATE; 493 sensorMagConfig(); 494 } else { 495 mTask.pendingSubState = CONFIG_SET_RATE; 496 } 497 498 return true; 499 } 500 501 static bool magFlush(void *cookie) 502 { 503 INFO_PRINT("magFlush\n"); 504 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL); 505 } 506 507 static bool magCfgData(void *data, void *cookie) 508 { 509 #if defined(ST_MAG40_CAL_ENABLED) 510 const struct AppToSensorHalDataPayload *p = data; 511 512 if (p->type == HALINTF_TYPE_MAG_CAL_BIAS && p->size == sizeof(struct MagCalBias)) { 513 const struct MagCalBias *d = p->magCalBias; 514 INFO_PRINT("magCfgData: calibration %ldnT, %ldnT, %ldnT\n", 515 (int32_t)(d->bias[0] * 1000), 516 (int32_t)(d->bias[1] * 1000), 517 (int32_t)(d->bias[2] * 1000)); 518 519 mTask.moc.x_bias = d->bias[0]; 520 mTask.moc.y_bias = d->bias[1]; 521 mTask.moc.z_bias = d->bias[2]; 522 } else if (p->type == HALINTF_TYPE_MAG_LOCAL_FIELD && p->size == sizeof(struct MagLocalField)) { 523 const struct MagLocalField *d = p->magLocalField; 524 INFO_PRINT("magCfgData: local field strength %dnT, dec %ddeg, inc %ddeg\n", 525 (int)(d->strength * 1000), 526 (int)(d->declination * 180 / M_PI + 0.5f), 527 (int)(d->inclination * 180 / M_PI + 0.5f)); 528 529 // Passing local field information to mag calibration routine 530 #ifdef DIVERSITY_CHECK_ENABLED 531 diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength); 532 #endif 533 // TODO: pass local field information to rotation vector sensor. 534 } else { 535 ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size); 536 } 537 #endif /* ST_MAG40_CAL_ENABLED */ 538 539 return true; 540 } 541 542 static void sendTestResult(uint8_t status, uint8_t sensorType) 543 { 544 struct TestResultData *data = heapAlloc(sizeof(struct TestResultData)); 545 if (!data) { 546 ERROR_PRINT("Couldn't alloc test result packet"); 547 return; 548 } 549 550 data->header.appId = ST_MAG40_APP_ID; 551 data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket)); 552 data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT; 553 data->data_header.sensorType = sensorType; 554 data->data_header.status = status; 555 556 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) 557 ERROR_PRINT("Couldn't send test result packet"); 558 } 559 560 static void magTestHandling(struct I2cTransfer *xfer) 561 { 562 int32_t dataGap[3]; 563 564 switch(mTask.mag_test_state) { 565 case MAG_SELFTEST_INIT: 566 mTask.mag_selftest_num = 0; 567 memset(mTask.dataNOST, 0, 3 * sizeof(int32_t)); 568 569 mTask.mag_test_state = MAG_SELFTEST_RUN_ST_OFF; 570 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_ODR_100_HZ, 0, false); 571 mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false); 572 mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON, 0, true); 573 break; 574 575 case MAG_SELFTEST_RUN_ST_OFF: 576 if (mTask.mag_selftest_num++ > 0) { 577 uint8_t *raw = &xfer->txrxBuf[0]; 578 579 mTask.dataNOST[0] += (*(int16_t *)&raw[0]); 580 mTask.dataNOST[1] += (*(int16_t *)&raw[2]); 581 mTask.dataNOST[2] += (*(int16_t *)&raw[4]); 582 } 583 584 if (mTask.mag_selftest_num <= ST_MAG40_ST_NUM_OF_SAMPLES) { 585 mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 10000, true); 586 587 break; 588 } 589 590 mTask.dataNOST[0] /= ST_MAG40_ST_NUM_OF_SAMPLES; 591 mTask.dataNOST[1] /= ST_MAG40_ST_NUM_OF_SAMPLES; 592 mTask.dataNOST[2] /= ST_MAG40_ST_NUM_OF_SAMPLES; 593 mTask.mag_test_state = MAG_SELFTEST_INIT_ST_EN; 594 /* fall through */ 595 596 case MAG_SELFTEST_INIT_ST_EN: 597 mTask.mag_selftest_num = 0; 598 memset(mTask.dataST, 0, 3 * sizeof(int32_t)); 599 600 mTask.mag_test_state = MAG_SELFTEST_RUN_ST_ON; 601 mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON | ST_MAG40_SELFTEST_EN, 0, true); 602 break; 603 604 case MAG_SELFTEST_RUN_ST_ON: 605 if (mTask.mag_selftest_num++ > 0) { 606 uint8_t *raw = &xfer->txrxBuf[0]; 607 608 mTask.dataST[0] += (*(int16_t *)&raw[0]); 609 mTask.dataST[1] += (*(int16_t *)&raw[2]); 610 mTask.dataST[2] += (*(int16_t *)&raw[4]); 611 } 612 613 if (mTask.mag_selftest_num <= ST_MAG40_ST_NUM_OF_SAMPLES) { 614 mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 10000, true); 615 616 break; 617 } 618 619 mTask.dataST[0] /= ST_MAG40_ST_NUM_OF_SAMPLES; 620 mTask.dataST[1] /= ST_MAG40_ST_NUM_OF_SAMPLES; 621 mTask.dataST[2] /= ST_MAG40_ST_NUM_OF_SAMPLES; 622 mTask.mag_test_state = MAG_SELFTEST_VERIFY; 623 624 /* fall through */ 625 626 case MAG_SELFTEST_VERIFY: 627 dataGap[0] = abs(mTask.dataST[0] - mTask.dataNOST[0]); 628 dataGap[1] = abs(mTask.dataST[1] - mTask.dataNOST[1]); 629 dataGap[2] = abs(mTask.dataST[2] - mTask.dataNOST[2]); 630 631 if (dataGap[0] >= ST_MAG40_ST_MIN_THRESHOLD && 632 dataGap[0] <= ST_MAG40_ST_MAX_THRESHOLD && 633 dataGap[1] >= ST_MAG40_ST_MIN_THRESHOLD && 634 dataGap[1] <= ST_MAG40_ST_MAX_THRESHOLD && 635 dataGap[2] >= ST_MAG40_ST_MIN_THRESHOLD && 636 dataGap[2] <= ST_MAG40_ST_MAX_THRESHOLD) 637 sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_MAG); 638 else 639 sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_MAG); 640 641 mTask.mag_test_state = MAG_SELFTEST_DONE; 642 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_IDLE, 0, false); 643 mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true); 644 break; 645 646 case MAG_SELFTEST_DONE: 647 break; 648 } 649 } 650 651 static bool magSelfTest(void *cookie) 652 { 653 INFO_PRINT("magSelfTest\n"); 654 655 if (!mTask.magOn && trySwitchState(SENSOR_SELF_TEST)) { 656 mTask.mag_test_state = MAG_SELFTEST_INIT; 657 magTestHandling(NULL); 658 return true; 659 } else { 660 ERROR_PRINT("cannot test mag because sensor is busy\n"); 661 sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_MAG); 662 return false; 663 } 664 } 665 666 #define DEC_OPS(power, firmware, rate, flush, test, cal, cfg) \ 667 .sensorPower = power, \ 668 .sensorFirmwareUpload = firmware, \ 669 .sensorSetRate = rate, \ 670 .sensorFlush = flush, \ 671 .sensorCalibrate = cal, \ 672 .sensorSelfTest = test, \ 673 .sensorCfgData = cfg 674 675 static const struct SensorOps st_mag40_SensorOps = 676 { 677 DEC_OPS(magPower, magFwUpload, magSetRate, magFlush, magSelfTest, NULL, magCfgData), 678 }; 679 680 static void enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 681 { 682 gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE); 683 syscfgSetExtiPort(pin); 684 extiEnableIntGpio(pin, EXTI_TRIGGER_RISING); 685 extiChainIsr(ST_MAG40_INT_IRQ, isr); 686 } 687 688 static void disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 689 { 690 extiUnchainIsr(ST_MAG40_INT_IRQ, isr); 691 extiDisableIntGpio(pin); 692 } 693 694 static bool st_mag40_int1_isr(struct ChainedIsr *isr) 695 { 696 if (!extiIsPendingGpio(mTask.Int1)) 697 return false; 698 699 /* Start sampling for a value */ 700 if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT, NULL, NULL, mTask.tid)) 701 ERROR_PRINT("st_mag40_int1_isr: osEnqueuePrivateEvt() failed\n"); 702 703 extiClearPendingGpio(mTask.Int1); 704 return true; 705 } 706 707 #define TIME_NS_TO_US(ns) cpuMathU64DivByU16(ns, 1000) 708 #define kScale_mag 0.15f /* in uT - (1.5f / 10) */ 709 710 static void parseRawData(uint8_t *raw) 711 { 712 struct TripleAxisDataEvent *magSample; 713 714 int32_t raw_x = (*(int16_t *)&raw[0]); 715 int32_t raw_y = (*(int16_t *)&raw[2]); 716 int32_t raw_z = (*(int16_t *)&raw[4]); 717 float x, y, z; 718 float xs, ys, zs; 719 bool newMagnCalibData; 720 #if defined(ST_MAG40_CAL_ENABLED) 721 float xi, yi, zi; 722 #endif 723 724 mTask.timestampInt = sensorGetTime(); 725 726 /* Discard samples generated during sensor turn-on time */ 727 if (mTask.samplesToDiscard > 0) { 728 mTask.samplesToDiscard--; 729 return; 730 } 731 732 /* in uT */ 733 xs = (float)raw_x * kScale_mag; 734 ys = (float)raw_y * kScale_mag; 735 zs = (float)raw_z * kScale_mag; 736 737 /* rotate axes */ 738 x = ST_MAG40_REMAP_X_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX); 739 y = ST_MAG40_REMAP_Y_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX); 740 z = ST_MAG40_REMAP_Z_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX); 741 742 #if defined(ST_MAG40_CAL_ENABLED) 743 magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi); 744 745 newMagnCalibData = magCalUpdate(&mTask.moc, TIME_NS_TO_US(mTask.timestampInt), xi, yi, zi); 746 747 magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z); 748 #endif 749 750 if (magAllocateEvt(&magSample) == false) 751 return; 752 753 magSample->referenceTime = mTask.timestampInt; 754 magSample->samples[0].deltaTime = 0; 755 magSample->samples[0].firstSample.numSamples = 1; 756 magSample->samples[0].x = x; 757 magSample->samples[0].y = y; 758 magSample->samples[0].z = z; 759 760 #if defined(ST_MAG40_CAL_ENABLED) 761 if (newMagnCalibData) { 762 magSample->samples[1].deltaTime = 0; 763 magCalGetBias(&mTask.moc, 764 &magSample->samples[1].x, 765 &magSample->samples[1].y, 766 &magSample->samples[1].z); 767 768 magSample->referenceTime = mTask.timestampInt; 769 magSample->samples[0].firstSample.numSamples = 2; 770 magSample->samples[0].firstSample.biasCurrent = true; 771 magSample->samples[0].firstSample.biasPresent = 1; 772 magSample->samples[0].firstSample.biasSample = 1; 773 } 774 #endif 775 776 osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_MAG), magSample, magFreeEvt); 777 } 778 779 static uint8_t *wai; 780 781 static void int2Evt(void) 782 { 783 if (trySwitchState(SENSOR_READ_SAMPLES)) { 784 mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, true); 785 } else { 786 mTask.pendingInt = true; 787 } 788 } 789 790 static void processPendingEvt(void) 791 { 792 if (mTask.pendingInt) { 793 mTask.pendingInt = false; 794 int2Evt(); 795 return; 796 } 797 798 if (mTask.pendingSubState != NO_SUBSTATE) { 799 if (trySwitchState(SENSOR_MAG_CONFIGURATION)) { 800 mTask.subState = mTask.pendingSubState; 801 mTask.pendingSubState = NO_SUBSTATE; 802 sensorMagConfig(); 803 } 804 } 805 } 806 807 static void sensorMagConfig(void) 808 { 809 uint8_t tmp; 810 811 switch (mTask.subState) { 812 case CONFIG_POWER_UP: 813 mTask.subState = CONFIG_POWER_UP_2; 814 mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false); 815 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, 816 ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_ON | mTask.currentODR, 0, true); 817 break; 818 819 case CONFIG_POWER_UP_2: 820 mTask.subState = CONFIG_DONE; 821 mTask.magOn = true; 822 sensorSignalInternalEvt(mTask.magHandle, 823 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); 824 break; 825 826 case CONFIG_POWER_DOWN: 827 mTask.subState = CONFIG_POWER_DOWN_2; 828 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, 829 ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_IDLE | mTask.currentODR, 0, true); 830 break; 831 832 case CONFIG_POWER_DOWN_2: 833 mTask.subState = CONFIG_DONE; 834 mTask.magOn = false; 835 sensorSignalInternalEvt(mTask.magHandle, 836 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0); 837 break; 838 839 case CONFIG_SET_RATE: 840 mTask.subState = CONFIG_SET_RATE_2; 841 tmp = mTask.magOn ? ST_MAG40_POWER_ON : ST_MAG40_POWER_IDLE; 842 tmp |= mTask.currentODR; 843 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_TEMP_COMP_EN | tmp, 0, true); 844 break; 845 846 case CONFIG_SET_RATE_2: 847 mTask.subState = CONFIG_DONE; 848 sensorSignalInternalEvt(mTask.magHandle, 849 SENSOR_INTERNAL_EVT_RATE_CHG, mTask.rate, mTask.latency); 850 break; 851 852 default: 853 /* Something weird happened */ 854 ERROR_PRINT("sensorMagConfig() subState=%d\n", mTask.subState); 855 mTask.subState = CONFIG_DONE; 856 break; 857 } 858 } 859 860 /* initial sensor configuration */ 861 static void sensorInit(void) 862 { 863 switch (mTask.subState) { 864 case INIT_START: 865 mTask.subState = INIT_ENABLE_DRDY; 866 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, 867 ST_MAG40_SOFT_RESET_BIT, 0, true); 868 break; 869 870 case INIT_ENABLE_DRDY: 871 mTask.subState = INIT_DONE; 872 mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, false); 873 mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, 874 ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true); 875 break; 876 877 default: 878 /* Something weird happened */ 879 ERROR_PRINT("sensorInit() subState=%d\n", mTask.subState); 880 mTask.subState = INIT_DONE; 881 break; 882 } 883 } 884 885 static void handleCommDoneEvt(const void* evtData) 886 { 887 bool returnIdle = false; 888 struct I2cTransfer *xfer = (struct I2cTransfer *)evtData; 889 890 switch (GET_STATE()) { 891 case SENSOR_BOOT: 892 SET_STATE(SENSOR_VERIFY_ID); 893 894 mTask.comm_rx(ST_MAG40_WAI_REG_ADDR, 1, 0, true); 895 break; 896 897 case SENSOR_VERIFY_ID: 898 /* Check the sensor ID */ 899 wai = &xfer->txrxBuf[0]; 900 901 if (ST_MAG40_WAI_REG_VAL != wai[0]) { 902 DEBUG_PRINT("WAI returned is: %02x\n\n", *wai); 903 SET_STATE(SENSOR_BOOT); 904 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, 905 ST_MAG40_SOFT_RESET_BIT, 0, true); 906 break; 907 } 908 909 INFO_PRINT( "Device ID is correct! (%02x)\n", *wai); 910 SET_STATE(SENSOR_INITIALIZATION); 911 mTask.subState = INIT_START; 912 sensorInit(); 913 914 break; 915 916 case SENSOR_INITIALIZATION: 917 if (mTask.subState == INIT_DONE) { 918 INFO_PRINT( "Initialization completed\n"); 919 returnIdle = true; 920 sensorRegisterInitComplete(mTask.magHandle); 921 } else { 922 sensorInit(); 923 } 924 925 break; 926 927 case SENSOR_MAG_CONFIGURATION: 928 if (mTask.subState != CONFIG_DONE) 929 sensorMagConfig(); 930 if (mTask.subState == CONFIG_DONE) 931 returnIdle = true; 932 break; 933 934 case SENSOR_READ_SAMPLES: 935 returnIdle = true; 936 937 if (gpioGet(mTask.Int1)) { 938 ERROR_PRINT("error read sensor, retry!\n"); 939 } 940 if (mTask.magOn) 941 parseRawData(&xfer->txrxBuf[0]); 942 break; 943 944 case SENSOR_SELF_TEST: 945 if (mTask.mag_test_state == MAG_SELFTEST_DONE) 946 returnIdle = true; 947 else 948 magTestHandling(xfer); 949 950 break; 951 952 case SENSOR_IDLE: 953 default: 954 break; 955 } 956 957 releaseXfer(xfer); 958 959 if (returnIdle) { 960 SET_STATE(SENSOR_IDLE); 961 processPendingEvt(); 962 } 963 } 964 965 static void handleEvent(uint32_t evtType, const void* evtData) 966 { 967 switch (evtType) { 968 case EVT_APP_START: 969 INFO_PRINT("EVT_APP_START\n"); 970 osEventUnsubscribe(mTask.tid, EVT_APP_START); 971 972 SET_STATE(SENSOR_BOOT); 973 mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, 974 ST_MAG40_SOFT_RESET_BIT, 0, true); 975 976 break; 977 978 case EVT_COMM_DONE: 979 handleCommDoneEvt(evtData); 980 break; 981 982 case EVT_SENSOR_INTERRUPT: 983 int2Evt(); 984 break; 985 986 default: 987 break; 988 } 989 990 } 991 992 static bool startTask(uint32_t task_id) 993 { 994 size_t slabSize; 995 996 mTask.tid = task_id; 997 998 INFO_PRINT("I2C DRIVER started\n"); 999 1000 mTask.magOn = false; 1001 mTask.pendingInt = false; 1002 mTask.pendingSubState = NO_SUBSTATE; 1003 1004 mTask.currentODR = ST_MAG40_ODR_10_HZ; 1005 mTask.timestampInt = 0; 1006 1007 slabSize = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint); 1008 #if defined(ST_MAG40_CAL_ENABLED) 1009 slabSize += sizeof(struct TripleAxisDataPoint); 1010 #endif 1011 1012 mTask.magDataSlab = slabAllocatorNew(slabSize, 4, ST_MAG40_MAX_MAG_EVENTS); 1013 if (!mTask.magDataSlab) { 1014 ERROR_PRINT("Failed to allocate magDataSlab memory\n"); 1015 return false; 1016 } 1017 1018 /* Init the communication part */ 1019 i2cMasterRequest(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_SPEED); 1020 1021 mTask.comm_tx = i2c_write; 1022 mTask.comm_rx = i2c_read; 1023 1024 /* irq */ 1025 mTask.Int1 = gpioRequest(ST_MAG40_INT_PIN); 1026 gpioConfigInput(mTask.Int1, GPIO_SPEED_LOW, GPIO_PULL_NONE); 1027 mTask.Isr1.func = st_mag40_int1_isr; 1028 enableInterrupt(mTask.Int1, &mTask.Isr1); 1029 1030 #if defined(ST_MAG40_CAL_ENABLED) 1031 #ifdef DIVERSITY_CHECK_ENABLED 1032 initMagCal(&mTask.moc, 1033 0.0f, 0.0f, 0.0f, // bias x, y, z 1034 1.0f, 0.0f, 0.0f, // c00, c01, c02 1035 0.0f, 1.0f, 0.0f, // c10, c11, c12 1036 0.0f, 0.0f, 1.0f, // c20, c21, c22 1037 3000000, // min_batch_window_in_micros 1038 8, // min_num_diverse_vectors 1039 1, // max_num_max_distance 1040 6.0f, // var_threshold 1041 10.0f, // max_min_threshold 1042 48.f, // local_field 1043 0.5f, // threshold_tuning_param 1044 2.552f); // max_distance_tuning_param 1045 #else 1046 initMagCal(&mTask.moc, 1047 0.0f, 0.0f, 0.0f, // bias x, y, z 1048 1.0f, 0.0f, 0.0f, // c00, c01, c02 1049 0.0f, 1.0f, 0.0f, // c10, c11, c12 1050 0.0f, 0.0f, 1.0f, // c20, c21, c22 1051 3000000); // min_batch_window_in_micros 1052 #endif /* DIVERSITY_CHECK_ENABLED */ 1053 #endif /* ST_MAG40_CAL_ENABLED */ 1054 1055 mTask.magHandle = 1056 sensorRegister(&st_mag40_SensorInfo, &st_mag40_SensorOps, NULL, false); 1057 1058 osEventSubscribe(mTask.tid, EVT_APP_START); 1059 1060 return true; 1061 } 1062 1063 static void endTask(void) 1064 { 1065 INFO_PRINT("ended\n"); 1066 #if defined(ST_MAG40_CAL_ENABLED) 1067 magCalDestroy(&mTask.moc); 1068 #endif /* ST_MAG40_CAL_ENABLED */ 1069 slabAllocatorDestroy(mTask.magDataSlab); 1070 disableInterrupt(mTask.Int1, &mTask.Isr1); 1071 } 1072 1073 INTERNAL_APP_INIT(ST_MAG40_APP_ID, 0, startTask, endTask, handleEvent); 1074
