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
baroAllocateEvt(struct SingleAxisDataEvent ** evPtr,float sample,uint64_t time)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
baroFreeEvt(void * ptr)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.
allocXfer(uint8_t state)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
releaseXfer(struct I2cTransfer * xfer)230 static inline void releaseXfer(struct I2cTransfer *xfer)
231 {
232 xfer->inUse = false;
233 }
234
i2cCallback(void * cookie,size_t tx,size_t rx,int err)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
i2c_read(uint8_t addr,uint16_t len,uint32_t delay,uint8_t state)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
i2c_write(uint8_t addr,uint8_t data,uint32_t delay,uint8_t state)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
sendCalibrationResult(uint8_t status,float value)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 */
sensorBaroTimerCallback(uint32_t timerId,void * data)304 static void sensorBaroTimerCallback(uint32_t timerId, void *data)
305 {
306 osEnqueuePrivateEvt(EVT_SENSOR_BARO_TIMER, data, NULL, mTask.tid);
307 }
308
sensorTempTimerCallback(uint32_t timerId,void * data)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_AMBIENT_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP,
347 20, lps22hbRates) },
348 };
349
350 /* Sensor Operations */
baroPower(bool on,void * cookie)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
baroFwUpload(void * cookie)384 static bool baroFwUpload(void *cookie)
385 {
386 return sensorSignalInternalEvt(mTask.sensors[BARO].handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
387 }
388
baroSetRate(uint32_t rate,uint64_t latency,void * cookie)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
baroFlush(void * cookie)403 static bool baroFlush(void *cookie)
404 {
405 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
406 }
407
baroCalibrate(void * cookie)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 */
baroCfgData(void * data,void * cookie)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
tempPower(bool on,void * cookie)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
tempFwUpload(void * cookie)481 static bool tempFwUpload(void *cookie)
482 {
483 return sensorSignalInternalEvt(mTask.sensors[TEMP].handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
484 }
485
tempSetRate(uint32_t rate,uint64_t latency,void * cookie)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
tempFlush(void * cookie)499 static bool tempFlush(void *cookie)
500 {
501 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_AMBIENT_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
handleCommDoneEvt(const void * evtData)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_AMBIENT_TEMP), sample.vptr, NULL);
625 }
626
627 break;
628
629 default:
630 break;
631 }
632
633 releaseXfer(xfer);
634 return (0);
635 }
636
handleEvent(uint32_t evtType,const void * evtData)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
startTask(uint32_t task_id)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
endTask(void)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