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
getStateName(int32_t s)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.
trySwitchState(enum st_mag40_SensorState newState)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
magAllocateEvt(struct TripleAxisDataEvent ** evPtr)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
magFreeEvt(void * ptr)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.
allocXfer(void)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
releaseXfer(struct I2cTransfer * xfer)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 */
i2cDelayCallback(uint32_t timerId,void * data)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
i2cCallback(void * cookie,size_t tx,size_t rx,int err)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
i2c_read(uint8_t addr,uint16_t len,uint32_t delay,bool last)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
i2c_write(uint8_t addr,uint8_t data,uint32_t delay,bool last)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
st_mag40_computeOdr(uint32_t rate)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 */
magPower(bool on,void * cookie)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
magFwUpload(void * cookie)473 static bool magFwUpload(void *cookie)
474 {
475 return sensorSignalInternalEvt(mTask.magHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
476 }
477
magSetRate(uint32_t rate,uint64_t latency,void * cookie)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
magFlush(void * cookie)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
magCfgData(void * data,void * cookie)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
sendTestResult(uint8_t status,uint8_t sensorType)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
magTestHandling(struct I2cTransfer * xfer)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
magSelfTest(void * cookie)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
enableInterrupt(struct Gpio * pin,struct ChainedIsr * isr)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
disableInterrupt(struct Gpio * pin,struct ChainedIsr * isr)666 static void disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
667 {
668 extiUnchainIsr(ST_MAG40_INT_IRQ, isr);
669 extiDisableIntGpio(pin);
670 }
671
st_mag40_int1_isr(struct ChainedIsr * isr)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
parseRawData(uint8_t * raw)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
int2Evt(void)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
processPendingEvt(void)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
sensorMagConfig(void)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 */
sensorInit(void)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
handleCommDoneEvt(const void * evtData)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
handleEvent(uint32_t evtType,const void * evtData)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
startTask(uint32_t task_id)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
endTask(void)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