Skip to content

Instantly share code, notes, and snippets.

@aprovecharLab

aprovecharLab/9DOF_NXP_fxos8700_fxas21002c.ipynb

Created March 3, 2019 17:23
Show Gist options
  • Save aprovecharLab/d971fba4ba0d440464004166f4b30d2e to your computer and use it in GitHub Desktop.
Save aprovecharLab/d971fba4ba0d440464004166f4b30d2e to your computer and use it in GitHub Desktop.
Download ZIP
Reading the NXP 9-DOF (fxos8700+fxas21002c) Sensors
Display the source blob
Display the rendered blob
Raw
9DOF_NXP_fxos8700_fxas21002c.ipynb
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Reading the NXP 9-DOF (fxos8700+fxas21002c) Sensors\n",
"### with FTDI USB/i2c\n",
"- Adafruit Precision NXP 9-DOF Breakout Board - FXOS8700 + FXAS21002C: https://www.adafruit.com/product/3463\n",
"- AdaFruit FT232H USB to i2c interface: https://www.adafruit.com/product/2264\n",
"- PyFtdi library: http://eblot.github.io/pyftdi/index.html"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from pyftdi.i2c import I2cController\n",
"from os import environ\n",
"\n",
"import math\n",
"import time\n",
"\n",
"import struct"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"###############################################################################\n",
"def recast14to16(byte0,byte1):\n",
" # pack 14 bits (1.5 bytes) into 16 bits byte-type\n",
" b14 = struct.pack('BB',byte0,byte1)\n",
"\n",
" # unpack to unsigned 16 bit integer\n",
" # note this device pads byte1 on the right with two empty bits\n",
" uL = struct.unpack('>H',b14)[0] >> 2\n",
" \n",
" # this is for 2's complement signed numbers - \n",
" # if negative assign sign bits for 16 bit case\n",
" if (uL & 0x2000):\n",
" uL = uL | 0xe000\n",
" \n",
" # repack as 16 bit unsigned byte-type\n",
" packed = struct.pack('>H', uL)\n",
" # unpack as 16 bit signed integer\n",
" unpacked = struct.unpack('>h', packed)[0]\n",
"\n",
" return unpacked"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"def twos_comp(val, bits):\n",
" # Convert an unsigned integer in 2's compliment form of the specified bit\n",
" # length to its signed integer value\n",
" if val & (1 << (bits - 1)) != 0:\n",
" return val - (1 << bits)\n",
" return val"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"class FTDIi2c:\n",
" def __init__(self):\n",
" self.i2c = I2cController()\n",
" url = environ.get('FTDI_DEVICE', 'ftdi://ftdi:232h/1')\n",
" self.i2c.configure(url, clockstretching=False)\n",
"\n",
" def connect(self,address):\n",
" port = self.i2c.get_port(address)\n",
" return port\n",
"\n",
" def close(self):\n",
" # Close the I2C connection\n",
" self.i2c.terminate()"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"###############################################################################\n",
"class i2cMagAccel:\n",
"\n",
" fxos8700_ADDRESS = 0x1F\n",
" fxos8700_ID = 0xC7\n",
" fxos8700_STATUS = 0x00\n",
" fxos8700_OUT_X_MSB = 0x01\n",
" fxos8700_OUT_X_LSB = 0x02\n",
" fxos8700_OUT_Y_MSB = 0x03\n",
" fxos8700_OUT_Y_LSB = 0x04\n",
" fxos8700_OUT_Z_MSB = 0x05\n",
" fxos8700_OUT_Z_LSB = 0x06\n",
" fxos8700_WHO_AM_I = 0x0D\n",
" fxos8700_XYZ_DATA_CFG = 0x0E\n",
" fxos8700_CTRL_REG1 = 0x2A\n",
" fxos8700_CTRL_REG2 = 0x2B\n",
" fxos8700_CTRL_REG3 = 0x2C\n",
" fxos8700_CTRL_REG4 = 0x2D\n",
" fxos8700_CTRL_REG5 = 0x2E\n",
" fxos8700_MSTATUS = 0x32\n",
" fxos8700_MOUT_X_MSB = 0x33\n",
" fxos8700_MOUT_X_LSB = 0x34\n",
" fxos8700_MOUT_Y_MSB = 0x35\n",
" fxos8700_MOUT_Y_LSB = 0x36\n",
" fxos8700_MOUT_Z_MSB = 0x37\n",
" fxos8700_MOUT_Z_LSB = 0x38\n",
" fxos8700_MCTRL_REG1 = 0x5B\n",
" fxos8700_MCTRL_REG2 = 0x5C\n",
" fxos8700_MCTRL_REG3 = 0x5D\n",
"\n",
" ACCEL_MG_LSB_2G = 0.000244\n",
" ACCEL_MG_LSB_4G = 0.000488\n",
" ACCEL_MG_LSB_8G = 0.000976\n",
" MAG_UT_LSB = 0.1 # uT/lsb\n",
"\n",
" GRAVITY_STANDARD = 9.80665\n",
" ACCEL_RANGE_2G = 0x00\n",
" ACCEL_RANGE_4G = 0x01\n",
" ACCEL_RANGE_8G = 0x02\n",
"\n",
" # standby mode for configuration\n",
" # write 0000 0000 = 0x00 to accelerometer control register 1 to place FXOS8700CQ into standby\n",
" # [7-1] = 0000 000\n",
" # [0]: active=0\n",
" standby_mode = 0x00\n",
"\n",
" # configure magnetometer\n",
" # write 0001 1111 = 0x1F to magnetometer control register 1\n",
" # [7]: m_acal=0: auto calibration disabled\n",
" # [6]: m_rst=0: no one-shot magnetic reset\n",
" # [5]: m_ost=0: no one-shot magnetic measurement\n",
" # [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise\n",
" # [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active \n",
" mag_settings_1 = 0x1F\n",
" \n",
" # write 0010 0000 = 0x20 to magnetometer control register 2\n",
" # [7]: reserved\n",
" # [6]: reserved\n",
" # [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow the accelerometer registers\n",
" # [4]: m_maxmin_dis=0 to retain default min/max latching even though not used\n",
" # [3]: m_maxmin_dis_ths=0\n",
" # [2]: m_maxmin_rst=0\n",
" # [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle\n",
" mag_settings_2 = 0x20\n",
" \n",
" # configure accelerometer\n",
" # write 0000 1101 = 0x0D to accelerometer control register 1\n",
" # [7-6]: aslp_rate=00\n",
" # [5-3]: dr=001 for 200Hz data rate (when in hybrid mode)\n",
" # [2]: lnoise=1 for low noise mode\n",
" # [1]: f_read=0 for normal 16 bit reads\n",
" # [0]: active=1 to take the part out of standby and enable sampling\n",
" accel_settings_1 = 0x0D\n",
"\n",
" # use accelerometer high resolution mode instead of low power mode\n",
" accel_settings_2 = 0x02\n",
"\n",
" # accelerometer range\n",
" # write 0000 0001= 0x01 to XYZ_DATA_CFG register\n",
" # [7]: reserved\n",
" # [6]: reserved\n",
" # [5]: reserved\n",
" # [4]: hpf_out=0\n",
" # [3]: reserved\n",
" # [2]: reserved\n",
" # [1-0]: fs=01 for accelerometer range of +/-4g range with 0.488mg/LSB\n",
" accel_range = ACCEL_RANGE_4G\n",
"\n",
"# accel_gain = ACCEL_MG_LSB_4G * GRAVITY_STANDARD\n",
" accel_gain = ACCEL_MG_LSB_4G # g\n",
" mag_gain = MAG_UT_LSB * 1000.0 # nT/lsb\n",
" \n",
" \n",
" ###########################################################################\n",
" def __init__(self,port):\n",
" self.port = port\n",
" \n",
" def configure(self):\n",
" # Configure the accelerometer\n",
" self.port.write_to(self.fxos8700_CTRL_REG1, self.standby_mode) # standby on\n",
" self.port.write_to(self.fxos8700_XYZ_DATA_CFG, [self.accel_range])\n",
" self.port.write_to(self.fxos8700_CTRL_REG2, [self.accel_settings_2])\n",
" self.port.write_to(self.fxos8700_CTRL_REG1, [self.accel_settings_1]) # standby off\n",
"\n",
" # Configure the magnetometer\n",
" self.port.write_to(self.fxos8700_MCTRL_REG1, [self.mag_settings_1])\n",
" self.port.write_to(self.fxos8700_MCTRL_REG2, [self.mag_settings_2])\n",
" \n",
" deviceConfig = [self.accel_range, self.accel_settings_1, self.accel_settings_2, \\\n",
" self.mag_settings_1, self.mag_settings_2]\n",
"\n",
" return deviceConfig\n",
"\n",
" def device_ID(self):\n",
" device_id = self.port.exchange([self.fxos8700_WHO_AM_I], 1)[0]\n",
" return int(device_id)\n",
"\n",
"\n",
" ###########################################################################\n",
" # Read accelerometer and magnetometer data\n",
" def read(self):\n",
" raw = self.port.read_from(self.fxos8700_OUT_X_MSB, 13)\n",
"# print(' '.join('{:02x}'.format(x) for x in raw))\n",
"\n",
" ## accelerometer\n",
" # data are 14 bit unsigned integers\n",
" accel_raw_x = struct.unpack_from('>H', raw[0:2])[0]\n",
" accel_raw_y = struct.unpack_from('>H', raw[2:4])[0]\n",
" accel_raw_z = struct.unpack_from('>H', raw[4:6])[0]\n",
" \n",
" # convert 14 bit two's complement to 16 bit signed integers\n",
" # note: this device pads LSB with two empty bits on right, \n",
" # so need to shift everything right 2 places\n",
" accel_raw_x = twos_comp(accel_raw_x >> 2, 14)\n",
" accel_raw_y = twos_comp(accel_raw_y >> 2, 14)\n",
" accel_raw_z = twos_comp(accel_raw_z >> 2, 14)\n",
" \n",
"# # alternative to above for 14 bit to 16 bit conversion\n",
"# accel_raw_x = recast14to16(raw[0],raw[1])\n",
"# accel_raw_y = recast14to16(raw[2],raw[3])\n",
"# accel_raw_z = recast14to16(raw[4],raw[5])\n",
"\n",
" aX = float(accel_raw_x) * self.accel_gain\n",
" aY = float(accel_raw_y) * self.accel_gain\n",
" aZ = float(accel_raw_z) * self.accel_gain\n",
"\n",
" ## magnetometer\n",
" # already 16 bit signed integers\n",
" mag_raw_x = struct.unpack_from('>h', raw[6:8])[0]\n",
" mag_raw_y = struct.unpack_from('>h', raw[8:10])[0]\n",
" mag_raw_z = struct.unpack_from('>h', raw[10:12])[0]\n",
" \n",
" mX = float(mag_raw_x) * self.mag_gain\n",
" mY = float(mag_raw_y) * self.mag_gain\n",
" mZ = float(mag_raw_z) * self.mag_gain\n",
"\n",
" mag = [mX, mY, mZ]\n",
" accel = [aX, aY, aZ]\n",
"\n",
" return mag, accel"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"###############################################################################\n",
"class i2cGyro:\n",
" \n",
" fxas21002c_ADDRESS = 0x21\n",
" fxas21002c_ID = 0xD7\n",
" fxas21002c_STATUS = 0x00\n",
" fxas21002c_OUT_X_MSB = 0x01\n",
" fxas21002c_OUT_X_LSB = 0x02\n",
" fxas21002c_OUT_Y_MSB = 0x03\n",
" fxas21002c_OUT_Y_LSB = 0x04\n",
" fxas21002c_OUT_Z_MSB = 0x05\n",
" fxas21002c_OUT_Z_LSB = 0x06\n",
" fxas21002c_WHO_AM_I = 0x0C\n",
" fxas21002c_CTRL_REG0 = 0x0D\n",
" fxas21002c_CTRL_REG1 = 0x13\n",
" fxas21002c_CTRL_REG2 = 0x14\n",
" \n",
" fxas21002c_SENSITIVITY_250DPS = 0.0078125 # radians/second/lsb\n",
" fxas21002c_SENSITIVITY_500DPS = 0.015625\n",
" fxas21002c_SENSITIVITY_1000DPS = 0.03125\n",
" fxas21002c_SENSITIVITY_2000DPS = 0.0625\n",
"\n",
" GYRO_RANGE_250DPS = 250\n",
" GYRO_RANGE_500DPS = 500\n",
" GYRO_RANGE_1000DPS = 1000\n",
" GYRO_RANGE_2000DPS = 2000\n",
" \n",
" reset_mode = 0x40\n",
" standby_mode = 0x00\n",
" active_mode = 0x0e\n",
"\n",
" # user settings\n",
" gyro_range = GYRO_RANGE_500DPS \n",
" gyro_gain = fxas21002c_SENSITIVITY_500DPS # radians/second/lsb\n",
"\n",
" if gyro_range == GYRO_RANGE_250DPS:\n",
" sensitivity_mode = 0x03\n",
" elif gyro_range == GYRO_RANGE_500DPS:\n",
" sensitivity_mode = 0x02\n",
" elif gyro_range == GYRO_RANGE_1000DPS:\n",
" sensitivity_mode = 0x01\n",
" elif gyro_range == GYRO_RANGE_2000DPS:\n",
" sensitivity_mode = 0x00\n",
"\n",
" \n",
" ###########################################################################\n",
" def __init__(self,port):\n",
" self.port = port\n",
" \n",
" def configure(self):\n",
" # Reset then switch to active mode with 100Hz output\n",
" self.port.write_to(self.fxas21002c_CTRL_REG1, [self.standby_mode]) # Standby\n",
" self.port.exchange([self.fxas21002c_CTRL_REG1], self.reset_mode) # Reset\n",
" self.port.write_to(self.fxas21002c_CTRL_REG0, [self.sensitivity_mode]) # Set sensitivity\n",
" self.port.write_to(self.fxas21002c_CTRL_REG1, [self.active_mode]) # Active\n",
" time.sleep(0.1) # 60 ms + 1/ODR\n",
"\n",
" deviceConfig = [self.sensitivity_mode]\n",
" return deviceConfig\n",
"\n",
" def device_ID(self):\n",
" device_id = self.port.exchange([self.fxas21002c_WHO_AM_I], 1)[0]\n",
" return int(device_id)\n",
"\n",
" ###########################################################################\n",
" # Read gyroscope\n",
" def read(self):\n",
" raw = self.port.read_from(self.fxas21002c_OUT_X_MSB, 7)\n",
"# print(' '.join('{:02x}'.format(x) for x in raw))\n",
" \n",
" ## gyroscope\n",
" # already 16 bit signed integers\n",
" gyro_raw_x = struct.unpack_from('>h', raw[0:2])[0]\n",
" gyro_raw_y = struct.unpack_from('>h', raw[2:4])[0]\n",
" gyro_raw_z = struct.unpack_from('>h', raw[4:6])[0]\n",
" \n",
" gX = float(gyro_raw_x) * self.gyro_gain\n",
" gY = float(gyro_raw_y) * self.gyro_gain\n",
" gZ = float(gyro_raw_z) * self.gyro_gain\n",
"\n",
" gyro = [gX, gY, gZ]\n",
"\n",
" return gyro"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"scrolled": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Device Config: 01 0d 02 1f 20\n",
"Device ID: 0xc7\n",
" \n",
"Device Config: 02\n",
"Device ID: 0xd7\n",
" \n",
"magXYZ: \t [33800.0, 44800.0, 22000.0] \t |M|: 60278.35432391963\n",
"accelXYZ: \t [0.019032, -0.033672, 1.042368]\n",
"gyroXYZ: \t [1.234375, 0.0, -0.0625]\n",
"\n",
"magXYZ: \t [34500.0, 45300.0, 20100.0] \t |M|: 60385.014697356826\n",
"accelXYZ: \t [0.017079999999999998, -0.034648, 1.043344]\n",
"gyroXYZ: \t [0.96875, -0.40625, 0.1875]\n",
"\n",
"magXYZ: \t [33400.0, 45100.0, 20200.0] \t |M|: 59645.703952589916\n",
"accelXYZ: \t [0.019032, -0.034648, 1.0389519999999999]\n",
"gyroXYZ: \t [0.703125, -0.171875, 0.234375]\n",
"\n",
"magXYZ: \t [33300.0, 43600.0, 20900.0] \t |M|: 58708.26176953292\n",
"accelXYZ: \t [0.020007999999999998, -0.034648, 1.04188]\n",
"gyroXYZ: \t [0.671875, -0.03125, 0.296875]\n",
"\n",
"magXYZ: \t [34300.0, 45600.0, 20700.0] \t |M|: 60698.76440258072\n",
"accelXYZ: \t [0.01952, -0.033672, 1.039928]\n",
"gyroXYZ: \t [0.859375, 0.078125, 0.140625]\n",
"\n",
"magXYZ: \t [33800.0, 43300.0, 20500.0] \t |M|: 58630.87923611584\n",
"accelXYZ: \t [0.021471999999999998, -0.033672, 1.045296]\n",
"gyroXYZ: \t [1.28125, 0.0625, 0.171875]\n",
"\n",
"magXYZ: \t [32700.0, 44000.0, 19400.0] \t |M|: 58151.95611499238\n",
"accelXYZ: \t [0.017568, -0.033672, 1.040904]\n",
"gyroXYZ: \t [0.9375, -0.109375, 0.390625]\n",
"\n",
"magXYZ: \t [33800.0, 45000.0, 21500.0] \t |M|: 60246.90863438555\n",
"accelXYZ: \t [0.015616, -0.032695999999999996, 1.04432]\n",
"gyroXYZ: \t [0.5625, -0.125, 0.234375]\n",
"\n",
"magXYZ: \t [34400.0, 44600.0, 19600.0] \t |M|: 59637.90740795656\n",
"accelXYZ: \t [0.020007999999999998, -0.036112, 1.03944]\n",
"gyroXYZ: \t [0.9375, 0.109375, 0.296875]\n",
"\n",
"magXYZ: \t [32900.0, 42100.0, 20100.0] \t |M|: 57086.162946899836\n",
"accelXYZ: \t [0.018056, -0.036112, 1.039928]\n",
"gyroXYZ: \t [0.90625, 0.0, -0.140625]\n",
"\n"
]
}
],
"source": [
"i2c = FTDIi2c()\n",
"\n",
"port_nxpMA = i2c.connect(0x1F)\n",
"MagAccel = i2cMagAccel(port_nxpMA)\n",
"deviceConfig = MagAccel.configure()\n",
"print ('Device Config: ',' '.join('{:02x}'.format(x) for x in deviceConfig))\n",
"id = MagAccel.device_ID()\n",
"print ('Device ID: ',hex(id))\n",
"print(' ')\n",
"\n",
"port_nxpGY = i2c.connect(0x21)\n",
"Gyro = i2cGyro(port_nxpGY)\n",
"deviceConfig = Gyro.configure()\n",
"print ('Device Config: ',' '.join('{:02x}'.format(x) for x in deviceConfig))\n",
"id = Gyro.device_ID()\n",
"print ('Device ID: ',hex(id))\n",
"print(' ')\n",
"\n",
"for i in range(10):\n",
" magXYZ, accelXYZ = MagAccel.read()\n",
" gyroXYZ = Gyro.read()\n",
"\n",
" M = math.sqrt(magXYZ[0]**2 + magXYZ[1]**2 + magXYZ[2]**2)\n",
"\n",
" print ('magXYZ: \\t', magXYZ, '\\t |M|: ', M)\n",
" print ('accelXYZ: \\t',accelXYZ)\n",
" print ('gyroXYZ: \\t', gyroXYZ)\n",
" print ('')\n",
"\n",
" time.sleep(0.1)\n",
" \n",
"i2c.close()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.7.1"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment