1 /*
2  * Copyright (C) 2015-2021 Alibaba Group Holding Limited
3  */
4 
5 #include "drv_mag_qst_qmc6310.h"
6 #include "aos/kernel.h"
7 #include "ulog/ulog.h"
8 #include <math.h>
9 #include <stdlib.h>
10 
11 #define QMC6310_I2C_PORT 1
12 uint8_t chipid = 0;
13 uint8_t mag_addr;
14 static int16_t x_max, x_min, z_min, y_max, y_min, z_max;
15 static uint8_t addr;
16 static uint8_t mode;
17 static uint8_t rate;
18 static uint8_t range;
19 static uint8_t oversampling;
20 
21 static unsigned char sensor_mask[4] = {
22     0x80,
23     0xA0,
24     0xB0,
25     0xC0
26 };
27 
28 static QMC6310_map c_map;
29 
qmc6310_i2c_master_send(const uint8_t * data,uint16_t size,uint32_t timeout)30 int qmc6310_i2c_master_send(const uint8_t *data, uint16_t size, uint32_t timeout)
31 {
32     return sensor_i2c_master_send(1, mag_addr, data, size, timeout);
33 }
34 
qmc6310_i2c_master_recv(uint8_t * data,uint16_t size,uint32_t timeout)35 int32_t qmc6310_i2c_master_recv(uint8_t *data, uint16_t size, uint32_t timeout)
36 {
37     return sensor_i2c_master_recv(1, mag_addr, data, size, timeout);
38 }
39 
qmc6310_read_register(uint8_t addr,uint8_t reg)40 uint8_t qmc6310_read_register(uint8_t addr, uint8_t reg)
41 {
42     uint8_t data;
43     qmc6310_i2c_master_send(&reg, 1, 1000);
44     aos_msleep(2);
45     qmc6310_i2c_master_recv(&data, 1, 1000);
46     return data;
47 }
48 
qmc6310_read_block(uint8_t addr,uint8_t * data,uint8_t len)49 int qmc6310_read_block(uint8_t addr, uint8_t *data, uint8_t len)
50 {
51 #if defined(QST_USE_SW_I2C)
52     return qst_sw_readreg(mag_addr, addr, data, len);
53 #else
54     sensor_i2c_mem_read(1, mag_addr, addr, 1, data, len, 100);
55     return 1;
56 #endif
57 }
58 
qmc6310_write_reg(uint8_t addr,uint8_t data)59 int qmc6310_write_reg(uint8_t addr, uint8_t data)
60 {
61 #if defined(QST_USE_SW_I2C)
62     return qst_sw_writereg(mag_addr, addr, data);
63 #else
64     uint8_t write_buffer[2] = {addr, data};
65     qmc6310_i2c_master_send(write_buffer, 2, 1000);
66 #endif
67 
68 }
69 
qmc6310_set_layout(int layout)70 static void qmc6310_set_layout(int layout)
71 {
72     if (layout == 0) {
73         c_map.sign[AXIS_X] = 1;
74         c_map.sign[AXIS_Y] = 1;
75         c_map.sign[AXIS_Z] = 1;
76         c_map.map[AXIS_X] = AXIS_X;
77         c_map.map[AXIS_Y] = AXIS_Y;
78         c_map.map[AXIS_Z] = AXIS_Z;
79     } else if (layout == 1) {
80         c_map.sign[AXIS_X] = -1;
81         c_map.sign[AXIS_Y] = 1;
82         c_map.sign[AXIS_Z] = 1;
83         c_map.map[AXIS_X] = AXIS_Y;
84         c_map.map[AXIS_Y] = AXIS_X;
85         c_map.map[AXIS_Z] = AXIS_Z;
86     } else if (layout == 2) {
87         c_map.sign[AXIS_X] = -1;
88         c_map.sign[AXIS_Y] = -1;
89         c_map.sign[AXIS_Z] = 1;
90         c_map.map[AXIS_X] = AXIS_X;
91         c_map.map[AXIS_Y] = AXIS_Y;
92         c_map.map[AXIS_Z] = AXIS_Z;
93     } else if (layout == 3) {
94         c_map.sign[AXIS_X] = 1;
95         c_map.sign[AXIS_Y] = -1;
96         c_map.sign[AXIS_Z] = 1;
97         c_map.map[AXIS_X] = AXIS_Y;
98         c_map.map[AXIS_Y] = AXIS_X;
99         c_map.map[AXIS_Z] = AXIS_Z;
100     } else if (layout == 4) {
101         c_map.sign[AXIS_X] = -1;
102         c_map.sign[AXIS_Y] = 1;
103         c_map.sign[AXIS_Z] = -1;
104         c_map.map[AXIS_X] = AXIS_X;
105         c_map.map[AXIS_Y] = AXIS_Y;
106         c_map.map[AXIS_Z] = AXIS_Z;
107     } else if (layout == 5) {
108         c_map.sign[AXIS_X] = 1;
109         c_map.sign[AXIS_Y] = 1;
110         c_map.sign[AXIS_Z] = -1;
111         c_map.map[AXIS_X] = AXIS_Y;
112         c_map.map[AXIS_Y] = AXIS_X;
113         c_map.map[AXIS_Z] = AXIS_Z;
114     } else if (layout == 6) {
115         c_map.sign[AXIS_X] = 1;
116         c_map.sign[AXIS_Y] = -1;
117         c_map.sign[AXIS_Z] = -1;
118         c_map.map[AXIS_X] = AXIS_X;
119         c_map.map[AXIS_Y] = AXIS_Y;
120         c_map.map[AXIS_Z] = AXIS_Z;
121     } else if (layout == 7) {
122         c_map.sign[AXIS_X] = -1;
123         c_map.sign[AXIS_Y] = -1;
124         c_map.sign[AXIS_Z] = -1;
125         c_map.map[AXIS_X] = AXIS_Y;
126         c_map.map[AXIS_Y] = AXIS_X;
127         c_map.map[AXIS_Z] = AXIS_Z;
128     } else {
129         c_map.sign[AXIS_X] = 1;
130         c_map.sign[AXIS_Y] = 1;
131         c_map.sign[AXIS_Z] = 1;
132         c_map.map[AXIS_X] = AXIS_X;
133         c_map.map[AXIS_Y] = AXIS_Y;
134         c_map.map[AXIS_Z] = AXIS_Z;
135     }
136 }
137 
qmc6310_get_chipid(void)138 static int qmc6310_get_chipid(void)
139 {
140     int ret = 0;
141     ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid, 1);
142     if (ret == 0) {
143         LOGD("SENSOR", "change_iic_addr = 0x3c\n");
144         mag_addr = QMC6310N_IIC_ADDR;
145     } else {
146         LOGE("SENSOR", "QMC6310_get_chipid chipid = 0x%x\n", chipid);
147         return 1;
148     }
149 
150     ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid, 1);
151     if (ret == 0) {
152         LOGD("SENSOR", "change_iic_addr = 0x1c\n");
153         mag_addr = QMC6310U_IIC_ADDR;
154     } else {
155         LOGD("SENSOR", "QMC6310N_get_chipid chipid = 0x%x,i2c_addr = 0x3c\n", chipid);
156         return 1;
157     }
158 
159     ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid, 1);
160     if (ret == 0) {
161         LOGD("SENSOR", "Get_Chip_ID_Failed!\n");
162         return 0;
163     } else {
164         LOGD("SENSOR", "QMC6310U_get_chipid chipid = 0x%x,i2c_addr = 0x1c\n", chipid);
165         return 1;
166     }
167     return 1;
168 }
169 
qmc6310_read_mag_xyz(float * data)170 uint8_t qmc6310_read_mag_xyz(float *data)
171 {
172     int res;
173     unsigned char mag_data[6];
174     short hw_d[3] = {0};
175     short raw_c[3];
176     int t1 = 0;
177     unsigned char rdy = 0;
178 
179     /* Check status register for data availability */
180     while (!(rdy & 0x01) && (t1 < 5)) {
181         rdy = QMC6310_STATUS_REG;
182         res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
183         t1++;
184     }
185 
186     mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
187 
188     res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
189     if (res == 0) {
190         return 0;
191     }
192 
193     hw_d[0] = (short)(((mag_data[1]) << 8) | mag_data[0]);
194     hw_d[1] = (short)(((mag_data[3]) << 8) | mag_data[2]);
195     hw_d[2] = (short)(((mag_data[5]) << 8) | mag_data[4]);
196 #if 1
197     // Unit:mG  1G = 100uT = 1000mG
198     // printf("Hx=%d, Hy=%d, Hz=%d\n",hw_d[0],hw_d[1],hw_d[2]);
199     raw_c[AXIS_X] = (int)(c_map.sign[AXIS_X] * hw_d[c_map.map[AXIS_X]]);
200     raw_c[AXIS_Y] = (int)(c_map.sign[AXIS_Y] * hw_d[c_map.map[AXIS_Y]]);
201     raw_c[AXIS_Z] = (int)(c_map.sign[AXIS_Z] * hw_d[c_map.map[AXIS_Z]]);
202 
203     data[0] = (float)raw_c[0] / 10.0f;
204     data[1] = (float)raw_c[1] / 10.0f;
205     data[2] = (float)raw_c[2] / 10.0f;
206 #endif
207     return res;
208 }
209 
qmc6310_resetCalibration()210 void qmc6310_resetCalibration()
211 {
212     x_max = y_max = z_max = INT16_MIN;
213     x_min = y_min = z_min = INT16_MAX;
214 }
215 
qmc6310_readHeading()216 int qmc6310_readHeading()
217 {
218     int16_t x_org, y_org, z_org;
219     float   x_offset, y_offset, z_offset;
220     float   x_fit, y_fit, z_fit;
221     int res;
222     unsigned char mag_data[6];
223     short hw_d[3] = {0};
224     short raw_c[3];
225     float data[3];
226     int t1 = 0;
227     unsigned char rdy = 0;
228 
229     /* Check status register for data availability */
230     while (!(rdy & 0x01) && (t1 < 5)) {
231         rdy = QMC6310_STATUS_REG;
232         res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
233         t1++;
234     }
235 
236     mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
237 
238     res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
239     if (res == 0) {
240         return 0;
241     }
242 
243     x_org = (short)(((mag_data[1]) << 8) | mag_data[0]);
244     y_org = (short)(((mag_data[3]) << 8) | mag_data[2]);
245     z_org = (short)(((mag_data[5]) << 8) | mag_data[4]);
246     LOGD("SENSOR", "org[%d,%d,%d],\n", x_org, y_org, z_org);
247 
248     x_min = x_org < x_min ? x_org : x_min;
249     x_max = x_org > x_max ? x_org : x_max;
250     y_min = y_org < y_min ? y_org : y_min;
251     y_max = y_org > y_max ? y_org : y_max;
252     z_min = z_org < z_min ? z_org : z_min;
253     z_max = z_org > z_max ? z_org : z_max;
254 
255     /* Bail out if not enough data is available. */
256     if (x_min == x_max || y_min == y_max || z_max == z_min)
257         return 0;
258 
259     /* Recenter the measurement by subtracting the average */
260     x_offset = (x_max + x_min) / 2.0;
261     y_offset = (y_max + y_min) / 2.0;
262     z_offset = (z_max + z_min) / 2.0;
263 
264     x_fit = (x_org - x_offset) * 1000.0 / (x_max - x_min);
265     y_fit = (y_org - y_offset) * 1000.0 / (y_max - y_min);
266     z_fit = (z_org - z_offset) * 1000.0 / (z_max - z_min);
267     // printf("fix[%f,%f,%f],\n", x_fit, y_fit, z_fit);
268     LOGD("SENSOR", "fix[%f,%f,%f],\n", x_fit, y_fit, z_fit);
269 
270     int heading = 180.0 * atan2(x_fit, y_fit) / M_PI;
271     heading     = (heading <= 0) ? (heading + 360) : heading;
272 
273     return heading;
274 
275 }
276 
277 /* Set the sensor mode */
qmc6310_set_mode(unsigned char mode)278 int qmc6310_set_mode(unsigned char mode)
279 {
280     int err = 0;
281     unsigned char ctrl1_value = 0;
282 
283     err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
284     ctrl1_value = (ctrl1_value & (~0x03)) | mode;
285     LOGD("SENSOR", "QMC6310_set_mode, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE, ctrl1_value);
286     err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
287 
288     return err;
289 }
290 
qmc6310_set_output_data_rate(unsigned char rate)291 int qmc6310_set_output_data_rate(unsigned char rate)
292 {
293 
294     int err = 0;
295     unsigned char ctrl1_value = 0;
296 
297     err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
298     ctrl1_value = (ctrl1_value & (~0xE8)) | (rate << 5);
299     LOGD("SENSOR", "QMC6310_set_output_data_rate, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE, ctrl1_value);
300     err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
301 
302     return err;
303 }
304 
qmc6308_enable(void)305 static int qmc6308_enable(void)
306 {
307     int ret = 0;
308     if (chipid == 0x80) {
309         qmc6310_write_reg(0x0a, 0xc3);
310         qmc6310_write_reg(0x0b, 0x00);
311         qmc6310_write_reg(0x0d, 0x40);
312     } else {
313         qmc6310_write_reg(0x0a, 0x63);
314         qmc6310_write_reg(0x0b, 0x08);
315         qmc6310_write_reg(0x0d, 0x40);
316 
317     }
318     return ret;
319 }
320 
qmc6310_enable(void)321 static int qmc6310_enable(void)
322 {
323     int ret = 0;
324 
325     qmc6310_write_reg(0x0d, 0x40);
326     qmc6310_write_reg(0x29, 0x06);
327     qmc6310_write_reg(0x0a, 0x0F); // 0XA9=ODR =100HZ 0XA5 = 50HZ
328     qmc6310_write_reg(0x0b, 0x00); // 30 GS
329 
330     return 1;
331 }
332 
qmc6310_init(void)333 int qmc6310_init(void)
334 {
335     mag_addr = QMC6310U_IIC_ADDR;
336 
337     int32_t ret = sensor_i2c_open(1, mag_addr, I2C_BUS_BIT_RATES_100K, 0);
338     if (ret) {
339         LOGE("SENSOR", "sensor i2c open failed, ret:%d\n", ret);
340         return;
341     }
342 
343     ret = qmc6310_get_chipid();
344     if (ret == 0) {
345         return 0;
346     }
347 
348     qmc6310_set_layout(0);
349     qmc6310_enable();
350 
351     {
352         unsigned char ctrl_value;
353         qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl_value, 1);
354         LOGD("SENSOR", "QMC6310  0x%x=0x%x \r\n", QMC6310_CTL_REG_ONE, ctrl_value);
355         qmc6310_read_block(QMC6310_CTL_REG_TWO, &ctrl_value, 1);
356         LOGD("SENSOR", "QMC6310  0x%x=0x%x \r\n", QMC6310_CTL_REG_TWO, ctrl_value);
357         qmc6310_read_block(0x0d, &ctrl_value, 1);
358         LOGD("SENSOR", "QMC6310  0x%x=0x%x \r\n", 0x0d, ctrl_value);
359     }
360     return 1;
361 }
362 
qmc6310_deinit(void)363 void qmc6310_deinit(void)
364 {
365     int32_t ret = sensor_i2c_close(QMC6310_I2C_PORT);
366     if (ret) {
367         LOGE("SENSOR", "sensor i2c close failed, ret:%d\n", ret);
368     }
369     return;
370 }
371