1
2 /*
3 * Copyright (C) 2015-2021 Alibaba Group Holding Limited
4 */
5 #include "drv_acc_gyro_qst_qmi8610.h"
6 #include "aos/kernel.h"
7 #include "ulog/ulog.h"
8 #include <math.h>
9 #include <stdio.h>
10 #include <stdlib.h>
11 #define FIS210X_SLAVE_ADDR 0x6a
12 #define FISIMU_UINT_MG_DPS
13
14 enum {
15 AXIS_X = 0,
16 AXIS_Y = 1,
17 AXIS_Z = 2,
18
19 AXIS_TOTAL
20 };
21
22 typedef struct {
23 short sign[AXIS_TOTAL];
24 unsigned short map[AXIS_TOTAL];
25 } qst_imu_layout;
26
27 static uint16_t acc_lsb_div = 0;
28 static uint16_t gyro_lsb_div = 0;
29 static struct FisImuConfig fisimu_config;
30
FisImu_write_reg(uint8_t reg,uint8_t value)31 uint8_t FisImu_write_reg(uint8_t reg, uint8_t value)
32 {
33 uint8_t ret = 0;
34
35 #if defined(QST_USE_SPI)
36 ret = qst_fis210x_spi_write(reg, value);
37 #elif defined(QST_USE_SW_I2C)
38 ret = qst_sw_writereg(FIS210X_SLAVE_ADDR, reg, value);
39 #else
40 uint8_t write_buf[2];
41 write_buf[0] = reg;
42 write_buf[1] = value;
43 sensor_i2c_master_send(1, FIS210X_SLAVE_ADDR, write_buf, 2, 1000);
44 #endif
45 return ret;
46 }
47
FisImu_write_regs(uint8_t reg,uint8_t * value,uint8_t len)48 uint8_t FisImu_write_regs(uint8_t reg, uint8_t *value, uint8_t len)
49 {
50 uint8_t ret = 0;
51 #if defined(QST_USE_SPI)
52 ret = qst_fis210x_spi_write_bytes(reg, value, len);
53 #elif defined(QST_USE_SW_I2C)
54 ret = qst_sw_writeregs(FIS210X_SLAVE_ADDR, reg, value, len);
55 #else
56 sensor_i2c_mem_write(1, FIS210X_SLAVE_ADDR, reg, 1, &value, len, 100);
57 #endif
58 return ret;
59 }
60
FisImu_read_reg(uint8_t reg,uint8_t * buf,uint16_t len)61 uint8_t FisImu_read_reg(uint8_t reg, uint8_t *buf, uint16_t len)
62 {
63 uint8_t ret = 0;
64
65 #if defined(QST_USE_SPI)
66 ret = qst_fis210x_spi_read(reg, buf, len);
67 #elif defined(QST_USE_SW_I2C)
68 ret = qst_sw_readreg(FIS210X_SLAVE_ADDR, reg, buf, len);
69 #else
70 sensor_i2c_mem_read(1, FIS210X_SLAVE_ADDR, reg, 1, buf, len, 100);
71 #endif
72 return ret;
73 }
74
75
76 static qst_imu_layout imu_map;
77
FisImu_set_layout(short layout)78 void FisImu_set_layout(short layout)
79 {
80 if (layout == 0) {
81 imu_map.sign[AXIS_X] = 1;
82 imu_map.sign[AXIS_Y] = 1;
83 imu_map.sign[AXIS_Z] = 1;
84 imu_map.map[AXIS_X] = AXIS_X;
85 imu_map.map[AXIS_Y] = AXIS_Y;
86 imu_map.map[AXIS_Z] = AXIS_Z;
87 } else if (layout == 1) {
88 imu_map.sign[AXIS_X] = -1;
89 imu_map.sign[AXIS_Y] = 1;
90 imu_map.sign[AXIS_Z] = 1;
91 imu_map.map[AXIS_X] = AXIS_Y;
92 imu_map.map[AXIS_Y] = AXIS_X;
93 imu_map.map[AXIS_Z] = AXIS_Z;
94 } else if (layout == 2) {
95 imu_map.sign[AXIS_X] = -1;
96 imu_map.sign[AXIS_Y] = -1;
97 imu_map.sign[AXIS_Z] = 1;
98 imu_map.map[AXIS_X] = AXIS_X;
99 imu_map.map[AXIS_Y] = AXIS_Y;
100 imu_map.map[AXIS_Z] = AXIS_Z;
101 } else if (layout == 3) {
102 imu_map.sign[AXIS_X] = 1;
103 imu_map.sign[AXIS_Y] = -1;
104 imu_map.sign[AXIS_Z] = 1;
105 imu_map.map[AXIS_X] = AXIS_Y;
106 imu_map.map[AXIS_Y] = AXIS_X;
107 imu_map.map[AXIS_Z] = AXIS_Z;
108 } else if (layout == 4) {
109 imu_map.sign[AXIS_X] = -1;
110 imu_map.sign[AXIS_Y] = 1;
111 imu_map.sign[AXIS_Z] = -1;
112 imu_map.map[AXIS_X] = AXIS_X;
113 imu_map.map[AXIS_Y] = AXIS_Y;
114 imu_map.map[AXIS_Z] = AXIS_Z;
115 } else if (layout == 5) {
116 imu_map.sign[AXIS_X] = 1;
117 imu_map.sign[AXIS_Y] = 1;
118 imu_map.sign[AXIS_Z] = -1;
119 imu_map.map[AXIS_X] = AXIS_Y;
120 imu_map.map[AXIS_Y] = AXIS_X;
121 imu_map.map[AXIS_Z] = AXIS_Z;
122 } else if (layout == 6) {
123 imu_map.sign[AXIS_X] = 1;
124 imu_map.sign[AXIS_Y] = -1;
125 imu_map.sign[AXIS_Z] = -1;
126 imu_map.map[AXIS_X] = AXIS_X;
127 imu_map.map[AXIS_Y] = AXIS_Y;
128 imu_map.map[AXIS_Z] = AXIS_Z;
129 } else if (layout == 7) {
130 imu_map.sign[AXIS_X] = -1;
131 imu_map.sign[AXIS_Y] = -1;
132 imu_map.sign[AXIS_Z] = -1;
133 imu_map.map[AXIS_X] = AXIS_Y;
134 imu_map.map[AXIS_Y] = AXIS_X;
135 imu_map.map[AXIS_Z] = AXIS_Z;
136 } else {
137 imu_map.sign[AXIS_X] = 1;
138 imu_map.sign[AXIS_Y] = 1;
139 imu_map.sign[AXIS_Z] = 1;
140 imu_map.map[AXIS_X] = AXIS_X;
141 imu_map.map[AXIS_Y] = AXIS_Y;
142 imu_map.map[AXIS_Z] = AXIS_Z;
143 }
144 }
145
146
FisImu_config_acc(enum FisImu_AccRange range,enum FisImu_AccOdr odr,enum FisImu_LpfConfig lpfEnable,enum FisImu_HpfConfig hpfEnable)147 void FisImu_config_acc(enum FisImu_AccRange range, enum FisImu_AccOdr odr, enum FisImu_LpfConfig lpfEnable, enum FisImu_HpfConfig hpfEnable)
148 {
149 unsigned char ctl_dada;
150 unsigned char range_set;
151
152 switch (range) {
153 case AccRange_2g:
154 range_set = 0 << 3;
155 acc_lsb_div = (1 << 14);
156 break;
157 case AccRange_4g:
158 range_set = 1 << 3;
159 acc_lsb_div = (1 << 13);
160 break;
161 case AccRange_8g:
162 range_set = 2 << 3;
163 acc_lsb_div = (1 << 12);
164 break;
165 default:
166 range_set = 2 << 3;
167 acc_lsb_div = (1 << 12);
168 }
169 ctl_dada = (unsigned char)range_set | (unsigned char)odr;
170 FisImu_write_reg(FisRegister_Ctrl2, ctl_dada);
171 // set LPF & HPF
172 FisImu_read_reg(FisRegister_Ctrl5, &ctl_dada, 1);
173 ctl_dada &= 0xfc;
174 if (lpfEnable == Lpf_Enable)
175 ctl_dada |= 0x02;
176 if (hpfEnable == Hpf_Enable)
177 ctl_dada |= 0x01;
178 FisImu_write_reg(FisRegister_Ctrl5, ctl_dada);
179 // set LPF & HPF
180 }
181
FisImu_config_gyro(enum FisImu_GyrRange range,enum FisImu_GyrOdr odr,enum FisImu_LpfConfig lpfEnable,enum FisImu_HpfConfig hpfEnable)182 void FisImu_config_gyro(enum FisImu_GyrRange range, enum FisImu_GyrOdr odr, enum FisImu_LpfConfig lpfEnable, enum FisImu_HpfConfig hpfEnable)
183 {
184 // Set the CTRL3 register to configure dynamic range and ODR
185 unsigned char ctl_dada;
186 ctl_dada = (unsigned char)range | (unsigned char)odr;
187 FisImu_write_reg(FisRegister_Ctrl3, ctl_dada);
188
189 // Store the scale factor for use when processing raw data
190 switch (range) {
191 case GyrRange_32dps:
192 gyro_lsb_div = 1024;
193 break;
194 case GyrRange_64dps:
195 gyro_lsb_div = 512;
196 break;
197 case GyrRange_128dps:
198 gyro_lsb_div = 256;
199 break;
200 case GyrRange_256dps:
201 gyro_lsb_div = 128;
202 break;
203 case GyrRange_512dps:
204 gyro_lsb_div = 64;
205 break;
206 case GyrRange_1024dps:
207 gyro_lsb_div = 32;
208 break;
209 case GyrRange_2048dps:
210 gyro_lsb_div = 16;
211 break;
212 case GyrRange_2560dps:
213 // gyro_lsb_div = 8;
214 break;
215 default:
216 gyro_lsb_div = 32;
217 break;
218 }
219
220 // Conversion from degrees/s to rad/s if necessary
221 // set LPF & HPF
222 FisImu_read_reg(FisRegister_Ctrl5, &ctl_dada, 1);
223 ctl_dada &= 0xf3;
224 if (lpfEnable == Lpf_Enable)
225 ctl_dada |= 0x08;
226 if (hpfEnable == Hpf_Enable)
227 ctl_dada |= 0x04;
228 FisImu_write_reg(FisRegister_Ctrl5, ctl_dada);
229 // set LPF & HPF
230 }
231
FisImu_config_ae(enum FisImu_AeOdr odr)232 void FisImu_config_ae(enum FisImu_AeOdr odr)
233 {
234 // Configure Accelerometer and Gyroscope settings
235 FisImu_config_acc(AccRange_8g, AccOdr_1024Hz, Lpf_Enable, Hpf_Disable);
236 FisImu_config_gyro(GyrRange_2048dps, GyrOdr_1024Hz, Lpf_Enable, Hpf_Disable);
237 FisImu_write_reg(FisRegister_Ctrl6, odr);
238 }
239
FisImu_config_mag(enum FisImu_MagDev device,enum FisImu_MagOdr odr)240 void FisImu_config_mag(enum FisImu_MagDev device, enum FisImu_MagOdr odr)
241 {
242
243 }
244
FisImu_readStatus0(void)245 uint8_t FisImu_readStatus0(void)
246 {
247 uint8_t status;
248 FisImu_read_reg(FisRegister_Status0, &status, sizeof(status));
249 return status;
250 }
251 /*!
252 * \brief Blocking read of data status register 1 (::FisRegister_Status1).
253 * \returns Status byte \see STATUS1 for flag definitions.
254 */
FisImu_readStatus1(void)255 uint8_t FisImu_readStatus1(void)
256 {
257 uint8_t status;
258 FisImu_read_reg(FisRegister_Status1, &status, sizeof(status));
259 return status;
260 }
261
FisImu_readTemp(void)262 int8_t FisImu_readTemp(void)
263 {
264 int8_t temp;
265 FisImu_read_reg(FisRegister_Temperature, (uint8_t *)&temp, sizeof(temp));
266 return temp;
267 }
268
FisImu_read_acc_xyz(float acc_xyz[3])269 void FisImu_read_acc_xyz(float acc_xyz[3])
270 {
271 unsigned char buf_reg[6];
272 short raw_acc_xyz[3];
273
274 #if defined(QST_USE_SPI)
275 FisImu_read_reg(FisRegister_Ax_L, &buf_reg[0], 6); // 0x19, 25
276 #else
277 FisImu_read_reg(FisRegister_Ax_L, &buf_reg[0], 1); // 0x19, 25
278 FisImu_read_reg(FisRegister_Ax_H, &buf_reg[1], 1);
279 FisImu_read_reg(FisRegister_Ay_L, &buf_reg[2], 1);
280 FisImu_read_reg(FisRegister_Ay_H, &buf_reg[3], 1);
281 FisImu_read_reg(FisRegister_Az_L, &buf_reg[4], 1);
282 FisImu_read_reg(FisRegister_Az_H, &buf_reg[5], 1);
283 #endif
284 raw_acc_xyz[0] = (short)((buf_reg[1] << 8) | (buf_reg[0]));
285 raw_acc_xyz[1] = (short)((buf_reg[3] << 8) | (buf_reg[2]));
286 raw_acc_xyz[2] = (short)((buf_reg[5] << 8) | (buf_reg[4]));
287 // if(cali_flag == FALSE)
288 // {
289 // raw_acc_xyz[0]+=offset[0];
290 // raw_acc_xyz[1]+=offset[1];
291 // raw_acc_xyz[2]+=offset[2];
292 // }
293
294 acc_xyz[0] = (raw_acc_xyz[0] * ONE_G) / acc_lsb_div;
295 acc_xyz[1] = (raw_acc_xyz[1] * ONE_G) / acc_lsb_div;
296 acc_xyz[2] = (raw_acc_xyz[2] * ONE_G) / acc_lsb_div;
297 // acc_layout = 0;
298 // acc_xyz[qst_map[acc_layout].map[0]] = qst_map[acc_layout].sign[0]*acc_xyz[0];
299 // acc_xyz[qst_map[acc_layout].map[1]] = qst_map[acc_layout].sign[1]*acc_xyz[1];
300 // acc_xyz[qst_map[acc_layout].map[2]] = qst_map[acc_layout].sign[2]*acc_xyz[2];
301 // LOGD("SENSOR", "fis210x acc: %f %f %f\n", acc_xyz[0], acc_xyz[1], acc_xyz[2]);
302 }
303
FisImu_read_gyro_xyz(float gyro_xyz[3])304 void FisImu_read_gyro_xyz(float gyro_xyz[3])
305 {
306 unsigned char buf_reg[6];
307 short raw_gyro_xyz[3];
308
309 #if defined(QST_USE_SPI)
310 FisImu_read_reg(FisRegister_Gx_L, &buf_reg[0], 6); // 0x19, 25
311 #else
312 FisImu_read_reg(FisRegister_Gx_L, &buf_reg[0], 1);
313 FisImu_read_reg(FisRegister_Gx_H, &buf_reg[1], 1);
314 FisImu_read_reg(FisRegister_Gy_L, &buf_reg[2], 1);
315 FisImu_read_reg(FisRegister_Gy_H, &buf_reg[3], 1);
316 FisImu_read_reg(FisRegister_Gz_L, &buf_reg[4], 1);
317 FisImu_read_reg(FisRegister_Gz_H, &buf_reg[5], 1);
318 #endif
319 raw_gyro_xyz[0] = (short)((buf_reg[1] << 8) | (buf_reg[0]));
320 raw_gyro_xyz[1] = (short)((buf_reg[3] << 8) | (buf_reg[2]));
321 raw_gyro_xyz[2] = (short)((buf_reg[5] << 8) | (buf_reg[4]));
322 // if (cali_flag == FALSE)
323 // {
324 // raw_gyro_xyz[0] += offset_gyro[0];
325 // raw_gyro_xyz[1] += offset_gyro[1];
326 // raw_gyro_xyz[2] += offset_gyro[2];
327 // }
328
329 gyro_xyz[0] = (raw_gyro_xyz[0] * 1.0f) / gyro_lsb_div;
330 gyro_xyz[1] = (raw_gyro_xyz[1] * 1.0f) / gyro_lsb_div;
331 gyro_xyz[2] = (raw_gyro_xyz[2] * 1.0f) / gyro_lsb_div;
332 // acc_layout = 0;
333 // gyro_xyz[qst_map[acc_layout].map[0]] = qst_map[acc_layout].sign[0]*gyro_xyz[0];
334 // gyro_xyz[qst_map[acc_layout].map[1]] = qst_map[acc_layout].sign[1]*gyro_xyz[1];
335 // gyro_xyz[qst_map[acc_layout].map[2]] = qst_map[acc_layout].sign[2]*gyro_xyz[2];
336 LOGD("SENSOR", "fis210x gyro: %f %f %f\n", gyro_xyz[0], gyro_xyz[1], gyro_xyz[2]);
337 }
338
FisImu_read_xyz(float acc[3],float gyro[3])339 void FisImu_read_xyz(float acc[3], float gyro[3])
340 {
341 unsigned char buf_reg[12];
342 short raw_acc_xyz[3];
343 short raw_gyro_xyz[3];
344 float acc_t[3];
345 float gyro_t[3];
346
347 #if defined(QST_USE_SPI)
348 if (tim_count) {
349 FisImu_read_reg(FisRegister_CountOut, tim_count, 1); // 0x18 24
350 }
351 FisImu_read_reg(FisRegister_Ax_L, &buf_reg[0], 12); // 0x19, 25
352 #else
353 FisImu_read_reg(FisRegister_Ax_L | 0x80, buf_reg, 12); // 0x19, 25
354 #endif
355 raw_acc_xyz[0] = (short)((buf_reg[1] << 8) | (buf_reg[0]));
356 raw_acc_xyz[1] = (short)((buf_reg[3] << 8) | (buf_reg[2]));
357 raw_acc_xyz[2] = (short)((buf_reg[5] << 8) | (buf_reg[4]));
358
359
360 raw_gyro_xyz[0] = (short)((buf_reg[7] << 8) | (buf_reg[6]));
361 raw_gyro_xyz[1] = (short)((buf_reg[9] << 8) | (buf_reg[8]));
362 raw_gyro_xyz[2] = (short)((buf_reg[11] << 8) | (buf_reg[10]));
363
364 // m/s2
365 acc_t[AXIS_X] = (float)(raw_acc_xyz[AXIS_X] * ONE_G) / acc_lsb_div;
366 acc_t[AXIS_Y] = (float)(raw_acc_xyz[AXIS_Y] * ONE_G) / acc_lsb_div;
367 acc_t[AXIS_Z] = (float)(raw_acc_xyz[AXIS_Z] * ONE_G) / acc_lsb_div;
368
369 acc[AXIS_X] = -acc_t[AXIS_X]; // imu_map.sign[AXIS_X]*acc_t[imu_map.map[AXIS_X]];
370 acc[AXIS_Y] = -acc_t[AXIS_Y]; // imu_map.sign[AXIS_Y]*acc_t[imu_map.map[AXIS_Y]];
371 acc[AXIS_Z] = acc_t[AXIS_Z]; // imu_map.sign[AXIS_Z]*acc_t[imu_map.map[AXIS_Z]];
372
373 // rad/s
374 gyro_t[AXIS_X] = (float)(raw_gyro_xyz[AXIS_X] * M_PI / 180) / gyro_lsb_div; // *pi/180
375 gyro_t[AXIS_Y] = (float)(raw_gyro_xyz[AXIS_Y] * M_PI / 180) / gyro_lsb_div;
376 gyro_t[AXIS_Z] = (float)(raw_gyro_xyz[AXIS_Z] * M_PI / 180) / gyro_lsb_div;
377
378 gyro[AXIS_X] = gyro_t[AXIS_X]; // imu_map.sign[AXIS_X]*gyro_t[imu_map.map[AXIS_X]];
379 gyro[AXIS_Y] = gyro_t[AXIS_Y]; // imu_map.sign[AXIS_Y]*gyro_t[imu_map.map[AXIS_Y]];
380 gyro[AXIS_Z] = -gyro_t[AXIS_Z]; // imu_map.sign[AXIS_Z]*gyro_t[imu_map.map[AXIS_Z]];
381 }
382
383 // for XKF3
applyScaleFactor(float scaleFactor,uint8_t nElements,uint8_t const * rawData,float * calibratedData)384 static inline void applyScaleFactor(float scaleFactor, uint8_t nElements, uint8_t const *rawData, float *calibratedData)
385 {
386 for (int i = 0; i < nElements; ++i) {
387 calibratedData[i] = scaleFactor * (int16_t)((uint16_t)rawData[2 * i] | ((uint16_t)rawData[2 * i + 1] << 8));
388 }
389 }
390
FisImu_processAccelerometerData(uint8_t const * rawData,float * calibratedData)391 void FisImu_processAccelerometerData(uint8_t const *rawData, float *calibratedData)
392 {
393 applyScaleFactor((float)(ONE_G / acc_lsb_div), 3, rawData, calibratedData);
394 }
395
FisImu_processGyroscopeData(uint8_t const * rawData,float * calibratedData)396 void FisImu_processGyroscopeData(uint8_t const *rawData, float *calibratedData)
397 {
398 applyScaleFactor((float)(M_PI / (gyro_lsb_div * 180)), 3, rawData, calibratedData);
399 }
400
FisImu_read_rawsample(struct FisImuRawSample * sample)401 void FisImu_read_rawsample(struct FisImuRawSample *sample)
402 {
403 FisImu_read_reg(FisRegister_CountOut, &(sample->sampleCounter), sizeof(uint8_t));
404 sample->accelerometerData = sample->sampleBuffer;
405 sample->gyroscopeData = sample->sampleBuffer + FISIMU_SAMPLE_SIZE;
406 FisImu_read_reg(FisRegister_Ax_L, (uint8_t *)sample->accelerometerData, FISIMU_SAMPLE_SIZE);
407 FisImu_read_reg(FisRegister_Gx_L, (uint8_t *)sample->gyroscopeData, FISIMU_SAMPLE_SIZE);
408 }
409
writeCalibrationVectorBuffer(float const * calVector,float conversionFactor,uint8_t fractionalBits)410 static void writeCalibrationVectorBuffer(float const *calVector, float conversionFactor, uint8_t fractionalBits)
411 {
412 int i;
413 int16_t o;
414 uint8_t calCmd[6];
415 // calCmd[0] = FisRegister_Cal1_L;
416
417 for (i = 0; i < 3; ++i) {
418 o = (int16_t)roundf(calVector[i] * conversionFactor * (1 << fractionalBits));
419 calCmd[(2 * i)] = o & 0xFF;
420 calCmd[(2 * i) + 1] = o >> 8;
421 }
422 FisImu_write_regs(FisRegister_Cal1_L, calCmd, sizeof(calCmd));
423 }
424
FisImu_doCtrl9Command(enum FisImu_Ctrl9Command cmd)425 void FisImu_doCtrl9Command(enum FisImu_Ctrl9Command cmd)
426 {
427 uint8_t gyroConfig;
428 const uint8_t oisModeBits = 0x06;
429 unsigned char oisEnabled;
430 uint8_t status = 0;
431 uint32_t count = 0;
432
433 FisImu_read_reg(FisRegister_Ctrl3, &gyroConfig, sizeof(gyroConfig));
434 oisEnabled = ((gyroConfig & oisModeBits) == oisModeBits);
435 if (oisEnabled) {
436 FisImu_write_reg(FisRegister_Ctrl3, (gyroConfig & ~oisModeBits));
437 }
438
439 // g_fisDriverHal->waitForEvent(Fis_Int1, FisInt_low);
440 // aos_msleep(300);
441
442 FisImu_write_reg(FisRegister_Ctrl9, cmd);
443 // g_fisDriverHal->waitForEvent(Fis_Int1, FisInt_high);
444 // aos_msleep(300);
445
446 // Check that command has been executed
447 while (((status & FISIMU_STATUS1_CMD_DONE) == 0) && (count < 10000)) {
448 FisImu_read_reg(FisRegister_Status1, &status, sizeof(status));
449 count++;
450 }
451 // assert(status & FISIMU_STATUS1_CMD_DONE);
452
453 // g_fisDriverHal->waitForEvent(Fis_Int1, FisInt_low);
454 // aos_msleep(300);
455
456 if (oisEnabled) {
457 // Re-enable OIS mode configuration if necessary
458 FisImu_write_reg(FisRegister_Ctrl3, gyroConfig);
459 }
460 }
461
FisImu_applyAccelerometerOffset(float const * offset,enum FisImu_AccUnit unit)462 void FisImu_applyAccelerometerOffset(float const *offset, enum FisImu_AccUnit unit)
463 {
464 const float conversionFactor = (unit == AccUnit_ms2) ? (1 / ONE_G) : 1;
465 writeCalibrationVectorBuffer(offset, conversionFactor, 11);
466 FisImu_doCtrl9Command(Ctrl9_SetAccelOffset);
467 }
468
FisImu_applyGyroscopeOffset(float const * offset,enum FisImu_GyrUnit unit)469 void FisImu_applyGyroscopeOffset(float const *offset, enum FisImu_GyrUnit unit)
470 {
471 const float conversionFactor = (unit == GyrUnit_rads) ? 180 / M_PI : 1;
472 writeCalibrationVectorBuffer(offset, conversionFactor, 6);
473 FisImu_doCtrl9Command(Ctrl9_SetGyroOffset);
474 }
475
FisImu_applyOffsetCalibration(struct FisImu_offsetCalibration const * cal)476 void FisImu_applyOffsetCalibration(struct FisImu_offsetCalibration const *cal)
477 {
478 FisImu_applyAccelerometerOffset(cal->accOffset, cal->accUnit);
479 FisImu_applyGyroscopeOffset(cal->gyrOffset, cal->gyrUnit);
480 }
481
482 // for XKF3
483
FisImu_enableWakeOnMotion(void)484 void FisImu_enableWakeOnMotion(void)
485 {
486 uint8_t womCmd[3];
487 enum FisImu_Interrupt interrupt = Fis_Int1;
488 enum FisImu_InterruptInitialState initialState = InterruptInitialState_low;
489 enum FisImu_WakeOnMotionThreshold threshold = WomThreshold_low;
490 uint8_t blankingTime = 0x00;
491 const uint8_t blankingTimeMask = 0x3F;
492
493 FisImu_enableSensors(FISIMU_CTRL7_DISABLE_ALL);
494 // FisImu_config_acc(AccRange_2g, AccOdr_LowPower_3Hz, Lpf_Disable, Hpf_Disable);
495 FisImu_config_acc(AccRange_2g, AccOdr_LowPower_25Hz, Lpf_Disable, Hpf_Disable);
496
497 womCmd[0] = FisRegister_Cal1_L; // WoM Threshold: absolute value in mg (with 1mg/LSB resolution)
498 womCmd[1] = threshold;
499 womCmd[2] = (uint8_t)interrupt | (uint8_t)initialState | (blankingTime & blankingTimeMask);
500 FisImu_write_reg(FisRegister_Cal1_L, womCmd[1]);
501 FisImu_write_reg(FisRegister_Cal1_H, womCmd[2]);
502
503 FisImu_doCtrl9Command(Ctrl9_ConfigureWakeOnMotion);
504 FisImu_enableSensors(FISIMU_CTRL7_ACC_ENABLE);
505 }
506
FisImu_disableWakeOnMotion(void)507 void FisImu_disableWakeOnMotion(void)
508 {
509 FisImu_enableSensors(FISIMU_CTRL7_DISABLE_ALL);
510 FisImu_write_reg(FisRegister_Cal1_L, 0);
511 FisImu_doCtrl9Command(Ctrl9_ConfigureWakeOnMotion);
512 }
513
FisImu_enableSensors(uint8_t enableFlags)514 void FisImu_enableSensors(uint8_t enableFlags)
515 {
516 if (enableFlags & FISIMU_CONFIG_AE_ENABLE) {
517 enableFlags |= FISIMU_CTRL7_ACC_ENABLE | FISIMU_CTRL7_GYR_ENABLE;
518 }
519
520 FisImu_write_reg(FisRegister_Ctrl7, enableFlags & FISIMU_CTRL7_ENABLE_MASK);
521 }
522
523
FisImu_Config_apply(struct FisImuConfig const * config)524 void FisImu_Config_apply(struct FisImuConfig const *config)
525 {
526 uint8_t fisSensors = config->inputSelection;
527
528 if (fisSensors & FISIMU_CONFIG_AE_ENABLE) {
529 FisImu_config_ae(config->aeOdr);
530 } else {
531 if (config->inputSelection & FISIMU_CONFIG_ACC_ENABLE) {
532 // FisImu_config_acc(config->accRange, config->accOdr, Lpf_Disable, Hpf_Disable);
533 FisImu_config_acc(config->accRange, config->accOdr, Lpf_Enable, Hpf_Disable);
534 }
535 if (config->inputSelection & FISIMU_CONFIG_GYR_ENABLE) {
536 // FisImu_config_gyro(config->gyrRange, config->gyrOdr, Lpf_Disable, Hpf_Disable);
537 FisImu_config_gyro(config->gyrRange, config->gyrOdr, Lpf_Enable, Hpf_Disable);
538 }
539 }
540
541 if (config->inputSelection & FISIMU_CONFIG_MAG_ENABLE) {
542 FisImu_config_mag(config->magDev, config->magOdr);
543 }
544 FisImu_enableSensors(fisSensors);
545 }
546
FisImu_init(void)547 uint8_t FisImu_init(void)
548 {
549 uint8_t chip_id = 0x00;
550
551 int32_t ret = sensor_i2c_open(1, FIS210X_SLAVE_ADDR, I2C_BUS_BIT_RATES_100K, 0);
552 if (ret) {
553 LOGE("SENSOR", "sensor i2c open 0x%x:0x%x failed, ret:%d\n", 0x1, FIS210X_SLAVE_ADDR, ret);
554 return;
555 }
556
557 FisImu_read_reg(FisRegister_WhoAmI, &chip_id, 1);
558 aos_msleep(100);
559 if (chip_id == 0xfc) {
560 fisimu_config.inputSelection = FISIMU_CONFIG_ACCGYR_ENABLE;
561 fisimu_config.accRange = AccRange_4g;
562 fisimu_config.accOdr = AccOdr_128Hz;
563 fisimu_config.gyrRange = GyrRange_1024dps; // GyrRange_1024dps;
564 fisimu_config.gyrOdr = GyrOdr_256Hz; // GyrOdr_1024Hz;
565 fisimu_config.magOdr = MagOdr_32Hz;
566 fisimu_config.magDev = MagDev_AK8963;
567 fisimu_config.aeOdr = AeOdr_32Hz;
568 aos_msleep(100);
569
570 FisImu_Config_apply(&fisimu_config);
571 aos_msleep(100);
572 FisImu_set_layout(2);
573 } else {
574 chip_id = 0;
575 }
576
577 return chip_id;
578 }
579
FisImu_deinit(void)580 void FisImu_deinit(void)
581 {
582 int32_t ret = sensor_i2c_close(1);
583 if (ret) {
584 LOGE("SENSOR", "sensor i2c close failed, ret:%d\n", ret);
585 }
586
587 return;
588 }
589