forked from adafruit/Adafruit_CircuitPython_FXOS8700
-
Notifications
You must be signed in to change notification settings - Fork 0
/
fxos8700.py
191 lines (171 loc) · 7.96 KB
/
fxos8700.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
# The MIT License (MIT)
#
# Copyright for portions of this work are held by:
# Tony DiCola for Adafruit Industries - Copyright (c) 2017
# Copyright for portions of this work are held by:
# Nick Viera for Sicada Co. - Copyright (c) 2018
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
"""
==============================================================================
Python module for the NXP FXOS8700 Gyroscope.
Intended for use with Beaglebone or similar platforms
which contain a standard CPython implementation and I2C
bus access through standard Linux OS facilities.
Based on the original C/C++ driver from:
https://github.com/adafruit/Adafruit_FXOS8700
==============================================================================
"""
try:
import ustruct as struct
except ImportError:
import struct
import Adafruit_libs.I2C as I2C
# Register addresses and other constants:
# pylint: disable=bad-whitespace
_FXOS8700_ADDRESS = 0x1F # 0011111
_FXOS8700_ID = 0xC7 # 1100 0111
_FXOS8700_REGISTER_STATUS = 0x00
_FXOS8700_REGISTER_OUT_X_MSB = 0x01
_FXOS8700_REGISTER_OUT_X_LSB = 0x02
_FXOS8700_REGISTER_OUT_Y_MSB = 0x03
_FXOS8700_REGISTER_OUT_Y_LSB = 0x04
_FXOS8700_REGISTER_OUT_Z_MSB = 0x05
_FXOS8700_REGISTER_OUT_Z_LSB = 0x06
_FXOS8700_REGISTER_WHO_AM_I = 0x0D # 11000111 r
_FXOS8700_REGISTER_XYZ_DATA_CFG = 0x0E
_FXOS8700_REGISTER_CTRL_REG1 = 0x2A # 00000000 r/w
_FXOS8700_REGISTER_CTRL_REG2 = 0x2B # 00000000 r/w
_FXOS8700_REGISTER_CTRL_REG3 = 0x2C # 00000000 r/w
_FXOS8700_REGISTER_CTRL_REG4 = 0x2D # 00000000 r/w
_FXOS8700_REGISTER_CTRL_REG5 = 0x2E # 00000000 r/w
_FXOS8700_REGISTER_MSTATUS = 0x32
_FXOS8700_REGISTER_MOUT_X_MSB = 0x33
_FXOS8700_REGISTER_MOUT_X_LSB = 0x34
_FXOS8700_REGISTER_MOUT_Y_MSB = 0x35
_FXOS8700_REGISTER_MOUT_Y_LSB = 0x36
_FXOS8700_REGISTER_MOUT_Z_MSB = 0x37
_FXOS8700_REGISTER_MOUT_Z_LSB = 0x38
_FXOS8700_REGISTER_MCTRL_REG1 = 0x5B # 00000000 r/w
_FXOS8700_REGISTER_MCTRL_REG2 = 0x5C # 00000000 r/w
_FXOS8700_REGISTER_MCTRL_REG3 = 0x5D # 00000000 r/w
_ACCEL_MG_LSB_2G = 0.000244
_ACCEL_MG_LSB_4G = 0.000488
_ACCEL_MG_LSB_8G = 0.000976
_MAG_UT_LSB = 0.1
_SENSORS_GRAVITY_STANDARD = 9.80665
# pylint: enable=bad-whitespace
# User-facing constants/module-level globals:
ACCEL_RANGE_2G = 0x00
ACCEL_RANGE_4G = 0x01
ACCEL_RANGE_8G = 0x02
def _twos_comp(val, bits):
# Convert an unsigned integer in 2's compliment form of the specified bit
# length to its signed integer value and return it.
if val & (1 << (bits - 1)) != 0:
return val - (1 << bits)
return val
class FXOS8700:
"""Driver for the NXP FXOS8700 accelerometer and magnetometer."""
# Class-level buffer for reading and writing data with the sensor.
# This reduces memory allocations but means the code is not re-entrant or
# thread safe!
_BUFFER = bytearray(13)
def __init__(self, i2c_bus, address=_FXOS8700_ADDRESS,
accel_range=ACCEL_RANGE_2G):
assert accel_range in (ACCEL_RANGE_2G, ACCEL_RANGE_4G, ACCEL_RANGE_8G)
self._accel_range = accel_range
self._device = I2C.Device(address, i2c_bus)
# Check for chip ID value.
if self._read_u8(_FXOS8700_REGISTER_WHO_AM_I) != _FXOS8700_ID:
raise RuntimeError('Failed to find FXOS8700, check wiring!')
# Set to standby mode (required to make changes to this register)
self._write_u8(_FXOS8700_REGISTER_CTRL_REG1, 0)
if accel_range == ACCEL_RANGE_2G:
self._write_u8(_FXOS8700_REGISTER_XYZ_DATA_CFG, 0x00)
elif accel_range == ACCEL_RANGE_4G:
self._write_u8(_FXOS8700_REGISTER_XYZ_DATA_CFG, 0x01)
elif accel_range == ACCEL_RANGE_8G:
self._write_u8(_FXOS8700_REGISTER_XYZ_DATA_CFG, 0x02)
# High resolution
self._write_u8(_FXOS8700_REGISTER_CTRL_REG2, 0x02)
# Active, Normal Mode, Low Noise, 100Hz in Hybrid Mode
self._write_u8(_FXOS8700_REGISTER_CTRL_REG1, 0x15)
# Configure the magnetometer
# Hybrid Mode, Over Sampling Rate = 16
self._write_u8(_FXOS8700_REGISTER_MCTRL_REG1, 0x1F)
# Jump to reg 0x33 after reading 0x06
self._write_u8(_FXOS8700_REGISTER_MCTRL_REG2, 0x20)
def _read_u8(self, address):
# Read an 8-bit unsigned value from the specified 8-bit address.
address = address & 0xFF
return self._device.readU8(address)
def _write_u8(self, address, val):
# Write an 8-bit unsigned value to the specified 8-bit address.
address = address & 0xFF
val = val & 0xFF
return self._device.write8(address, val)
def read_raw_accel_mag(self):
"""Read the raw accelerometer and magnetometer readings. Returns a
2-tuple of 3-tuples:
- Accelerometer X, Y, Z axis 14-bit signed raw values
- Magnetometer X, Y, Z axis 16-bit signed raw values
If you want the acceleration or magnetometer values in friendly units
consider using the accelerometer and magnetometer properties!
"""
# Read accelerometer data from sensor.
self._BUFFER = self._device.readList(_FXOS8700_REGISTER_OUT_X_MSB, 6)
accel_raw_x = struct.unpack_from('>H', self._BUFFER[0:2])[0]
accel_raw_y = struct.unpack_from('>H', self._BUFFER[2:4])[0]
accel_raw_z = struct.unpack_from('>H', self._BUFFER[4:6])[0]
# Convert accelerometer data to signed 14-bit value from 16-bit
# left aligned 2's compliment value.
accel_raw_x = _twos_comp(accel_raw_x >> 2, 14)
accel_raw_y = _twos_comp(accel_raw_y >> 2, 14)
accel_raw_z = _twos_comp(accel_raw_z >> 2, 14)
# Read magnetometer data from sensor. No need to convert as this is
# 16-bit signed data so struct parsing can handle it directly.
self._BUFFER = self._device.readList(_FXOS8700_REGISTER_MOUT_X_MSB, 6)
mag_raw_x = struct.unpack_from('>h', self._BUFFER[0:2])[0]
mag_raw_y = struct.unpack_from('>h', self._BUFFER[2:4])[0]
mag_raw_z = struct.unpack_from('>h', self._BUFFER[4:6])[0]
return ((accel_raw_x, accel_raw_y, accel_raw_z),
(mag_raw_x, mag_raw_y, mag_raw_z))
@property
def accelerometer(self):
"""Read the acceleration from the accelerometer and return its X, Y, Z axis values as a
3-tuple in m/s^2.
"""
accel_raw, _ = self.read_raw_accel_mag()
# Convert accel values to m/s^2
factor = 0
if self._accel_range == ACCEL_RANGE_2G:
factor = _ACCEL_MG_LSB_2G
elif self._accel_range == ACCEL_RANGE_4G:
factor = _ACCEL_MG_LSB_4G
elif self._accel_range == ACCEL_RANGE_8G:
factor = _ACCEL_MG_LSB_8G
return [x * factor * _SENSORS_GRAVITY_STANDARD for x in accel_raw]
@property
def magnetometer(self):
"""Read the magnetometer values and return its X, Y, Z axis values as a 3-tuple in uTeslas.
"""
_, mag_raw = self.read_raw_accel_mag()
# Convert mag values to uTesla
return [x * _MAG_UT_LSB for x in mag_raw]