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