1# -*- coding: UTF-8 -*-
2
3"""
4The driver for qmc5883 chip
5"""
6
7from driver import I2C
8from utime import sleep_ms
9import math
10
11QMC5883L_ADDR  = 0x0D;
12x_max = 0;
13x_min = 0;
14z_min = 0;
15y_max = 0;
16y_min = 0;
17z_max = 0;
18addr = 0;
19mode = 0;
20rate = 0;
21g_range = 0;
22oversampling = 0;
23
24INT16_MIN = (-32767-1)
25INT16_MAX = 32767
26
27# Register numbers
28QMC5883L_X_LSB    = 0
29QMC5883L_X_MSB    = 1
30QMC5883L_Y_LSB    = 2
31QMC5883L_Y_MSB    = 3
32QMC5883L_Z_LSB    = 4
33QMC5883L_Z_MSB    = 5
34QMC5883L_STATUS   = 6
35QMC5883L_TEMP_LSB = 7
36QMC5883L_TEMP_MSB = 8
37QMC5883L_CONFIG   = 9
38QMC5883L_CONFIG2  = 10
39QMC5883L_RESET    = 11
40QMC5883L_RESERVED = 12
41QMC5883L_CHIP_ID  = 13
42
43QMC5883L_STATUS_DRDY = 1
44QMC5883L_STATUS_OVL  = 2
45QMC5883L_STATUS_DOR  = 4
46
47# Oversampling values for the CONFIG register
48QMC5883L_CONFIG_OS512 = 0b00000000
49QMC5883L_CONFIG_OS256 = 0b01000000
50QMC5883L_CONFIG_OS128 = 0b10000000
51QMC5883L_CONFIG_OS64  = 0b11000000
52
53# Range values for the CONFIG register
54QMC5883L_CONFIG_2GAUSS = 0b00000000
55QMC5883L_CONFIG_8GAUSS = 0b00010000
56
57# Rate values for the CONFIG register
58QMC5883L_CONFIG_10HZ  = 0b00000000
59QMC5883L_CONFIG_50HZ  = 0b00000100
60QMC5883L_CONFIG_100HZ = 0b00001000
61QMC5883L_CONFIG_200HZ = 0b00001100
62
63# Mode values for the CONFIG register
64QMC5883L_CONFIG_STANDBY = 0b00000000
65QMC5883L_CONFIG_CONT    = 0b00000001
66
67# Apparently M_PI isn't available in all environments.
68M_PI = 3.14159265358979323846264338327950288
69
70class qmc5883Error(Exception):
71    def __init__(self, value=0, msg="qmc5883 common error"):
72        self.value = value
73        self.msg = msg
74
75    def __str__(self):
76        return "Error code:%d, Error message: %s" % (self.value, str(self.msg))
77
78    __repr__ = __str__
79
80class QMC5883(object):
81    """
82    This class implements qmc5883 chip's defs.
83    """
84    def __init__(self):
85        self.i2cDev = None
86
87    def open(self, devid):
88        self.i2cDev = I2C()
89        self.i2cDev.open(devid)
90
91    def devRegRead1Byte(self, addr):
92        return self.devRegReadWrite1Byte(0, addr, 0);
93
94    def devRegReadWrite1Byte(self, mode, addr, value):
95        #0 read mode
96        #1 write mode
97        if (mode == 0):
98            Reg = bytearray([addr])
99            self.i2cDev.write(Reg);
100            sleep_ms(30)
101            tmp = bytearray(1)
102            self.i2cDev.read(tmp)
103            print("<-- read addr " + str(addr) + ", value = " + str(tmp[0]));
104            return tmp[0];
105        else:
106            Reg = bytearray([addr, value])
107            self.i2cDev.write(Reg);
108            print("--> write addr " + str(addr) + ", value = " + str(value));
109            return 0;
110
111    def devRegWrite1Byte(self, data):
112        Reg = bytearray([data])
113        self.i2cDev.write(Reg);
114        print("--> write value = " + str(Reg[0]));
115
116    def devRegReadNByte(self, addr, len):
117        reg = bytearray([addr]);
118        data = bytearray(len);
119        self.i2cDev.write(reg);
120        sleep_ms(20)
121        self.i2cDev.read(data);
122        print("--> read " + str(len) + " bytes from addr " + str(addr) + ", " + str(len) + " bytes value = " + str(data));
123        return data;
124
125    def qmc5883l_write_register(self, addr, reg,  data):
126        print(">>>> wirte reg: %d, data: %d\n" %(reg, data))
127        self.devRegReadWrite1Byte(1, reg, data);
128
129    def qmc5883l_read_register(self, addr, reg):
130        return self.devRegRead1Byte(reg);
131
132    def qmc5883l_read_len(self, reg, len):
133        return self.devRegReadNByte(reg, len);
134
135    def qmc5883l_reconfig(self):
136        self.qmc5883l_write_register(addr, QMC5883L_CONFIG, oversampling | g_range | rate | mode);
137        sleep_ms(50)
138        self.qmc5883l_write_register(addr, QMC5883L_CONFIG2, 0x1);
139
140    def qmc5883l_reset(self):
141        self.qmc5883l_write_register(addr, QMC5883L_RESET, 0x01);
142        sleep_ms(500)
143        self.qmc5883l_reconfig();
144        sleep_ms(50)
145        self.qmc5883l_resetCalibration();
146
147    def qmc5883l_setOversampling(self, x):
148        global oversampling
149        if (x == 512):
150            oversampling = QMC5883L_CONFIG_OS512;
151        elif (x == 256):
152            oversampling = QMC5883L_CONFIG_OS256;
153        elif (x == 128):
154            oversampling = QMC5883L_CONFIG_OS128;
155        elif (x == 64):
156            oversampling = QMC5883L_CONFIG_OS64;
157        self.qmc5883l_reconfig();
158
159    def qmc5883l_setRange(self, x):
160        global g_range
161        if (x == 2):
162            g_range = QMC5883L_CONFIG_2GAUSS;
163        elif (x == 8):
164            g_range = QMC5883L_CONFIG_8GAUSS;
165        self.qmc5883l_reconfig();
166
167    def qmc5883l_setSamplingRate(self, x):
168        global rate
169        if (x == 10):
170            rate = QMC5883L_CONFIG_10HZ;
171        elif (x == 50):
172            rate = QMC5883L_CONFIG_50HZ;
173        elif (x == 100):
174            rate = QMC5883L_CONFIG_100HZ;
175        elif (x == 200):
176            rate = QMC5883L_CONFIG_200HZ;
177        self.qmc5883l_reconfig();
178
179    def _qmc5883l_init(self):
180        global addr
181        global oversampling
182        global g_range
183        global rate
184        global mode
185        # This assumes the wire library has been initialized.
186        addr         = QMC5883L_ADDR;
187        oversampling = QMC5883L_CONFIG_OS512;
188        g_range        = QMC5883L_CONFIG_8GAUSS;
189        rate         = QMC5883L_CONFIG_200HZ;
190        mode         = QMC5883L_CONFIG_CONT;
191        print("addr %d,oversampling %d,g_range %d,rate %d, mode %d" %(addr, oversampling, g_range, rate, mode))
192
193        self.qmc5883l_reset();
194
195    def qmc5883l_ready(self):
196        sleep_ms(200);
197        tmp = self.qmc5883l_read_register(addr, QMC5883L_STATUS) & QMC5883L_STATUS_DRDY
198        return tmp;
199
200    def qmc5883l_readRaw(self):
201        timeout = 10000;
202        arr = [1, 2, 3];
203        data = bytearray(6);
204        ready = self.qmc5883l_ready();
205        while (ready == 0 and timeout):
206            ready = self.qmc5883l_ready();
207            timeout -= 1;
208            print("ready = %d" %(ready))
209
210        data = self.qmc5883l_read_len(QMC5883L_X_LSB, 6);
211
212        x = data[0] | (data[1] << 8);
213        y = data[2] | (data[3] << 8);
214        z = data[4] | (data[5] << 8);
215
216        print("read_raw[%f,%f,%f],\n" %(x ,y, z));
217
218        if (x > (1 << 15)):
219            x = x - (1<<16)
220
221        if (y > (1 << 15)):
222            y = y - (1<<16)
223
224        if (z > (1 << 15)):
225            z = z - (1<<16)
226
227        arr[0] = x;
228        arr[1] = y;
229        arr[2] = z;
230        return arr;
231
232    def qmc5883l_resetCalibration(self):
233        global x_max
234        global x_min
235        global z_min
236        global y_max
237        global y_min
238        global z_max
239        x_max = y_max = z_max = INT16_MIN;
240        x_min = y_min = z_min = INT16_MAX;
241
242    def qmc5883l_readHeading(self):
243        global x_max
244        global x_min
245        global z_min
246        global y_max
247        global y_min
248        global z_max
249        global addr
250        global mode
251        global rate
252        global g_range
253        global oversampling
254
255        tmp = self.qmc5883l_read_register(addr, QMC5883L_STATUS) & QMC5883L_STATUS_DRDY;
256        print("read QMC5883L_STATUS: %d\n" %(tmp))
257
258        xyz_org = self.qmc5883l_readRaw();
259        x_org = xyz_org[0];
260        y_org = xyz_org[1];
261        z_org = xyz_org[2];
262        print("org[%f,%f,%f]\n" %(x_org ,y_org, z_org));
263
264        # Update the observed boundaries of the measurements
265        if (x_org < x_min):
266            x_min = x_org;
267
268        if (x_org > x_max):
269            x_max = x_org;
270
271        if (y_org < y_min):
272            y_min = y_org;
273
274        if (y_org > y_max):
275            y_max = y_org;
276
277        if (z_org < z_min):
278            z_min = z_org;
279
280        if (z_org > z_max):
281            z_max = z_org;
282
283        # Bail out if not enough data is available.
284        if ((x_min == x_max) or (y_min == y_max) or (z_max == z_min)):
285            print("x_min %f == x_max %f or y_min %f == y_max %f or z_max%f == z_min%f\n" %(x_min, x_max, y_min, y_max, z_max, z_min))
286            return 0;
287
288        # Recenter the measurement by subtracting the average
289        x_offset = (x_max + x_min) / 2.0;
290        y_offset = (y_max + y_min) / 2.0;
291        z_offset = (z_max + z_min) / 2.0;
292
293        x_fit = (x_org - x_offset) * 1000.0 / (x_max - x_min);
294        y_fit = (y_org - y_offset) * 1000.0 / (y_max - y_min);
295        z_fit = (z_org - z_offset) * 1000.0 / (z_max - z_min);
296
297        print("fix[%f,%f,%f],\n" %(x_fit ,y_fit, z_fit));
298
299        heading = 180.0 * math.atan2(x_fit, y_fit) / M_PI;
300        if (heading <= 0):
301            heading = heading + 360;
302        print("heading = %f\n", heading);
303        return heading;
304
305    def init(self):
306        self._qmc5883l_init();
307
308    def close(self):
309        self.i2cDev.close()
310