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 <nanohubPacket.h> 20 #include <plat/exti.h> 21 #include <plat/gpio.h> 22 #include <platform.h> 23 #include <plat/syscfg.h> 24 #include <heap.h> 25 #include <sensors.h> 26 #include <seos.h> 27 #include <slab.h> 28 #include <i2c.h> 29 #include <timer.h> 30 #include <stdlib.h> 31 #include <string.h> 32 #include <variant/variant.h> 33 34 #define LPS22HB_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 1) 35 36 /* Sensor defs */ 37 #define LPS22HB_INT_CFG_REG_ADDR 0x0B 38 #define LPS22HB_LIR_BIT 0x04 39 40 #define LPS22HB_WAI_REG_ADDR 0x0F 41 #define LPS22HB_WAI_REG_VAL 0xB1 42 43 #define LPS22HB_SOFT_RESET_REG_ADDR 0x11 44 #define LPS22HB_SOFT_RESET_BIT 0x04 45 #define LPS22HB_I2C_DIS 0x08 46 #define LPS22HB_IF_ADD_INC 0x10 47 48 #define LPS22HB_ODR_REG_ADDR 0x10 49 #define LPS22HB_ODR_ONE_SHOT 0x00 50 #define LPS22HB_ODR_1_HZ 0x10 51 #define LPS22HB_ODR_10_HZ 0x20 52 #define LPS22HB_ODR_25_HZ 0x30 53 #define LPS22HB_ODR_50_HZ 0x40 54 #define LPS22HB_ODR_75_HZ 0x50 55 56 #define LPS22HB_RPDS_L 0x18 57 #define LPS22HB_RPDS_H 0x19 58 59 #define LPS22HB_PRESS_OUTXL_REG_ADDR 0x28 60 #define LPS22HB_TEMP_OUTL_REG_ADDR 0x2B 61 62 #define LPS22HB_HECTO_PASCAL(baro_val) (baro_val/4096) 63 #define LPS22HB_CENTIGRADES(temp_val) (temp_val/100) 64 65 #define INFO_PRINT(fmt, ...) \ 66 do { \ 67 osLog(LOG_INFO, "%s " fmt, "[LPS22HB]", ##__VA_ARGS__); \ 68 } while (0); 69 70 #define DEBUG_PRINT(fmt, ...) \ 71 do { \ 72 if (LPS22HB_DBG_ENABLED) { \ 73 osLog(LOG_DEBUG, "%s " fmt, "[LPS22HB]", ##__VA_ARGS__); \ 74 } \ 75 } while (0); 76 77 #define ERROR_PRINT(fmt, ...) \ 78 do { \ 79 osLog(LOG_ERROR, "%s " fmt, "[LPS22HB]", ##__VA_ARGS__); \ 80 } while (0); 81 82 /* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */ 83 #ifndef LPS22HB_DBG_ENABLED 84 #define LPS22HB_DBG_ENABLED 0 85 #endif /* LPS22HB_DBG_ENABLED */ 86 87 enum lps22hbSensorEvents 88 { 89 EVT_COMM_DONE = EVT_APP_START + 1, 90 EVT_SENSOR_BARO_TIMER, 91 EVT_SENSOR_TEMP_TIMER, 92 EVT_TEST, 93 }; 94 95 enum lps22hbSensorState { 96 SENSOR_BOOT, 97 SENSOR_VERIFY_ID, 98 SENSOR_BARO_POWER_UP, 99 SENSOR_BARO_POWER_DOWN, 100 SENSOR_BARO_START_CAL, 101 SENSOR_BARO_READ_CAL_MEAS, 102 SENSOR_BARO_CAL_DONE, 103 SENSOR_BARO_SET_OFFSET, 104 SENSOR_BARO_CFG_DONE, 105 SENSOR_TEMP_POWER_UP, 106 SENSOR_TEMP_POWER_DOWN, 107 SENSOR_READ_SAMPLES, 108 }; 109 110 #ifndef LPS22HB_I2C_BUS_ID 111 #error "LPS22HB_I2C_BUS_ID is not defined; please define in variant.h" 112 #endif 113 114 #ifndef LPS22HB_I2C_SPEED 115 #error "LPS22HB_I2C_SPEED is not defined; please define in variant.h" 116 #endif 117 118 #ifndef LPS22HB_I2C_ADDR 119 #error "LPS22HB_I2C_ADDR is not defined; please define in variant.h" 120 #endif 121 122 enum lps22hbSensorIndex { 123 BARO = 0, 124 TEMP, 125 NUM_OF_SENSOR, 126 }; 127 128 //#define NUM_OF_SENSOR 1 129 130 struct lps22hbSensor { 131 uint32_t handle; 132 }; 133 134 struct CalibrationData { 135 struct HostHubRawPacket header; 136 struct SensorAppEventHeader data_header; 137 float value; 138 } __attribute__((packed)); 139 140 #define LPS22HB_MAX_PENDING_I2C_REQUESTS 4 141 #define LPS22HB_MAX_I2C_TRANSFER_SIZE 6 142 #define LPS22HB_MAX_BARO_EVENTS 4 143 144 struct I2cTransfer 145 { 146 size_t tx; 147 size_t rx; 148 int err; 149 uint8_t txrxBuf[LPS22HB_MAX_I2C_TRANSFER_SIZE]; 150 uint8_t state; 151 bool inUse; 152 }; 153 154 /* Task structure */ 155 struct lps22hbTask { 156 uint32_t tid; 157 158 struct SlabAllocator *baroSlab; 159 160 /* timer */ 161 uint32_t baroTimerHandle; 162 uint32_t tempTimerHandle; 163 164 /* sensor flags */ 165 bool baroOn; 166 bool baroReading; 167 bool baroWantRead; 168 bool tempOn; 169 bool tempReading; 170 bool tempWantRead; 171 172 uint8_t offset_L; 173 uint8_t offset_H; 174 175 //int sensLastRead; 176 177 struct I2cTransfer transfers[LPS22HB_MAX_PENDING_I2C_REQUESTS]; 178 179 /* Communication functions */ 180 bool (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, uint8_t state); 181 bool (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, uint8_t state); 182 183 /* sensors */ 184 struct lps22hbSensor sensors[NUM_OF_SENSOR]; 185 }; 186 187 static struct lps22hbTask mTask; 188 189 static bool baroAllocateEvt(struct SingleAxisDataEvent **evPtr, float sample, uint64_t time) 190 { 191 struct SingleAxisDataEvent *ev; 192 193 ev = *evPtr = slabAllocatorAlloc(mTask.baroSlab); 194 if (!ev) { 195 ERROR_PRINT("Failed to allocate baro evt memory"); 196 return false; 197 } 198 199 memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); 200 ev->referenceTime = time; 201 ev->samples[0].firstSample.numSamples = 1; 202 ev->samples[0].fdata = sample; 203 204 return true; 205 } 206 207 static void baroFreeEvt(void *ptr) 208 { 209 slabAllocatorFree(mTask.baroSlab, ptr); 210 } 211 212 // Allocate a buffer and mark it as in use with the given state, or return NULL 213 // if no buffers available. Must *not* be called from interrupt context. 214 static struct I2cTransfer *allocXfer(uint8_t state) 215 { 216 size_t i; 217 218 for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) { 219 if (!mTask.transfers[i].inUse) { 220 mTask.transfers[i].inUse = true; 221 mTask.transfers[i].state = state; 222 return &mTask.transfers[i]; 223 } 224 } 225 226 ERROR_PRINT("Ran out of i2c buffers!"); 227 return NULL; 228 } 229 230 static inline void releaseXfer(struct I2cTransfer *xfer) 231 { 232 xfer->inUse = false; 233 } 234 235 static void i2cCallback(void *cookie, size_t tx, size_t rx, int err) 236 { 237 struct I2cTransfer *xfer = cookie; 238 239 xfer->tx = tx; 240 xfer->rx = rx; 241 xfer->err = err; 242 243 osEnqueuePrivateEvt(EVT_COMM_DONE, cookie, NULL, mTask.tid); 244 if (err != 0) 245 ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err); 246 } 247 248 static bool i2c_read(uint8_t addr, uint16_t len, uint32_t delay, uint8_t state) 249 { 250 struct I2cTransfer *xfer = allocXfer(state); 251 int ret = -1; 252 253 if (xfer != NULL) { 254 xfer->txrxBuf[0] = 0x80 | addr; 255 if ((ret = i2cMasterTxRx(LPS22HB_I2C_BUS_ID, LPS22HB_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, i2cCallback, xfer)) < 0) { 256 releaseXfer(xfer); 257 DEBUG_PRINT("i2c_read: i2cMasterTxRx operation failed (ret: %d)\n", ret); 258 return false; 259 } 260 } 261 262 return (ret == -1) ? false : true; 263 } 264 265 static bool i2c_write(uint8_t addr, uint8_t data, uint32_t delay, uint8_t state) 266 { 267 struct I2cTransfer *xfer = allocXfer(state); 268 int ret = -1; 269 270 if (xfer != NULL) { 271 xfer->txrxBuf[0] = addr; 272 xfer->txrxBuf[1] = data; 273 if ((ret = i2cMasterTx(LPS22HB_I2C_BUS_ID, LPS22HB_I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer)) < 0) { 274 releaseXfer(xfer); 275 DEBUG_PRINT("i2c_write: i2cMasterTx operation failed (ret: %d)\n", ret); 276 return false; 277 } 278 } 279 280 return (ret == -1) ? false : true; 281 } 282 283 static void sendCalibrationResult(uint8_t status, float value) 284 { 285 struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData)); 286 if (!data) { 287 ERROR_PRINT("Couldn't alloc cal result pkt\n"); 288 return; 289 } 290 291 data->header.appId = LPS22HB_APP_ID; 292 data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket)); 293 data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT; 294 data->data_header.sensorType = SENS_TYPE_BARO; 295 data->data_header.status = status; 296 297 data->value = value; 298 299 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) 300 ERROR_PRINT("Couldn't send cal result evt\n"); 301 } 302 303 /* Sensor Info */ 304 static void sensorBaroTimerCallback(uint32_t timerId, void *data) 305 { 306 osEnqueuePrivateEvt(EVT_SENSOR_BARO_TIMER, data, NULL, mTask.tid); 307 } 308 309 static void sensorTempTimerCallback(uint32_t timerId, void *data) 310 { 311 osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, data, NULL, mTask.tid); 312 } 313 314 #define DEC_INFO(name, type, axis, inter, samples, rates) \ 315 .sensorName = name, \ 316 .sensorType = type, \ 317 .numAxis = axis, \ 318 .interrupt = inter, \ 319 .minSamples = samples, \ 320 .supportedRates = rates 321 322 static uint32_t lps22hbRates[] = { 323 SENSOR_HZ(1.0f), 324 SENSOR_HZ(10.0f), 325 SENSOR_HZ(25.0f), 326 SENSOR_HZ(50.0f), 327 SENSOR_HZ(75.0f), 328 0 329 }; 330 331 // should match "supported rates in length" and be the timer length for that rate in nanosecs 332 static const uint64_t lps22hbRatesRateVals[] = 333 { 334 1 * 1000000000ULL, 335 1000000000ULL / 10, 336 1000000000ULL / 25, 337 1000000000ULL / 50, 338 1000000000ULL / 75, 339 }; 340 341 342 static const struct SensorInfo lps22hbSensorInfo[NUM_OF_SENSOR] = 343 { 344 { DEC_INFO("Pressure", SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP, 345 300, lps22hbRates) }, 346 { DEC_INFO("Temperature", SENS_TYPE_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 347 20, lps22hbRates) }, 348 }; 349 350 /* Sensor Operations */ 351 static bool baroPower(bool on, void *cookie) 352 { 353 bool oldMode = mTask.baroOn || mTask.tempOn; 354 bool newMode = on || mTask.tempOn; 355 uint32_t state = on ? SENSOR_BARO_POWER_UP : SENSOR_BARO_POWER_DOWN; 356 bool ret = true; 357 358 INFO_PRINT("baroPower %s\n", on ? "enable" : "disable"); 359 if (!on && mTask.baroTimerHandle) { 360 timTimerCancel(mTask.baroTimerHandle); 361 mTask.baroTimerHandle = 0; 362 mTask.baroReading = false; 363 } 364 365 if (oldMode != newMode) { 366 if (on) 367 ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_10_HZ, 0, state); 368 else 369 ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_ONE_SHOT, 0, state); 370 } else 371 sensorSignalInternalEvt(mTask.sensors[BARO].handle, 372 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 373 374 if (!ret) { 375 DEBUG_PRINT("baroPower comm_tx failed\n"); 376 return(false); 377 } 378 379 mTask.baroReading = false; 380 mTask.baroOn = on; 381 return true; 382 } 383 384 static bool baroFwUpload(void *cookie) 385 { 386 return sensorSignalInternalEvt(mTask.sensors[BARO].handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 387 } 388 389 static bool baroSetRate(uint32_t rate, uint64_t latency, void *cookie) 390 { 391 INFO_PRINT("baroSetRate %lu Hz - %llu ns\n", rate, latency); 392 393 if (mTask.baroTimerHandle) 394 timTimerCancel(mTask.baroTimerHandle); 395 396 mTask.baroTimerHandle = timTimerSet(sensorTimerLookupCommon(lps22hbRates, 397 lps22hbRatesRateVals, rate), 0, 50, sensorBaroTimerCallback, NULL, false); 398 399 return sensorSignalInternalEvt(mTask.sensors[BARO].handle, 400 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 401 } 402 403 static bool baroFlush(void *cookie) 404 { 405 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL); 406 } 407 408 static bool baroCalibrate(void *cookie) 409 { 410 INFO_PRINT("baroCalibrate\n"); 411 412 if (mTask.baroOn) { 413 ERROR_PRINT("cannot calibrate while baro is active\n"); 414 sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, 0.0f); 415 return false; 416 } 417 418 mTask.comm_tx(LPS22HB_RPDS_L, 0, 0, SENSOR_BARO_START_CAL); 419 return true; 420 } 421 422 /* 423 * Offset data is sent in hPa, and must be transformed in 16th of hPa. 424 * Since offset is expected to be summed to the out regs but the sensor 425 * will actually subctract it then we need to invert the sign. 426 */ 427 static bool baroCfgData(void *data, void *cookie) 428 { 429 float offset_f = *((float *)data) * 16; 430 int32_t offset; 431 bool ret; 432 433 offset_f = (offset_f > 0) ? offset_f + 0.5f : offset_f - 0.5f; 434 offset = -(int32_t)offset_f; 435 436 INFO_PRINT("baroCfgData %ld\n", offset); 437 438 mTask.offset_H = (offset >> 8) & 0xff; 439 mTask.offset_L = (offset & 0xff); 440 441 ret = mTask.comm_tx(LPS22HB_RPDS_L, mTask.offset_L, 0, SENSOR_BARO_SET_OFFSET); 442 if (!ret) 443 DEBUG_PRINT("baroCfgData: comm_tx failed\n"); 444 445 return ret; 446 } 447 448 static bool tempPower(bool on, void *cookie) 449 { 450 bool oldMode = mTask.baroOn || mTask.tempOn; 451 bool newMode = on || mTask.baroOn; 452 uint32_t state = on ? SENSOR_TEMP_POWER_UP : SENSOR_TEMP_POWER_DOWN; 453 bool ret = true; 454 455 INFO_PRINT("tempPower %s\n", on ? "enable" : "disable"); 456 if (!on && mTask.tempTimerHandle) { 457 timTimerCancel(mTask.tempTimerHandle); 458 mTask.tempTimerHandle = 0; 459 mTask.tempReading = false; 460 } 461 462 if (oldMode != newMode) { 463 if (on) 464 ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_10_HZ, 0, state); 465 else 466 ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_ONE_SHOT, 0, state); 467 } else 468 sensorSignalInternalEvt(mTask.sensors[TEMP].handle, 469 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 470 471 if (!ret) { 472 DEBUG_PRINT("tempPower comm_tx failed\n"); 473 return(false); 474 } 475 476 mTask.tempReading = false; 477 mTask.tempOn = on; 478 return true; 479 } 480 481 static bool tempFwUpload(void *cookie) 482 { 483 return sensorSignalInternalEvt(mTask.sensors[TEMP].handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 484 } 485 486 static bool tempSetRate(uint32_t rate, uint64_t latency, void *cookie) 487 { 488 if (mTask.tempTimerHandle) 489 timTimerCancel(mTask.tempTimerHandle); 490 491 INFO_PRINT("tempSetRate %lu Hz - %llu ns\n", rate, latency); 492 mTask.tempTimerHandle = timTimerSet(sensorTimerLookupCommon(lps22hbRates, 493 lps22hbRatesRateVals, rate), 0, 50, sensorTempTimerCallback, NULL, false); 494 495 return sensorSignalInternalEvt(mTask.sensors[TEMP].handle, 496 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 497 } 498 499 static bool tempFlush(void *cookie) 500 { 501 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL); 502 } 503 504 #define DEC_OPS(power, firmware, rate, flush, cal, cfg) \ 505 .sensorPower = power, \ 506 .sensorFirmwareUpload = firmware, \ 507 .sensorSetRate = rate, \ 508 .sensorFlush = flush, \ 509 .sensorCalibrate = cal, \ 510 .sensorCfgData = cfg 511 512 static const struct SensorOps lps22hbSensorOps[NUM_OF_SENSOR] = 513 { 514 { DEC_OPS(baroPower, baroFwUpload, baroSetRate, baroFlush, baroCalibrate, baroCfgData) }, 515 { DEC_OPS(tempPower, tempFwUpload, tempSetRate, tempFlush, NULL, NULL) }, 516 }; 517 518 static int handleCommDoneEvt(const void* evtData) 519 { 520 uint8_t i; 521 int baro_val; 522 short temp_val; 523 //uint32_t state = (uint32_t)evtData; 524 struct SingleAxisDataEvent *baroSample; 525 union EmbeddedDataPoint sample; 526 struct I2cTransfer *xfer = (struct I2cTransfer *)evtData; 527 uint8_t *ptr_samples; 528 529 switch (xfer->state) { 530 case SENSOR_BOOT: 531 if (!mTask.comm_rx(LPS22HB_WAI_REG_ADDR, 1, 1, SENSOR_VERIFY_ID)) { 532 DEBUG_PRINT("Not able to read WAI\n"); 533 return -1; 534 } 535 break; 536 537 case SENSOR_VERIFY_ID: 538 /* Check the sensor ID */ 539 if (xfer->err != 0 || xfer->txrxBuf[0] != LPS22HB_WAI_REG_VAL) { 540 DEBUG_PRINT("WAI returned is: %02x\n", xfer->txrxBuf[0]); 541 break; 542 } 543 544 545 INFO_PRINT("Device ID is correct! (%02x)\n", xfer->txrxBuf[0]); 546 for (i = 0; i < NUM_OF_SENSOR; i++) 547 sensorRegisterInitComplete(mTask.sensors[i].handle); 548 549 /* TEST the environment in standalone mode */ 550 //osEnqueuePrivateEvt(EVT_TEST, NULL, NULL, mTask.tid); 551 break; 552 553 case SENSOR_BARO_POWER_UP: 554 sensorSignalInternalEvt(mTask.sensors[BARO].handle, 555 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); 556 break; 557 558 case SENSOR_BARO_POWER_DOWN: 559 sensorSignalInternalEvt(mTask.sensors[BARO].handle, 560 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0); 561 break; 562 563 case SENSOR_TEMP_POWER_UP: 564 sensorSignalInternalEvt(mTask.sensors[TEMP].handle, 565 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); 566 break; 567 568 case SENSOR_TEMP_POWER_DOWN: 569 sensorSignalInternalEvt(mTask.sensors[TEMP].handle, 570 SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0); 571 break; 572 573 case SENSOR_BARO_START_CAL: 574 mTask.comm_tx(LPS22HB_RPDS_H, 0, 0, SENSOR_BARO_READ_CAL_MEAS); 575 break; 576 577 case SENSOR_BARO_READ_CAL_MEAS: 578 mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 3, 1, SENSOR_BARO_CAL_DONE); 579 break; 580 581 case SENSOR_BARO_CAL_DONE: 582 ptr_samples = xfer->txrxBuf; 583 584 baro_val = ((ptr_samples[2] << 16) & 0xff0000) | 585 ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]); 586 587 sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, LPS22HB_HECTO_PASCAL((float)baro_val)); 588 break; 589 590 case SENSOR_BARO_SET_OFFSET: 591 mTask.comm_tx(LPS22HB_RPDS_H, mTask.offset_H, 0, SENSOR_BARO_CFG_DONE); 592 break; 593 594 case SENSOR_BARO_CFG_DONE: 595 break; 596 597 case SENSOR_READ_SAMPLES: 598 if (mTask.baroOn && mTask.baroWantRead) { 599 float pressure_hPa; 600 601 mTask.baroWantRead = false; 602 ptr_samples = xfer->txrxBuf; 603 604 baro_val = ((ptr_samples[2] << 16) & 0xff0000) | 605 ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]); 606 607 mTask.baroReading = false; 608 pressure_hPa = LPS22HB_HECTO_PASCAL((float)baro_val); 609 //osLog(LOG_INFO, "baro: %p\n", sample.vptr); 610 if (baroAllocateEvt(&baroSample, pressure_hPa, sensorGetTime())) { 611 osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_BARO), baroSample, baroFreeEvt); 612 } 613 } 614 615 if (mTask.tempOn && mTask.tempWantRead) { 616 mTask.tempWantRead = false; 617 ptr_samples = &xfer->txrxBuf[3]; 618 619 temp_val = ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]); 620 621 mTask.tempReading = false; 622 sample.fdata = LPS22HB_CENTIGRADES((float)temp_val); 623 //osLog(LOG_INFO, "temp: %p\n", sample.vptr); 624 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), sample.vptr, NULL); 625 } 626 627 break; 628 629 default: 630 break; 631 } 632 633 releaseXfer(xfer); 634 return (0); 635 } 636 637 static void handleEvent(uint32_t evtType, const void* evtData) 638 { 639 switch (evtType) { 640 case EVT_APP_START: 641 INFO_PRINT("EVT_APP_START\n"); 642 osEventUnsubscribe(mTask.tid, EVT_APP_START); 643 644 mTask.comm_tx(LPS22HB_SOFT_RESET_REG_ADDR, 645 LPS22HB_SOFT_RESET_BIT, 0, SENSOR_BOOT); 646 break; 647 648 case EVT_COMM_DONE: 649 //INFO_PRINT("EVT_COMM_DONE %d\n", (int)evtData); 650 handleCommDoneEvt(evtData); 651 break; 652 653 case EVT_SENSOR_BARO_TIMER: 654 //INFO_PRINT("EVT_SENSOR_BARO_TIMER\n"); 655 656 mTask.baroWantRead = true; 657 658 /* Start sampling for a value */ 659 if (!mTask.baroReading && !mTask.tempReading) { 660 mTask.baroReading = true; 661 662 mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 5, 1, SENSOR_READ_SAMPLES); 663 } 664 665 break; 666 667 case EVT_SENSOR_TEMP_TIMER: 668 //INFO_PRINT("EVT_SENSOR_TEMP_TIMER\n"); 669 670 mTask.tempWantRead = true; 671 672 /* Start sampling for a value */ 673 if (!mTask.baroReading && !mTask.tempReading) { 674 mTask.tempReading = true; 675 676 mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 5, 1, SENSOR_READ_SAMPLES); 677 } 678 679 break; 680 681 case EVT_TEST: 682 INFO_PRINT("EVT_TEST\n"); 683 684 baroPower(true, NULL); 685 tempPower(true, NULL); 686 baroSetRate(SENSOR_HZ(1), 0, NULL); 687 tempSetRate(SENSOR_HZ(1), 0, NULL); 688 break; 689 690 default: 691 break; 692 } 693 694 } 695 696 static bool startTask(uint32_t task_id) 697 { 698 uint8_t i; 699 size_t slabSize; 700 701 mTask.tid = task_id; 702 703 INFO_PRINT("task started\n"); 704 705 mTask.baroOn = mTask.tempOn = false; 706 mTask.baroReading = mTask.tempReading = false; 707 708 mTask.offset_H = 0; 709 mTask.offset_L = 0; 710 711 slabSize = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint); 712 713 mTask.baroSlab = slabAllocatorNew(slabSize, 4, LPS22HB_MAX_BARO_EVENTS); 714 if (!mTask.baroSlab) { 715 ERROR_PRINT("Failed to allocate baroSlab memory\n"); 716 return false; 717 } 718 719 /* Init the communication part */ 720 i2cMasterRequest(LPS22HB_I2C_BUS_ID, LPS22HB_I2C_SPEED); 721 722 mTask.comm_tx = i2c_write; 723 mTask.comm_rx = i2c_read; 724 725 for (i = 0; i < NUM_OF_SENSOR; i++) { 726 mTask.sensors[i].handle = 727 sensorRegister(&lps22hbSensorInfo[i], &lps22hbSensorOps[i], NULL, false); 728 } 729 730 osEventSubscribe(mTask.tid, EVT_APP_START); 731 732 return true; 733 } 734 735 static void endTask(void) 736 { 737 INFO_PRINT("task ended\n"); 738 slabAllocatorDestroy(mTask.baroSlab); 739 } 740 741 INTERNAL_APP_INIT(LPS22HB_APP_ID, 0, startTask, endTask, handleEvent); 742