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 <stdlib.h> 18 #include <string.h> 19 #include <float.h> 20 21 #include <eventnums.h> 22 #include <heap.h> 23 #include <hostIntf.h> 24 #include <i2c.h> 25 #include <nanohubPacket.h> 26 #include <sensors.h> 27 #include <seos.h> 28 #include <timer.h> 29 #include <util.h> 30 31 #define BMP280_APP_ID APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 5) 32 33 #define BMP280_APP_VERSION 2 34 35 #define I2C_BUS_ID 0 36 #define I2C_SPEED 400000 37 #define I2C_ADDR 0x76 38 39 #define BOSCH_BMP280_ID 0x58 40 41 #define BOSCH_BMP280_REG_RESET 0x60 42 #define BOSCH_BMP280_REG_DIG_T1 0x88 43 #define BOSCH_BMP280_REG_ID 0xd0 44 #define BOSCH_BMP280_REG_CTRL_MEAS 0xf4 45 #define BOSCH_BMP280_REG_CONFIG 0xf5 46 #define BOSCH_BMP280_REG_PRES_MSB 0xf7 47 48 #define BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS 4 49 #define BOSCH_BMP280_MAX_I2C_TRANSFER_SIZE 6 50 51 // temp: 2x oversampling, baro: 16x oversampling, power: normal 52 #define CTRL_ON ((2 << 5) | (5 << 2) | 3) 53 // temp: 2x oversampling, baro: 16x oversampling, power: sleep 54 #define CTRL_SLEEP ((2 << 5) | (5 << 2)) 55 56 enum BMP280SensorEvents 57 { 58 EVT_SENSOR_I2C = EVT_APP_START + 1, 59 EVT_SENSOR_BARO_TIMER, 60 EVT_SENSOR_TEMP_TIMER, 61 }; 62 63 enum BMP280TaskState 64 { 65 STATE_RESET, 66 STATE_VERIFY_ID, 67 STATE_AWAITING_COMP_PARAMS, 68 STATE_CONFIG, 69 STATE_FINISH_INIT, 70 STATE_IDLE, 71 STATE_ENABLING_BARO, 72 STATE_ENABLING_TEMP, 73 STATE_DISABLING_BARO, 74 STATE_DISABLING_TEMP, 75 STATE_SAMPLING, 76 }; 77 78 struct BMP280CompParams 79 { 80 uint16_t dig_T1; 81 int16_t dig_T2, dig_T3; 82 uint16_t dig_P1; 83 int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9; 84 } __attribute__((packed)); 85 86 struct I2cTransfer 87 { 88 size_t tx; 89 size_t rx; 90 int err; 91 uint8_t txrxBuf[BOSCH_BMP280_MAX_I2C_TRANSFER_SIZE]; 92 uint8_t state; 93 bool inUse; 94 }; 95 96 static struct BMP280Task 97 { 98 struct BMP280CompParams comp; 99 100 uint32_t id; 101 uint32_t baroHandle; 102 uint32_t tempHandle; 103 uint32_t baroTimerHandle; 104 uint32_t tempTimerHandle; 105 106 float offset; 107 108 struct I2cTransfer transfers[BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS]; 109 110 bool baroOn; 111 bool tempOn; 112 bool baroReading; 113 bool baroCalibrating; 114 bool tempReading; 115 } mTask; 116 117 struct CalibrationData { 118 struct HostHubRawPacket header; 119 struct SensorAppEventHeader data_header; 120 float value; 121 } __attribute__((packed)); 122 123 static const uint32_t tempSupportedRates[] = 124 { 125 SENSOR_HZ(0.1), 126 SENSOR_HZ(1), 127 SENSOR_HZ(5), 128 SENSOR_HZ(10), 129 SENSOR_HZ(25), 130 0, 131 }; 132 133 static const uint64_t rateTimerValsTemp[] = //should match "supported rates in length" and be the timer length for that rate in nanosecs 134 { 135 10 * 1000000000ULL, 136 1 * 1000000000ULL, 137 1000000000ULL / 5, 138 1000000000ULL / 10, 139 1000000000ULL / 25, 140 }; 141 142 static const uint32_t baroSupportedRates[] = 143 { 144 SENSOR_HZ(0.1), 145 SENSOR_HZ(1), 146 SENSOR_HZ(5), 147 SENSOR_HZ(10), 148 0 149 }; 150 151 static const uint64_t rateTimerValsBaro[] = //should match "supported rates in length" and be the timer length for that rate in nanosecs 152 { 153 10 * 1000000000ULL, 154 1 * 1000000000ULL, 155 1000000000ULL / 5, 156 1000000000ULL / 10, 157 }; 158 159 static void i2cCallback(void *cookie, size_t tx, size_t rx, int err); 160 161 // Allocate a buffer and mark it as in use with the given state, or return NULL 162 // if no buffers available. Must *not* be called from interrupt context. 163 static struct I2cTransfer *allocXfer(uint8_t state) 164 { 165 size_t i; 166 167 for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) { 168 if (!mTask.transfers[i].inUse) { 169 mTask.transfers[i].inUse = true; 170 mTask.transfers[i].state = state; 171 return &mTask.transfers[i]; 172 } 173 } 174 175 osLog(LOG_ERROR, "[BMP280]: Ran out of i2c buffers!"); 176 return NULL; 177 } 178 179 // Helper function to write a one byte register. Returns true if we got a 180 // successful return value from i2cMasterTx(). 181 static bool writeRegister(uint8_t reg, uint8_t value, uint8_t state) 182 { 183 struct I2cTransfer *xfer = allocXfer(state); 184 int ret = -1; 185 186 if (xfer != NULL) { 187 xfer->txrxBuf[0] = reg; 188 xfer->txrxBuf[1] = value; 189 ret = i2cMasterTx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer); 190 } 191 192 return (ret == 0); 193 } 194 195 /* sensor callbacks from nanohub */ 196 197 static void i2cCallback(void *cookie, size_t tx, size_t rx, int err) 198 { 199 struct I2cTransfer *xfer = cookie; 200 201 xfer->tx = tx; 202 xfer->rx = rx; 203 xfer->err = err; 204 205 osEnqueuePrivateEvt(EVT_SENSOR_I2C, cookie, NULL, mTask.id); 206 if (err != 0) 207 osLog(LOG_INFO, "[BMP280] i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err); 208 } 209 210 static void baroTimerCallback(uint32_t timerId, void *cookie) 211 { 212 osEnqueuePrivateEvt(EVT_SENSOR_BARO_TIMER, cookie, NULL, mTask.id); 213 } 214 215 static void tempTimerCallback(uint32_t timerId, void *cookie) 216 { 217 osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, cookie, NULL, mTask.id); 218 } 219 220 static void setMode(bool on, uint8_t state) 221 { 222 writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, (on) ? CTRL_ON : CTRL_SLEEP, state); 223 } 224 225 static void sendCalibrationResult(uint8_t status, float value) { 226 struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData)); 227 if (!data) { 228 osLog(LOG_WARN, "[BMP280] Couldn't alloc cal result pkt"); 229 return; 230 } 231 232 data->header.appId = BMP280_APP_ID; 233 data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket)); 234 data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT; 235 data->data_header.sensorType = SENS_TYPE_BARO; 236 data->data_header.status = status; 237 238 data->value = value; 239 240 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) 241 osLog(LOG_WARN, "[BMP280] Couldn't send cal result evt"); 242 } 243 244 // TODO: only turn on the timer when enabled 245 static bool sensorPowerBaro(bool on, void *cookie) 246 { 247 bool oldMode = mTask.baroOn || mTask.tempOn; 248 bool newMode = on || mTask.tempOn; 249 250 if (!on && mTask.baroTimerHandle) { 251 timTimerCancel(mTask.baroTimerHandle); 252 mTask.baroTimerHandle = 0; 253 mTask.baroReading = false; 254 } 255 256 if (oldMode != newMode) 257 setMode(newMode, (on ? STATE_ENABLING_BARO : STATE_DISABLING_BARO)); 258 else 259 sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 260 261 mTask.baroOn = on; 262 263 return true; 264 } 265 266 static bool sensorFirmwareBaro(void *cookie) 267 { 268 return sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 269 } 270 271 static bool sensorRateBaro(uint32_t rate, uint64_t latency, void *cookie) 272 { 273 if (mTask.baroTimerHandle) 274 timTimerCancel(mTask.baroTimerHandle); 275 mTask.baroTimerHandle = timTimerSet(sensorTimerLookupCommon(baroSupportedRates, rateTimerValsBaro, rate), 0, 50, baroTimerCallback, NULL, false); 276 return sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 277 } 278 279 static bool sensorFlushBaro(void *cookie) 280 { 281 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL); 282 } 283 284 static bool sensorCalibrateBaro(void *cookie) 285 { 286 if (mTask.baroOn || mTask.tempOn) { 287 osLog(LOG_ERROR, "[BMP280] cannot calibrate while baro or temp are active\n"); 288 sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, 0.0f); 289 return false; 290 } 291 292 if (mTask.baroTimerHandle) 293 timTimerCancel(mTask.baroTimerHandle); 294 mTask.baroTimerHandle = timTimerSet(100000000ull, 0, 50, baroTimerCallback, NULL, false); 295 296 mTask.offset = 0.0f; 297 mTask.baroOn = true; 298 mTask.baroCalibrating = true; 299 300 return writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, CTRL_ON, STATE_IDLE); 301 } 302 303 static bool sensorCfgDataBaro(void *data, void *cookie) 304 { 305 mTask.offset = *((float*)data) * 100.0f; // offset is given in hPa, but used as Pa in compensation 306 return true; 307 } 308 309 static bool sensorPowerTemp(bool on, void *cookie) 310 { 311 bool oldMode = mTask.baroOn || mTask.tempOn; 312 bool newMode = on || mTask.baroOn; 313 314 if (!on && mTask.tempTimerHandle) { 315 timTimerCancel(mTask.tempTimerHandle); 316 mTask.tempTimerHandle = 0; 317 mTask.tempReading = false; 318 } 319 320 if (oldMode != newMode) 321 setMode(newMode, (on ? STATE_ENABLING_TEMP : STATE_DISABLING_TEMP)); 322 else 323 sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 324 325 mTask.tempOn = on; 326 327 return true; 328 } 329 330 static bool sensorFirmwareTemp(void *cookie) 331 { 332 sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 333 return true; 334 } 335 336 static bool sensorRateTemp(uint32_t rate, uint64_t latency, void *cookie) 337 { 338 if (mTask.tempTimerHandle) 339 timTimerCancel(mTask.tempTimerHandle); 340 mTask.tempTimerHandle = timTimerSet(sensorTimerLookupCommon(tempSupportedRates, rateTimerValsTemp, rate), 0, 50, tempTimerCallback, NULL, false); 341 sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 342 return true; 343 } 344 345 static bool sensorFlushTemp(void *cookie) 346 { 347 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL); 348 } 349 350 static const struct SensorInfo sensorInfoBaro = 351 { 352 .sensorName = "Pressure", 353 .supportedRates = baroSupportedRates, 354 .sensorType = SENS_TYPE_BARO, 355 .numAxis = NUM_AXIS_EMBEDDED, 356 .interrupt = NANOHUB_INT_NONWAKEUP, 357 .minSamples = 300 358 }; 359 360 static const struct SensorOps sensorOpsBaro = 361 { 362 .sensorPower = sensorPowerBaro, 363 .sensorFirmwareUpload = sensorFirmwareBaro, 364 .sensorSetRate = sensorRateBaro, 365 .sensorFlush = sensorFlushBaro, 366 .sensorCalibrate = sensorCalibrateBaro, 367 .sensorCfgData = sensorCfgDataBaro, 368 }; 369 370 static const struct SensorInfo sensorInfoTemp = 371 { 372 .sensorName = "Temperature", 373 .supportedRates = tempSupportedRates, 374 .sensorType = SENS_TYPE_TEMP, 375 .numAxis = NUM_AXIS_EMBEDDED, 376 .interrupt = NANOHUB_INT_NONWAKEUP, 377 .minSamples = 20 378 }; 379 380 static const struct SensorOps sensorOpsTemp = 381 { 382 .sensorPower = sensorPowerTemp, 383 .sensorFirmwareUpload = sensorFirmwareTemp, 384 .sensorSetRate = sensorRateTemp, 385 .sensorFlush = sensorFlushTemp, 386 }; 387 388 // Returns temperature in units of 0.01 degrees celsius. 389 static int32_t compensateTemp( int32_t adc_T, int32_t *t_fine) 390 { 391 int32_t var1 = 392 (((adc_T >> 3) - ((int32_t)mTask.comp.dig_T1 << 1)) 393 * (int32_t)mTask.comp.dig_T2) >> 11; 394 395 int32_t tmp = (adc_T >> 4) - (int32_t)mTask.comp.dig_T1; 396 397 int32_t var2 = (((tmp * tmp) >> 12) * (int32_t)mTask.comp.dig_T3) >> 14; 398 399 int32_t sum = var1 + var2; 400 401 *t_fine = sum; 402 403 return (sum * 5 + 128) >> 8; 404 } 405 406 static float compensateBaro(int32_t t_fine, int32_t adc_P) 407 { 408 float f = t_fine - 128000, fSqr = f * f; 409 float a = 1048576 - adc_P; 410 float v1, v2, p, pSqr; 411 412 v2 = fSqr * mTask.comp.dig_P6 + f * mTask.comp.dig_P5 * (float)(1ULL << 17) + mTask.comp.dig_P4 * (float)(1ULL << 35); 413 v1 = fSqr * mTask.comp.dig_P1 * mTask.comp.dig_P3 * (1.0f/(1ULL << 41)) + f * mTask.comp.dig_P1 * mTask.comp.dig_P2 * (1.0f/(1ULL << 21)) + mTask.comp.dig_P1 * (float)(1ULL << 14); 414 415 p = (a * (float)(1ULL << 31) - v2) * 3125 / v1; 416 pSqr = p * p; 417 418 return pSqr * mTask.comp.dig_P9 * (1.0f/(1ULL << 59)) + p * (mTask.comp.dig_P8 * (1.0f/(1ULL << 19)) + 1) * (1.0f/(1ULL << 8)) + 16.0f * mTask.comp.dig_P7; 419 } 420 421 static void getTempAndBaro(const uint8_t *tmp, float *pressure_Pa, float *temp_centigrade) 422 { 423 int32_t pres_adc = ((int32_t)tmp[0] << 12) | ((int32_t)tmp[1] << 4) | (tmp[2] >> 4); 424 int32_t temp_adc = ((int32_t)tmp[3] << 12) | ((int32_t)tmp[4] << 4) | (tmp[5] >> 4); 425 426 int32_t T_fine; 427 int32_t temp = compensateTemp(temp_adc, &T_fine); 428 float pres = compensateBaro(T_fine, pres_adc); 429 430 *temp_centigrade = (float)temp * 0.01f; 431 *pressure_Pa = pres * (1.0f / 256.0f) + mTask.offset; 432 } 433 434 static void handleI2cEvent(struct I2cTransfer *xfer) 435 { 436 union EmbeddedDataPoint sample; 437 struct I2cTransfer *newXfer; 438 439 switch (xfer->state) { 440 case STATE_RESET: { 441 newXfer = allocXfer(STATE_VERIFY_ID); 442 if (newXfer != NULL) { 443 newXfer->txrxBuf[0] = BOSCH_BMP280_REG_ID; 444 i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 1, i2cCallback, newXfer); 445 } 446 break; 447 } 448 449 case STATE_VERIFY_ID: { 450 /* Check the sensor ID */ 451 if (xfer->err != 0 || xfer->txrxBuf[0] != BOSCH_BMP280_ID) { 452 osLog(LOG_INFO, "[BMP280] not detected\n"); 453 break; 454 } 455 456 /* Get compensation parameters */ 457 newXfer = allocXfer(STATE_AWAITING_COMP_PARAMS); 458 if (newXfer != NULL) { 459 newXfer->txrxBuf[0] = BOSCH_BMP280_REG_DIG_T1; 460 i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, (uint8_t*)&mTask.comp, 24, i2cCallback, newXfer); 461 } 462 463 break; 464 } 465 466 case STATE_AWAITING_COMP_PARAMS: { 467 writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, CTRL_SLEEP, STATE_CONFIG); 468 break; 469 } 470 471 case STATE_CONFIG: { 472 // standby time: 62.5ms, IIR filter coefficient: 4 473 writeRegister(BOSCH_BMP280_REG_CONFIG, (1 << 5) | (2 << 2), STATE_FINISH_INIT); 474 } 475 476 case STATE_ENABLING_BARO: { 477 sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); 478 break; 479 } 480 481 case STATE_ENABLING_TEMP: { 482 sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); 483 break; 484 } 485 486 case STATE_DISABLING_BARO: { 487 sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0); 488 break; 489 } 490 491 case STATE_DISABLING_TEMP: { 492 sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0); 493 break; 494 } 495 496 case STATE_FINISH_INIT: { 497 sensorRegisterInitComplete(mTask.baroHandle); 498 sensorRegisterInitComplete(mTask.tempHandle); 499 break; 500 } 501 502 case STATE_SAMPLING: { 503 float pressure_Pa, temp_centigrade; 504 getTempAndBaro(xfer->txrxBuf, &pressure_Pa, &temp_centigrade); 505 506 if (mTask.baroOn && mTask.baroReading) { 507 if (mTask.baroCalibrating) { 508 sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, pressure_Pa * 0.01f); 509 510 if (mTask.baroTimerHandle) 511 timTimerCancel(mTask.baroTimerHandle); 512 513 mTask.baroOn = false; 514 mTask.baroCalibrating = false; 515 516 writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, CTRL_SLEEP, STATE_IDLE); 517 } else { 518 sample.fdata = pressure_Pa * 0.01f; 519 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), sample.vptr, NULL); 520 } 521 } 522 523 if (mTask.tempOn && mTask.tempReading) { 524 sample.fdata = temp_centigrade; 525 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), sample.vptr, NULL); 526 } 527 528 mTask.baroReading = false; 529 mTask.tempReading = false; 530 531 break; 532 } 533 534 default: 535 break; 536 } 537 538 xfer->inUse = false; 539 } 540 541 static void handleEvent(uint32_t evtType, const void* evtData) 542 { 543 struct I2cTransfer *newXfer; 544 545 switch (evtType) { 546 case EVT_APP_START: 547 { 548 osEventUnsubscribe(mTask.id, EVT_APP_START); 549 i2cMasterRequest(I2C_BUS_ID, I2C_SPEED); 550 551 /* Reset chip */ 552 writeRegister(BOSCH_BMP280_REG_RESET, 0xB6, STATE_RESET); 553 break; 554 } 555 556 case EVT_SENSOR_I2C: 557 { 558 handleI2cEvent((struct I2cTransfer *)evtData); 559 break; 560 } 561 562 case EVT_SENSOR_BARO_TIMER: 563 { 564 /* Start sampling for a value */ 565 if (!mTask.baroReading && !mTask.tempReading) { 566 newXfer = allocXfer(STATE_SAMPLING); 567 if (newXfer != NULL) { 568 newXfer->txrxBuf[0] = BOSCH_BMP280_REG_PRES_MSB; 569 i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 6, i2cCallback, newXfer); 570 } 571 } 572 573 mTask.baroReading = true; 574 break; 575 } 576 577 case EVT_SENSOR_TEMP_TIMER: 578 { 579 /* Start sampling for a value */ 580 if (!mTask.baroReading && !mTask.tempReading) { 581 newXfer = allocXfer(STATE_SAMPLING); 582 if (newXfer != NULL) { 583 newXfer->txrxBuf[0] = BOSCH_BMP280_REG_PRES_MSB; 584 i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 6, i2cCallback, newXfer); 585 } 586 } 587 588 mTask.tempReading = true; 589 break; 590 } 591 } 592 } 593 594 static bool startTask(uint32_t taskId) 595 { 596 mTask.id = taskId; 597 mTask.offset = 0.0f; 598 599 /* Register sensors */ 600 mTask.baroHandle = sensorRegister(&sensorInfoBaro, &sensorOpsBaro, NULL, false); 601 mTask.tempHandle = sensorRegister(&sensorInfoTemp, &sensorOpsTemp, NULL, false); 602 603 osEventSubscribe(taskId, EVT_APP_START); 604 605 return true; 606 } 607 608 static void endTask(void) 609 { 610 611 } 612 613 INTERNAL_APP_INIT(BMP280_APP_ID, BMP280_APP_VERSION, startTask, endTask, handleEvent); 614
