1
2 /*
3 * Copyright (C) 2015-2020 Alibaba Group Holding Limited
4 */
5
6 #include "drv_mag_qst_qmc5883l.h"
7 #include "aos/hal/i2c.h"
8 #include "ulog/ulog.h"
9
10 #define QMC5883L_I2C_PORT 1
11
12 static int16_t x_max, x_min, z_min, y_max, y_min, z_max;
13 static uint8_t addr;
14 static uint8_t mode;
15 static uint8_t rate;
16 static uint8_t range;
17 static uint8_t oversampling;
18
qmc5883l_i2c_master_send(const uint8_t * data,uint16_t size,uint32_t timeout)19 int qmc5883l_i2c_master_send(const uint8_t *data, uint16_t size, uint32_t timeout)
20 {
21 return sensor_i2c_master_send(QMC5883L_I2C_PORT, QMC5883L_ADDR, data, size, timeout);
22 }
23
qmc5883l_i2c_master_recv(uint8_t * data,uint16_t size,uint32_t timeout)24 int32_t qmc5883l_i2c_master_recv(uint8_t *data, uint16_t size, uint32_t timeout)
25 {
26 return sensor_i2c_master_recv(QMC5883L_I2C_PORT, QMC5883L_ADDR, data, size, timeout);
27 }
28
qmc5883l_write_register(uint8_t addr,uint8_t reg,uint8_t data)29 static qmc5883l_write_register(uint8_t addr, uint8_t reg, uint8_t data)
30 {
31 uint8_t write_buffer[2] = {reg, data};
32 qmc5883l_i2c_master_send(write_buffer, 2, 1000);
33 }
34
qmc5883l_read_register(uint8_t addr,uint8_t reg)35 uint8_t qmc5883l_read_register(uint8_t addr, uint8_t reg)
36 {
37 uint8_t data;
38 qmc5883l_i2c_master_send(®, 1, 1000);
39 aos_msleep(2);
40 qmc5883l_i2c_master_recv(&data, 1, 1000);
41 return data;
42 }
43
qmc5883l_read_len(uint8_t addr,uint8_t reg,uint8_t * buf,uint8_t len)44 uint8_t qmc5883l_read_len(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len)
45 {
46 qmc5883l_i2c_master_send(®, 1, 1000);
47 aos_msleep(20);
48 qmc5883l_i2c_master_recv(buf, len, 1000);
49 return 1;
50 }
51
qmc5883l_reconfig()52 static void qmc5883l_reconfig()
53 {
54 qmc5883l_write_register(addr, QMC5883L_CONFIG,
55 oversampling | range | rate | mode);
56 qmc5883l_write_register(addr, QMC5883L_CONFIG2, 0x1);
57 }
58
qmc5883l_reset()59 static void qmc5883l_reset()
60 {
61 qmc5883l_write_register(addr, QMC5883L_RESET, 0x01);
62 qmc5883l_reconfig();
63 qmc5883l_resetCalibration();
64 }
65
qmc5883l_setOversampling(int x)66 void qmc5883l_setOversampling(int x)
67 {
68 switch (x) {
69 case 512:
70 oversampling = QMC5883L_CONFIG_OS512;
71 break;
72 case 256:
73 oversampling = QMC5883L_CONFIG_OS256;
74 break;
75 case 128:
76 oversampling = QMC5883L_CONFIG_OS128;
77 break;
78 case 64:
79 oversampling = QMC5883L_CONFIG_OS64;
80 break;
81 default:
82 break;
83 }
84 qmc5883l_reconfig();
85 }
86
qmc5883l_setRange(int x)87 static void qmc5883l_setRange(int x)
88 {
89 switch (x) {
90 case 2:
91 range = QMC5883L_CONFIG_2GAUSS;
92 break;
93 case 8:
94 range = QMC5883L_CONFIG_8GAUSS;
95 break;
96 default:
97 break;
98 }
99
100 qmc5883l_reconfig();
101 }
102
qmc5883l_setSamplingRate(int x)103 void qmc5883l_setSamplingRate(int x)
104 {
105 switch (x) {
106 case 10:
107 rate = QMC5883L_CONFIG_10HZ;
108 break;
109 case 50:
110 rate = QMC5883L_CONFIG_50HZ;
111 break;
112 case 100:
113 rate = QMC5883L_CONFIG_100HZ;
114 break;
115 case 200:
116 rate = QMC5883L_CONFIG_200HZ;
117 break;
118 default:
119 break;
120 }
121 qmc5883l_reconfig();
122 }
123
_qmc5883l_init()124 static void _qmc5883l_init()
125 {
126 /* This assumes the wire library has been initialized. */
127 addr = QMC5883L_ADDR;
128 oversampling = QMC5883L_CONFIG_OS512;
129 range = QMC5883L_CONFIG_8GAUSS;
130 rate = QMC5883L_CONFIG_200HZ;
131 mode = QMC5883L_CONFIG_CONT;
132
133 qmc5883l_reset();
134 }
135
qmc5883l_ready()136 int qmc5883l_ready()
137 {
138 return qmc5883l_read_register(addr, QMC5883L_STATUS) & QMC5883L_STATUS_DRDY;
139 }
140
qmc5883l_readRaw(int16_t * x,int16_t * y,int16_t * z)141 int qmc5883l_readRaw(int16_t *x, int16_t *y, int16_t *z)
142 {
143 uint8_t data[6];
144 uint32_t timeout = 10000;
145
146 while (!qmc5883l_ready() && (timeout--))
147 ;
148
149 if (!qmc5883l_read_len(addr, QMC5883L_X_LSB, data, 6))
150 return 0;
151 *x = data[0] | (data[1] << 8);
152 *y = data[2] | (data[3] << 8);
153 *z = data[4] | (data[5] << 8);
154
155 return 1;
156 }
157
qmc5883l_resetCalibration()158 void qmc5883l_resetCalibration()
159 {
160 x_max = y_max = z_max = INT16_MIN;
161 x_min = y_min = z_min = INT16_MAX;
162 }
163
qmc5883l_readHeading()164 int qmc5883l_readHeading()
165 {
166 int16_t x_org, y_org, z_org;
167 float x_offset, y_offset, z_offset;
168 float x_fit, y_fit, z_fit;
169
170 if (!qmc5883l_readRaw(&x_org, &y_org, &z_org))
171 return 0;
172
173 /* Update the observed boundaries of the measurements */
174 x_min = x_org < x_min ? x_org : x_min;
175 x_max = x_org > x_max ? x_org : x_max;
176 y_min = y_org < y_min ? y_org : y_min;
177 y_max = y_org > y_max ? y_org : y_max;
178 z_min = z_org < z_min ? z_org : z_min;
179 z_max = z_org > z_max ? z_org : z_max;
180
181 /* Bail out if not enough data is available. */
182 if (x_min == x_max || y_min == y_max || z_max == z_min)
183 return 0;
184
185 /* Recenter the measurement by subtracting the average */
186 x_offset = (x_max + x_min) / 2.0;
187 y_offset = (y_max + y_min) / 2.0;
188 z_offset = (z_max + z_min) / 2.0;
189
190 x_fit = (x_org - x_offset) * 1000.0 / (x_max - x_min);
191 y_fit = (y_org - y_offset) * 1000.0 / (y_max - y_min);
192 z_fit = (z_org - z_offset) * 1000.0 / (z_max - z_min);
193
194 LOGD("SENSOR", "fix[%f,%f,%f],\n", x_fit, y_fit, z_fit);
195
196 int heading = 180.0 * atan2(x_fit, y_fit) / M_PI;
197 heading = (heading <= 0) ? (heading + 360) : heading;
198
199 return heading;
200 }
201
qmc5883l_init(void)202 void qmc5883l_init(void)
203 {
204 int32_t ret = sensor_i2c_open(QMC5883L_I2C_PORT, QMC5883L_ADDR, I2C_BUS_BIT_RATES_100K, 0);
205 if (ret) {
206 LOGE("SENSOR", "sensor i2c open failed, ret:%d\n", ret);
207 return;
208 }
209
210 _qmc5883l_init();
211 }
212
qmc5883l_deinit(void)213 void qmc5883l_deinit(void)
214 {
215 int32_t ret = sensor_i2c_close(QMC5883L_I2C_PORT);
216 if (ret) {
217 LOGE("SENSOR", "sensor i2c close failed, ret:%d\n", ret);
218 }
219 return;
220 }
221