I need help porting MinIMU9V2 code to python

codac · May 28, 2014, 6:57am

Hi guys,

I’m trying to port the c++ code for the MinIMU9V2 into python.

Currently I’m working on the LSM303DLHC chip (Accelerometer and Magnetometer).

The aim is to get a working python version for the LSM303DLHC with automatic calibration, returning the values for magnetic X/Y/Z, acceleration X/Y/Z, the GAUSS values, the temperature and the heading.

I know the code is still a very alpha version, but this is what I’ve got so far.

Please test the code and provide helpful information in order to improve and correct the code.

LSM303DLHC.py

#!/usr/bin/python
import smbus
import time
import math
import numpy

#initialize accelerations variables
x_data_a = 0
y_data_a = 0
z_data_a = 0

#initialize magnetics variables
x_data_m = 0
y_data_m = 0
z_data_m = 0

#set some initial values
lsb_xy_m_g                  = 1100            #as value_crb_reg_m is set to +-1.3
lsb_z_m_g                   = 980             #as value_crb_reg_m is set to +-1.3

#set the I2C ports (0x19 = accelerometer, 0x1e = magnetometer)
device_a = 0x19
device_m = 0x1E

bus=smbus.SMBus(1)

#set register values
#ctrl_reg1_a
odr_ctrl_reg1_a   = 0b0100 #50 Hz
lpen_ctrl_reg1_a  = 0b0    #low-power-mode = normal
zen_ctrl_reg1_a   = 0b1    #z axis enabled 
yen_ctrl_reg1_a   = 0b1    #y axis enabled
xen_ctrl_reg1_a   = 0b1    #x axis enabled
value_ctrl_reg1_a = (odr_ctrl_reg1_a << 4) + (lpen_ctrl_reg1_a << 3) + (zen_ctrl_reg1_a << 2) + (yen_ctrl_reg1_a << 1) + xen_ctrl_reg1_a
#ctrl_reg4_a
bdu_ctrl_reg4_a   = 0b0    #continuos update
ble_ctrl_reg4_a   = 0b0    #data LSB @ lower address
fs_ctrl_reg4_a    = 0b00   #+/- 2g
hr_ctrl_reg4_a    = 0b1    #high resolution enabled
sim_ctrl_reg4_a   = 0b0    #4-wire interface
value_ctrl_reg4_a = (bdu_ctrl_reg4_a << 7) + (ble_ctrl_reg4_a << 6) + (fs_ctrl_reg4_a << 4) + (hr_ctrl_reg4_a << 3) + sim_ctrl_reg4_a
#cra_reg_m
tempen_cra_reg_m  = 0b1    #temperature sensor enabled
do_cra_reg_m      = 0b011  #7.5 minimum data output rate (Hz)
value_cra_reg_m   = (tempen_cra_reg_m << 7) + (do_cra_reg_m << 2)
#crb_reg_m
gn_crb_reg_m      = 0b001  # +-1.3 sensor input field range (Gauss), 1100 Gain X, Y and Z (LSB/Gauss) 980 Gain Z (LSB/Gauss), 0xF800-0x07ff (-2048-2047)
value_crb_reg_m   = (gn_crb_reg_m << 5)
#mr_reg_m
md_mr_reg_m       = 00     #Continuous-conversion mode
value_mr_reg_m    = md_mr_reg_m

#register address mapping accelerometer
ctrl_reg1_a       = 0x20 #rw   010 0000
ctrl_reg2_a       = 0x21 #rw   010 0001
ctrl_reg3_a       = 0x22 #rw   010 0010
ctrl_reg4_a       = 0x23 #rw   010 0011
ctrl_reg5_a       = 0x24 #rw   010 0100
ctrl_reg6_a       = 0x25 #rw   010 0101
reference_a       = 0x26 #rw   010 0110
status_reg_a      = 0x27 #r    010 0111
out_x_l_a         = 0x28 #r    010 1000
out_x_h_a         = 0x29 #r    010 1001
out_y_l_a         = 0x2A #r    010 1010
out_y_h_a         = 0x2B #r    010 1011
out_z_l_a         = 0x2C #r    010 1100
out_z_h_a         = 0x2D #r    010 1101
fifo_ctrl_reg_a   = 0x2E #rw   010 1110
fifo_src_reg_a    = 0x2F #r    010 1111
int1_cfg_a        = 0x30 #rw   011 0000
int1_source_a     = 0x31 #r    011 0001
int1_ths_a        = 0x32 #rw   011 0010
int1_duration_a   = 0x33 #rw   011 0011
int2_cfg_a        = 0x34 #rw   011 0100
int2_source_a     = 0x35 #r    011 0101
int2_ths_a        = 0x36 #rw   011 0110
int2_duration_a   = 0x37 #rw   011 0111
click_cfg_a       = 0x38 #rw   011 1000
click_src_a       = 0x39 #rw   011 1001
click_ths_a       = 0x3A #rw   011 1010
time_limit_a      = 0x3B #rw   011 1011
time_latency_a    = 0x3C #rw   011 1100
time_window_a     = 0x3D #rw   011 1101

#register address mapping magnetometer
cra_reg_m         = 0x00 #rw   00000000
crb_reg_m         = 0x01 #rw   00000001
mr_reg_m          = 0x02 #rw   00000010
out_x_h_m         = 0x03 #r    00000011
out_x_l_m         = 0x04 #r    00000100
out_z_h_m         = 0x05 #r    00000101
out_z_l_m         = 0x06 #r    00000110
out_y_h_m         = 0x07 #r    00000111
out_y_l_m         = 0x08 #r    00001000
sr_reg_mg         = 0x09 #r    00001001
ira_reg_m         = 0x0A #r    00001010
irb_reg_m         = 0x0B #r    00001011
irc_reg_m         = 0x0C #r    00001100
temp_out_h_m      = 0x31 #r    00000000
temp_out_l_m      = 0x32 #r    00000000

#Enable the Accelerometer

#power mode selection
#CTRL_REG1_A register
#ODR3 ODR2 ODR1 ODR0 LPen Zen Yen Xen
#CTRL_REG1_A description
#ODR3-0 = Data rate selection. Default value: 0 (0000: power-down, others: refer to data rate configuration
#LPen = Low-power mode enable. Default value: 0 (0: normal mode, 1: low-power mode)
#Zen = Z axis enable. Default value: 1 (0: Z axis disabled, 1: Z axis enabled)
#Yen = Y axis enable. Default value: 1 (0: Y axis disabled, 1: Y axis enabled)
#Xen = X axis enable. Default value: 1 (0: X axis disabled, 1: X axis enabled)
#rate configuration:
#0000 = Power-down mode
#0001 = Normal / low-power mode (1 Hz)
#0010 = Normal / low-power mode (10 Hz)
#0011 = Normal / low-power mode (25 Hz)
#0100 = Normal / low-power mode (50 Hz)
#0101 = Normal / low-power mode (100 Hz)
#0110 = Normal / low-power mode (200 Hz)
#0111 = Normal / low-power mode (400 Hz)
#1000 = low-power mode (1.620 Hz)
#1001 = Normal (1.344 kHz) / low-power mode (5.376 kHz)
bus.write_byte_data (device_a,ctrl_reg1_a,value_ctrl_reg1_a) 
time.sleep (0.01)

#CTRL_REG4_A register
#BDU BLE FS1 FS0 HR 0(1) 0(1) SIM
#(1) This bit must be set to '0' for correct working of the device.
#CTRL_REG4_A description
#BDU = Block data update. Default value: 0 (0: continuos update, 1: output registers not updated until MSB and LSB reading
#BLE = Big/little endian data selection. Default value 0. (0: data LSB @ lower address, 1: data MSB @ lower address)
#FS1-FS0  = Full-scale selection. Default value: 00 (00: +/- 2G, 01: +/- 4G, 10: +/- 8G, 11: +/- 16G)
#HR = High resolution output mode: Default value: 0 (0: high resolution disable, 1: high resolution enable)
#SIM = SPI serial interface mode selection. Default value: 0 (0: 4-wire interface, 1: 3-wire interface).
bus.write_byte_data (device_a,ctrl_reg4_a,value_ctrl_reg4_a)
time.sleep (0.01)

#Enable the Magnetometer and Temperature sensor

#CRA_REG_M register
#TEMP_EN 0(1) 0(1) DO2 DO1 DO0 0(1) 0(1)
#(1) This bit must be set to '0' for correct working of the device
#TEMP_EN = Temperature sensor enable. 0: temperature sensor disabled (default), 1: temperature sensor enabled
#DO2 to DO0 = Data output rate bits. These bits set the rate at which data is written to all three data output registers (refer to Data rate configurations) Default value: 100
#data rate configuration:
#000 = 0.75 Minimum data output rate (Hz)
#001 = 1.5  Minimum data output rate (Hz)
#010 = 3.0  Minimum data output rate (Hz)
#011 = 7.5  Minimum data output rate (Hz)
#100 = 15   Minimum data output rate (Hz)
#101 = 30   Minimum data output rate (Hz)
#110 = 75   Minimum data output rate (Hz)
#111 = 220  Minimum data output rate (Hz)
bus.write_byte_data (device_m,cra_reg_m,value_cra_reg_m)
time.sleep (0.01)

#CRB_REG_M register
#GN2 GN1 GN0 0(1) 0(1) 0(1) 0(1) 0(1)
#(1) This bit must be set to '0' for correct working of the device
#GN1 to GN0 = Gain configuration bits. The gain configuration is common for all channels (refer to gain setting)
#gain setting:
#001 = +-1.3 sensor input field range (Gauss), 1100 Gain X, Y and Z (LSB/Gauss) 980 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
#010 = +-1.9 sensor input field range (Gauss), 855 Gain X, Y and Z (LSB/Gauss) 760 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
#011 = +-2.5 sensor input field range (Gauss), 670 Gain X, Y and Z (LSB/Gauss) 600 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
#100 = +-4.0 sensor input field range (Gauss), 450 Gain X, Y and Z (LSB/Gauss) 400 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
#101 = +-4.7 sensor input field range (Gauss), 400 Gain X, Y and Z (LSB/Gauss) 355 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
#110 = +-5.6 sensor input field range (Gauss), 330 Gain X, Y and Z (LSB/Gauss) 295 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
#111 = +-8.1 sensor input field range (Gauss), 230 Gain X, Y and Z (LSB/Gauss) 205 Gain Z (LSB/Gauss), 0xF800 - 0x07FF (-2048-2047) output range
bus.write_byte_data (device_m,crb_reg_m,value_crb_reg_m)
time.sleep (0.01)

#MR_REG_M register
#0(1) 0(1) 0(1) 0(1) 0(1) 0(1) MD1 MD0
#(1) This bit must be set to '0' for correct working of the device
#MD1 to MD0 = Mode select bits. These bits select the operation mode of this device (refer to magnetic sensor operation mode)
#magnetic sensor operating mode:
#00 = Continuous-conversion mode
#01 = Single-conversion mode
#10 = Sleep-mode. Device is placed in sleep-mode
#11 = Sleep-mode. Device is placed in sleep-mode
bus.write_byte_data (device_m,mr_reg_m,value_mr_reg_m)
time.sleep (0.01)

def readMagnetics(*offset):

  #read magnetics
  try:
    val_out_x_h_m = bus.read_byte_data (device_m,out_x_h_m)
    val_out_x_l_m = bus.read_byte_data (device_m,out_x_l_m)
    val_out_z_h_m = bus.read_byte_data (device_m,out_z_h_m)
    val_out_z_l_m = bus.read_byte_data (device_m,out_z_l_m)
    val_out_y_h_m = bus.read_byte_data (device_m,out_y_h_m)
    val_out_y_l_m = bus.read_byte_data (device_m,out_y_l_m)
  except IOError:
    subprocess.call(['i2cdetect', '-y', '1'])
    flag = 1     #optional flag to signal your code to resend or something

  #magnetics bite slide
  x_data_m = (((val_out_x_h_m & 0x000F) << 8) + val_out_x_l_m)
  y_data_m = (((val_out_y_h_m & 0x000F) << 8) + val_out_y_l_m)
  z_data_m = (((val_out_z_h_m & 0x000F) << 8) + val_out_z_l_m)

  #calculate two's complement
  x_data_m = twos_comp(x_data_m, 12)
  y_data_m = twos_comp(y_data_m, 12)
  z_data_m = twos_comp(z_data_m, 12)

  #remove the noise
  x_data_m = x_data_m - offset[0]
  y_data_m = y_data_m - offset[1]
  z_data_m = z_data_m - offset[2]

  return [(x_data_m),(y_data_m),(z_data_m)]

def readMagneticGauss(*offset):

  global lsb_xy_m_g
  global lsb_z_m_g

  mag_factor = 1 / float(lsb_xy_m_g) #magnetic factor

  #get the magnetic values
  magnetics = readMagnetics(*offset)
  x_data_m = magnetics[0]
  y_data_m = magnetics[1]
  z_data_m = magnetics[2]

  #adjust the magnetic gain value
  if (x_data_m >= 4090 or x_data_m <= -4090 or y_data_m >= 4090 or y_data_m <= -4090 or z_data_m >= 4090 or z_data_m <= -4090):
    magneticGain = setMagneticGain(gn_crb_reg_m)
    lsb_xy_m_g = magneticGain[0]
    lsb_z_m_g = magneticGain[1]
    mag_factor = magneticGain[2]

  #calculate magnetic Gauss unit
  x_data_m_g = x_data_m * mag_factor
  y_data_m_g = y_data_m * mag_factor
  z_data_m_g = z_data_m * mag_factor

  return [(x_data_m_g),(y_data_m_g),(z_data_m_g)]

def readMagneticHeading(*offset):

  #get the magnetic gauss values
  magnetic_gauss = readMagneticGauss(*offset)
  x_data_m_g = magnetic_gauss[0]
  y_data_m_g = magnetic_gauss[1]

  #calculate the heading
  heading_d_m = math.degrees(math.atan2(y_data_m_g, x_data_m_g))
  heading_wd_m = ''

  if (heading_d_m < 0):
    heading_d_m += 360

  #get the wind direction
  if (heading_d_m > 337.5 or heading_d_m <= 22.5):
    heading_wd_m = 'N'
  elif (heading_d_m > 22.5 and heading_d_m <= 67.5):
    heading_wd_m = 'NE'
  elif (heading_d_m > 67.5 and heading_d_m <= 110.5):
    heading_wd_m = 'E'
  elif (heading_d_m > 110.5 and heading_d_m <= 157.5):
    heading_wd_m = 'SE'
  elif (heading_d_m > 157.5 and heading_d_m <= 202.5):
    heading_wd_m = 'S'
  elif (heading_d_m > 202.5 and heading_d_m <= 247.5):
    heading_wd_m = 'SW'
  elif (heading_d_m > 247.5 and heading_d_m <= 292.5):
    heading_wd_m = 'W'
  elif (heading_d_m > 292.5 and heading_d_m <= 337.5):
    heading_wd_m = 'NW'
  else:
    heading_wd_m = heading_d_m

  return [(heading_d_m), (heading_wd_m)]

def readTemperatures():

  temp_data_m_c_offset = 19.5

  #read temperature
  val_temp_out_l_m = bus.read_byte_data (device_m, temp_out_l_m)
  val_temp_out_h_m = bus.read_byte_data (device_m, temp_out_h_m)

  #temperature bite slide
  temp_data_m = (((val_temp_out_h_m << 8) + val_temp_out_l_m) >> 4) #temperature raw
  temp_data_m_c = (twos_comp(temp_data_m, 12) / 8.0) + temp_data_m_c_offset #temperature in celsuis + offset
  temp_data_m_f = temp_data_m_c * 1.8 + 32 #temperature in fahrenheit

  return [(temp_data_m_c),(temp_data_m_f)]

def readAccelerations():

  #read accelerations
  try:
    val_out_x_l_a = bus.read_byte_data (device_a,out_x_l_a)
    val_out_x_h_a = bus.read_byte_data (device_a,out_x_h_a)
    val_out_y_l_a = bus.read_byte_data (device_a,out_y_l_a)
    val_out_y_h_a = bus.read_byte_data (device_a,out_y_h_a)
    val_out_z_l_a = bus.read_byte_data (device_a,out_z_l_a)
    val_out_z_h_a = bus.read_byte_data (device_a,out_z_h_a)
  except IOError:
    subprocess.call(['i2cdetect', '-y', '1'])
    flag = 1     #optional flag to signal your code to resend or something

  #accelerations bite slide
  x_data_a = (((val_out_x_h_a << 8) + val_out_x_l_a) >> 4) 
  y_data_a = (((val_out_y_h_a << 8) + val_out_y_l_a) >> 4) 
  z_data_a = (((val_out_z_h_a << 8) + val_out_z_l_a) >> 4)

  #calculate two's complement
  x_data_a = twos_comp(x_data_a, 12)
  y_data_a = twos_comp(y_data_a, 12)
  z_data_a = twos_comp(z_data_a, 12)

  return [(x_data_a),(y_data_a),(z_data_a)]

def readAccelerationG():

  #get the acceleration values
  accelerations = readAccelerations()
  x_data_a = accelerations[0]
  y_data_a = accelerations[1]
  z_data_a = accelerations[2]

  accelFactor = setAccelerationScale(fs_ctrl_reg4_a)

  #calc accelerations in G unit
  x_data_a_g = x_data_a * accelFactor
  y_data_a_g = y_data_a * accelFactor
  z_data_a_g = z_data_a * accelFactor

  return [(x_data_a_g),(y_data_a_g),(z_data_a_g)]

def setAccelerationScale(scale):
  if scale == 0b00: #+/- 2G
    accelFactor = 0.001
  elif scale == 0b01: #+/- 4G
    accelFactor = 0.002
  elif scale == 0b10: #+/- 8G
    accelFactor = 0.004
  elif scale == 0b11: #+/- 16G
    accelFactor = 0.012
  
  return accelFactor

def setMagneticGain(gain):

  test1 = bus.read_byte_data (device_m,crb_reg_m,value_crb_reg_m)
  print "Test1: " % test1

  if gain == 0b001: #+-1.3
    lsb_xy_m_g = 855
    lsb_z_m_g = 760
    gn_crb_reg_m = 0b010
    print "Changing range to +/- 1.9"
  elif gain == 0b010: #+-1.9
    lsb_xy_m_g = 670
    lsb_z_m_g = 600
    gn_crb_reg_m = 0b011
    print "Changing range to +/- 2.5"
  elif gain == 0b011: #+-2.5
    lsb_xy_m_g = 450
    lsb_z_m_g = 400
    gn_crb_reg_m = 0b100
    print "Changing range to +/- 4.0"
  elif gain == 0b100: #+-4.0
    lsb_xy_m_g = 400
    lsb_z_m_g = 255
    gn_crb_reg_m = 0b101
    print "Changing range to +/- 4.7"
  elif gain == 0b101: #+-4.7
    lsb_xy_m_g = 330
    lsb_z_m_g = 295
    gn_crb_reg_m = 0b110
    print "Changing range to +/- 5.6"
  elif gain == 0b110: #+-5.6
    lsb_xy_m_g = 230
    lsb_z_m_g = 205
    gn_crb_reg_m = 0b111
    print "Changing range to +/- 8.1"
  elif gain == 0b111: #+-8.1
    lsb_xy_m_g = 230
    lsb_z_m_g = 205
    gn_crb_reg_m = 0b111
    print "Remaining at +/- 8.1"

  mag_factor = 1 / lsb_xy_m_g

  value_crb_reg_m   = (gn_crb_reg_m << 5)
  bus.write_byte_data (device_m,crb_reg_m,value_crb_reg_m)

  print "bus written..."

  test2 = bus.read_byte_data (device_m,crb_reg_m,value_crb_reg_m)
  print "Test2: %s" % test2

  time.sleep (0.01)

  return [(lsb_xy_m_g), (lsb_z_m_g), (mag_factor)]

def twos_comp(val, bits):

  #compute the 2's compliment of int value val
  if((val&(1<<(bits-1))) != 0):
    val = val - (1<<bits)

  return val

def calibrateMag():

  print "Calibrating Magnetometer, please move sensor to all directions..."
  time.sleep(15) #delay for 15 seconds
  print "Start moving now..."

  x_min_m = 999999
  y_min_m = 999999
  z_min_m = 999999
  x_max_m = -999999
  y_max_m = -999999
  z_max_m = -999999

  for i in range (1,1000):

    x_value_m = readMagnetics(0,0,0)[0]
    y_value_m = readMagnetics(0,0,0)[1]
    z_value_m = readMagnetics(0,0,0)[2]

    if(x_value_m < x_min_m):
      x_min_m = x_value_m
    elif(x_value_m > x_max_m):
      x_max_m = x_value_m

    if(y_value_m < y_min_m):
      y_min_m = y_value_m
    elif(y_value_m > y_max_m):
      y_max_m = y_value_m

    if(z_value_m < z_min_m):
      z_min_m = z_value_m
    elif(z_value_m > z_max_m):
      z_max_m = z_value_m

  x_offset_m = (x_min_m + x_max_m) / 2
  y_offset_m = (y_min_m + y_max_m) / 2
  z_offset_m = (z_min_m + z_max_m) / 2

  print "Magnetometer minimum values are: {0}, {1}, {2}".format(x_min_m, y_min_m, z_min_m)
  print "Magnetometer maximum values are: {0}, {1}, {2}".format(x_max_m, y_max_m, z_max_m)
  print "Magnetometer average values are: {0}, {1}, {2}".format(x_offset_m, y_offset_m, z_offset_m)

  return [(x_offset_m),(y_offset_m),(z_offset_m)]

minimutest.py

#!/usr/bin/python
import LSM303DLHC
import L3GD20
import time
import sys

#calibrate the Gyrometer
xyz_offset_g = L3GD20.calibrateGyro()

#calibrate the Magnetometer
xyz_offset_m = LSM303DLHC.calibrateMag()

print "XYZ_OFFSET_G: {0}".format(xyz_offset_g)
print "XYZ_OFFSET_M: {0}".format(xyz_offset_m)

while 1:

  magneticsRaw = LSM303DLHC.readMagnetics(*xyz_offset_m)
  magneticGauss = LSM303DLHC.readMagneticGauss(*xyz_offset_m)
  magneticHeading = LSM303DLHC.readMagneticHeading(*xyz_offset_m)
  magneticTemp = LSM303DLHC.readTemperatures()
  accelerationsRaw = LSM303DLHC.readAccelerations()
  accelerationG = LSM303DLHC.readAccelerationG()
  gyroRaw = L3GD20.readGyro(*xyz_offset_g)
  gyroDPS = L3GD20.readDPS(*xyz_offset_g)

  print "MagnetX: {0}, MagnetY: {1}, MagnetZ: {2}".format(magneticsRaw[0], magneticsRaw[1], magneticsRaw[2])
  print "AccelX: {0}, AccelY: {1}, AccelZ: {2}".format(accelerationsRaw[0], accelerationsRaw[1], accelerationsRaw[2])
  print "MGaussX: {0}, MGaussY: {1}, MGaussZ: {2}".format(magneticGauss[0], magneticGauss[1], magneticGauss[2])
  print "MuTX: {0}, MuTY: {1}, MuTZ: {2}".format(magneticGauss[0] * 100, magneticGauss[1] * 100, magneticGauss[2] * 100)
  print "AgX: {0}, AgY: {1}, AgZ: {2}".format(accelerationG[0],  accelerationG[1],  accelerationG[2])
  print "MHeading: {0}".format(magneticHeading[0])
  print "MWindDir: {0}".format(magneticHeading[1])
  print "MTemp(F): {0}, MTemp(C): {1}".format(magneticTemp[0], magneticTemp[1])
  print "GyroX: {0}, GyroY: {1}, GyroZ: {2}".format(gyroRaw[0], gyroRaw[1], gyroRaw[2])
  print "GyroDPSX: {0}, GyroDPSY: {1}, GyroDPSZ: {2}".format(gyroDPS[0], gyroDPS[1], gyroDPS[2])

  sys.stdout.flush()

  time.sleep(0.1)

L3GD20.py

#!/usr/bin/python
import smbus 
import time

#initialize gyro variables
x_data_g = 0
y_data_g = 0
z_data_g = 0

#set the I2C ports (0x6b = gyrometer)
device_g = 0x6b

bus=smbus.SMBus(1)

#set register values

#ctrl_reg1_g
dr_ctrl_reg1_g    = 0b00   #ODR 95 Hz
bw_ctrl_reg1_g    = 0b00   #Cut-Off 12.5
pd_ctrl_reg1_g    = 0b0    #power-down mode
zen_ctrl_reg1_g   = 0b1    #z axis enabled
xen_ctrl_reg1_g   = 0b1    #x axis enabled
yen_ctrl_reg1_g   = 0b1    #y axis enabled
value_ctrl_reg1_g = (dr_ctrl_reg1_g << 6) + (bw_ctrl_reg1_g << 4) + (pd_ctrl_reg1_g << 3) + (zen_ctrl_reg1_g << 2) + (xen_ctrl_reg1_g << 1) + yen_ctrl_reg1_g
#ctrl_reg2_g
hpm_ctrl_reg2_g   = 0b00   #High pass filter mode normal
hpc_ctrl_reg2_g   = 0b0000 #ODR 95Hz / cut-off 7.2, ODR 190Hz / cut-off 13.5, ODR 380Hz / cut-off 27, ODR 760Hz / cut-off 51.4
value_ctrl_reg2_g = (hpm_ctrl_reg2_g << 4) + hpc_ctrl_reg2_g
#ctrl_reg3_g
i1_ctrl_reg3_g    = 0b00   #Interrupt disable on INT1 pin, Boot status available on INT1 disabled
h_ctrl_reg3_g     = 0b0    #Interrupt active configuration on INT1 high
pp_ctrl_reg3_g    = 0b0    #push-pull
i2_ctrl_reg3_g    = 0b0000 #Date-ready on DRDYANT2 disabled, FIFO watermarking interrupt on DRDYANT2 disabled, FIFO overrun interrupt on DRDYANT2 disabled, FIFO empty interrupt on DRDYANT2 disabled
value_ctrl_reg3_g = (i1_ctrl_reg3_g << 6) + (h_ctrl_reg3_g << 5) + (pp_ctrl_reg3_g << 4) + i2_ctrl_reg3_g
#ctrl_reg4_g
bdu_ctrl_reg4_g   = 0b0    #Block data update continuos
ble_ctrl_reg4_g   = 0b0    #Big/little endian data selection data lsb @ low address
fs_ctrl_reg4_g    = 0b00   #full scale selection 250 dps
sim_ctrl_reg4_g   = 0b0    #SPI serial interface mode selection 4-wire interface
value_ctrl_reg4_g = (bdu_ctrl_reg4_g << 7) + (ble_ctrl_reg4_g << 6) + (fs_ctrl_reg4_g << 4) + sim_ctrl_reg4_g
#ctrl_reg5_g
boot_ctrl_reg5_g  = 0b0    #Reboot memory content normal mode
fifo_ctrl_reg5_g  = 0b0    #FIFO disabled
hpen_ctrl_reg5_g  = 0b0    #High-pass filter disabled
int1_ctrl_reg5_g  = 0b00   #INT1 selection configuration wait disabled
out_ctrl_reg5_g   = 0b00   #OUT selection configuration wait disabled
value_ctrl_reg5_g = (boot_ctrl_reg5_g << 7) + (fifo_ctrl_reg5_g << 6) + (hpen_ctrl_reg5_g << 4) + (int1_ctrl_reg5_g << 2) + out_ctrl_reg5_g

who_am_i_g      = 0x0f #r   0001111  Device identification register
ctrl_reg1_g     = 0x20 #rw  0100000  Control register 1
ctrl_reg2_g     = 0x21 #rw  0100001  Control register 2
ctrl_reg3_g     = 0x22 #rw  0100010  Control register 3
ctrl_reg4_g     = 0x23 #rw  0100011  Control register 4
ctrl_reg5_g     = 0x24 #rw  0100100  Control register 5
reference_g     = 0x25 #rw  0100101  Reference value for interrupt generation
out_temp_g      = 0x26 #r   0100110  Output temperature
status_reg_g    = 0x27 #r   0100111  Status register
out_x_l_g       = 0x28 #r   0101000  X-axis angular data rate LSB
out_x_h_g       = 0x29 #r   0101001  X-axis angular data rate MSB
out_y_l_g       = 0x2a #r   0101010  Y-axis angular data rate LSB
out_y_h_g       = 0x2b #r   0101011  Y-axis angular data rate MSB
out_z_l_g       = 0x2c #r   0101100  Z-axis angular data rate LSB
out_z_h_g       = 0x2d #r   0101101  Z-axis angular data rate MSB
fifo_ctrl_reg_g = 0x2e #rw  0101110  Fifo control register
fifo_src_reg_g  = 0x2f #r   0101111  Fifo src register
int1_cfg_g      = 0x30 #rw  0110000  Interrupt 1 configuration register
int1_src_g      = 0x31 #r   0110001  Interrupt source register
int1_ths_xh_g   = 0x32 #rw  0110010  Interrupt 1 threshold level X MSB register
int1_ths_xl_g   = 0x33 #rw  0110011  Interrupt 1 threshold level X LSB register
int1_ths_yh_g   = 0x34 #rw  0110100  Interrupt 1 threshold level Y MSB register
int1_ths_yl_g   = 0x35 #rw  0110101  Interrupt 1 threshold level Y LSB register
int1_ths_zh_g   = 0x36 #rw  0110110  Interrupt 1 threshold level Z MSB register
int1_ths_zl_g   = 0x37 #rw  0110111  Interrupt 1 threshold level Z LSB register
int1_duration_g = 0x38 #rw  0111000  Interrupt 1 duration register

#Enable the Gyrometer

#CTRL_REG1_G register
#DR1 DR0 BW1 BW0 PD Zen Xen Yen
#CTRL_REG1_G description
#DR1-DR0 = Output data rate selection (refer to DR and BW configuration setting)
#BW1-BW0 = Bandwidth selection (refer to DR and BW configuration setting)
#PD = Power-down mode enable. Devault value: 0 (0: power-down mode, 1: normal mode or sleep mode) (refer to power mode selection configuration)
#Zen = Z axis enable, Default value: 1 (0: Z axis disabled, 1: Z axis enabled)
#Yen = Y axis enable, Default value: 1 (0: Y axis disabled, 1: Y axis enabled)
#Xen = X axis enable, Default value: 1 (0: X axis disabled, 1: X axis enabled)
#DR and BW configuration setting:
#0000 = ODR 95 Hz, Cut-Off 12.5
#0001 = ODR 95 Hz, Cut-Off 25
#0010 = ODR 95 Hz, Cut-Off 25
#0011 = ODR 95 Hz, Cut-Off 25
#0100 = ODR 190 Hz, Cut-Off 12.5
#0101 = ODR 190 Hz, Cut-Off 25
#0110 = ODR 190 Hz, Cut-Off 50
#0111 = ODR 190 Hz, Cut-Off 70
#1000 = ODR 380 Hz, Cut-Off 20
#1001 = ODR 380 Hz, Cut-Off 25
#1010 = ODR 380 Hz, Cut-Off 50
#1011 = ODR 380 Hz, Cut-Off 100
#1100 = ODR 760 Hz, Cut-Off 30
#1101 = ODR 760 Hz, Cut-Off 35
#1110 = ODR 760 Hz, Cut-Off 50
#1111 = ODR 760 Hz, Cut-Off 100
#power mode selection configuration:
#PD Zen Yen Xen Mode
#0  -   -   -   power-down
#1  0   0   0   sleep
#1  -   -   -   normal 
bus.write_byte_data (device_g,ctrl_reg1_g,value_ctrl_reg1_g)
time.sleep (0.01)

#CTRL_REG2_G register
#0(1) 0(1) HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
#(1) These bits must be set to '0' to ensure proper operation of the device
#CTRL_REG2_G description
#HPM1-HPM0 = High-pass filter mode selection. Default value: 00 (refer to high-pass filter mode configuration)
#HPCF3-HPCF0 = High-pass filter cutoff frequency selection (refer to high-pass filter cut off frequency configuration (Hz)
#high-pass filter mode configuration
#HPM1 HPM0 High-pass filter mode
#0    0    Normal mode (reset reading HP_RESET_FILTER)
#0    1    Reference signal for filtering
#1    0    Normal mode
#1    1    Autoreset on interrupt event
#high-pass filter cut off frequency configuration
#HPCF3-0 ODR=95 Hz ODR=190 Hz ODR=380 Hz ODR=760 Hz
#0000    7.2       13.5       27         51.4
#0001    3.5        7.2       13.5       27
#0010    1.8        3.5        7.2       13.5
#0011    0.9        1.8        3.5        7.2
#0100    0.45       0.9        1.8        3.5
#0101    0.18       0.45       0.9        1.8
#0110    0.09       0.18       0.45       0.9
#0111    0.045      0.09       0.18       0.45
#1000    0.018      0.045      0.09       0.18
#1001    0.009      0.018      0.045      0.09
bus.write_byte_data (device_g,ctrl_reg2_g,value_ctrl_reg2_g)
time.sleep (0.01)
    
#CTRL_REG3_G register
#I1_Int1 I1_Boot H_Lactive PP_OD I2_DRDY I2_WTM I2_ORun I2_Empty
#CTRL_REG3_G description
#I1_Int1 = Interrupt enable on INT1 pin. Default value 0. (0: disable; 1: enable)
#I1_Boot = Boot status available on INT1. Default value 0. (0: disable; 1: enable)
#H_Lactive = Interrupt active configuration on INT1. Default value 0. (0: high; 1:low)
#PP_OD = Push-pull / Open drain. Default value: 0. (0: push- pull; 1: open drain)
#I2_DRDY = Date-ready on DRDY/INT2. Default value 0. (0: disable; 1: enable)
#I2_WTM = FIFO watermark interrupt on DRDY/INT2. Default value: 0. (0: disable; 1: enable)
#I2_ORun = FIFO overrun interrupt on DRDY/INT2 Default value: 0. (0: disable; 1: enable)
#I2_Empty = FIFO empty interrupt on DRDY/INT2. Default value: 0. (0: disable; 1: enable)
bus.write_byte_data (device_g,ctrl_reg3_g,value_ctrl_reg3_g)
time.sleep (0.01)

#CTRL_REG4_G register
#BDU BLE FS1 FS0 - 0(1) 0(1) SIM
#(1) This value must not be changed.
#CTRL_REG4_G description
#BDU = Block data update. Default value: 0 (0: continuos update; 1: output registers not updated until MSb and LSb read-ing)
#BLE = Big/little endian data selection. Default value 0.(0: Data LSb @ lower address; 1: Data MSb @ lower address)
#FS1-FS0 =  Full scale selection. Default value: 00 (00: 250 dps; 01: 500 dps; 10: 2000 dps; 11: 2000 dps)
#SIM = SPI serial interface mode selection. Default value: 0 (0: 4-wire interface; 1: 3-wire interface).
bus.write_byte_data (device_g,ctrl_reg4_g,value_ctrl_reg4_g)
time.sleep (0.01)

#CTRL_REG5_G register
#BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
#CTRL_REG5_G description
#BOOT = Reboot memory content. Default value: 0 (0: normal mode; 1: reboot memory content)
#FIFO_EN = FIFO enable. Default value: 0 (0: FIFO disable; 1: FIFO Enable)
#HPen = High-pass filter enable. Default value: 0 (0: HPF disabled; 1: HPF enabled See Figure 20)
#INT1_Sel1-INT1_-Sel0 = INT1 selection configuration. Default value: 0 (0: wait disabled, 1: wait enabled)
#Out_Sel1-Out_-Sel1 = Out selection configuration. Default value: 0 (0: wait disabled, 1: wait enabled)
bus.write_byte_data (device_g,ctrl_reg5_g,value_ctrl_reg5_g)
time.sleep (0.01)

def readGyro(*offset):

  #read gyro values
  try:
    val_out_x_h_g = bus.read_byte_data (device_g,out_x_h_g)
    val_out_x_l_g = bus.read_byte_data (device_g,out_x_l_g)
    val_out_z_h_g = bus.read_byte_data (device_g,out_z_h_g)
    val_out_z_l_g = bus.read_byte_data (device_g,out_z_l_g)
    val_out_y_h_g = bus.read_byte_data (device_g,out_y_h_g)
    val_out_y_l_g = bus.read_byte_data (device_g,out_y_l_g)
  except IOError:
    subprocess.call(['i2cdetect', '-y', '1'])
    flag = 1     #optional flag to signal your code to resend or something

  #gyro bite slide
  x_data_g = ((val_out_x_h_g << 8) + val_out_x_l_g)
  y_data_g = ((val_out_y_h_g << 8) + val_out_y_l_g)
  z_data_g = ((val_out_z_h_g << 8) + val_out_z_l_g)

  #calculate two's complement
  y_data_g = twos_comp(x_data_g, 16)
  y_data_g = twos_comp(y_data_g, 16)
  z_data_g = twos_comp(z_data_g, 16)
  #remove the noise
  x_data_g = x_data_g - offset[0]
  y_data_g = y_data_g - offset[1]
  z_data_g = z_data_g - offset[2]

  return [(x_data_g),(y_data_g),(z_data_g)]

def readDPS(*offset):

  #read the gyro values
  xyz_data_g = readGyro(*offset)
  gyroFactor = setGyroScale(fs_ctrl_reg4_g)

  #convert values into degrees per second
  value_dps_x_g = xyz_data_g[0] * gyroFactor
  value_dps_y_g = xyz_data_g[1] * gyroFactor
  value_dps_z_g = xyz_data_g[2] * gyroFactor
 
  return [(value_dps_x_g),(value_dps_y_g),(value_dps_z_g)]

def setGyroScale(scale):
  if scale == 0b00: #250 dps
    gyroFactor = 0.00875
  elif scale == 0b01: #500 dps
    gyroFactor = 0.01750
  elif scale == 0b10: #2000 dps
    gyroFactor = 0.070

  return gyroFactor

def twos_comp(val, bits):
  #compute the 2's compliment of int value val
  if((val&(1<<(bits-1))) != 0):
    val = val - (1<<bits)

  return val

def calibrateGyro():

  x_value_g = 0
  y_value_g = 0
  z_value_g = 0
  x_offset_g = 0
  y_offset_g = 0
  z_offset_g = 0

  print "Calibrating Gyrometer, please do not move sensor..."
  time.sleep(15) #delay for 15 seconds

  for i in range (1,1000):

    #read the gyro values
    x_value_g = readGyro(0,0,0)[0]
    y_value_g = readGyro(0,0,0)[1]
    z_value_g = readGyro(0,0,0)[2]

    #sum up the readings
    x_offset_g += x_value_g
    y_offset_g += y_value_g
    z_offset_g += z_value_g

  #average the readings
  x_offset_g = x_offset_g / 1000
  y_offset_g = y_offset_g / 1000
  z_offset_g = z_offset_g / 1000

  print "Gyrometer offset values are: '{0}', '{1}', '{2}'".format(x_offset_g, y_offset_g, z_offset_g)

  return [(x_offset_g),(y_offset_g),(z_offset_g)]

Thanks!

updated 26.08.2014

JonathanKosh · May 29, 2014, 6:27pm

Hello.

Is there anything specific you are having trouble with or want to improve?

-Jon

codac · May 30, 2014, 6:24am

Hi Jon,

yes ineed, I struggle with getting proper values.

The Raw values for the Magnetometer and Accelerometer seem to be wrong already (the register settings might be a reason).
Furthermore I’m not exactly sure how to calculate the Gauss values out of the raw values and the correct heading.

Currently I just get some values, where I dont have any idea how to compare them with realistic values and do a proper calibration.

I tried to change the magnetic gain settings, but changing the registers to any other value doesn’t seem to have any effect (see setMagneticGain())

codac

JonathanKosh · May 31, 2014, 12:27am

What makes you think your raw values are wrong? Usually the exact magnitude of the magnetic field is not important; it’s just useful to know which direction the field is pointing. If you post a sample of the output you are getting, I might be able to tell you if I notice anything obviously wrong.

Do you have an Arduino you can use to try running our program on to compare your results with?

-Jon

codac · May 31, 2014, 2:35pm

I have updated the code in my last code related post. I have removed the automatic calibration as I just got worse values.

MagnetX: 209, MagnetY: 4015, MagnetZ: 3515
AccelX: 3954.000, AccelY: 310.000, AccelZ: 927.000
MGaussX: 0.190, MGaussY: -0.074, MGaussZ: 0.190
AgX: -0.142, AgY: 0.310, AgZ: 0.927
MHeading: 338.816
Temp(F): 68.900, Temp(C): 20.500

MagnetX: 211, MagnetY: 4015, MagnetZ: 3515
AccelX: 3961.000, AccelY: 309.000, AccelZ: 924.000
MGaussX: 0.192, MGaussY: -0.074, MGaussZ: 0.192
AgX: -0.135, AgY: 0.309, AgZ: 0.924
MHeading: 338.999
Temp(F): 68.900, Temp(C): 20.500

MagnetX: 211, MagnetY: 4019, MagnetZ: 3516
AccelX: 3957.000, AccelY: 311.000, AccelZ: 928.000
MGaussX: 0.192, MGaussY: -0.070, MGaussZ: 0.192
AgX: -0.139, AgY: 0.311, AgZ: 0.928
MHeading: 339.952
Temp(F): 68.900, Temp(C): 20.500

MagnetX: 212, MagnetY: 4014, MagnetZ: 3518
AccelX: 3956.000, AccelY: 308.000, AccelZ: 932.000
MGaussX: 0.193, MGaussY: -0.075, MGaussZ: 0.193
AgX: -0.140, AgY: 0.308, AgZ: 0.932
MHeading: 338.854
Temp(F): 68.900, Temp(C): 20.500

MagnetX: 211, MagnetY: 4013, MagnetZ: 3518
AccelX: 3960.000, AccelY: 311.000, AccelZ: 926.000
MGaussX: 0.192, MGaussY: -0.075, MGaussZ: 0.192
AgX: -0.136, AgY: 0.311, AgZ: 0.926
MHeading: 338.527
Temp(F): 68.900, Temp(C): 20.500

MagnetX: 212, MagnetY: 4014, MagnetZ: 3516
AccelX: 3952.000, AccelY: 313.000, AccelZ: 929.000
MGaussX: 0.193, MGaussY: -0.075, MGaussZ: 0.193
AgX: -0.144, AgY: 0.313, AgZ: 0.929
MHeading: 338.854
Temp(F): 68.900, Temp(C): 20.500

Magnetometer: I have actually no idea if the values are half way realistic or not, which makes it quite hard to implement a proper calibration. Maybe someone can help here?
Accelerometer: If I’m not mistaken the X and Y are a bit off as they should read 0 but will of course have some noise. The Z axis shows ~1G (0.929G) which is a bit off as well. Any suggestions about how to get more accurate values here?
MagneticHeading: The heading is ~30 - 40 degrees off (please see the attached photo where the sensor shows a heading of 0.00, but the magnetic compass shows 327°

The temperature is fine.

I would love to have an automated calibration, starting everytime I start the script as I’m going to use this sensor at many different places.
My goal here is to read as realistic values as possible, not to use the raw values for navigation purposes.

Unfortunately I dont have an Arduino… a Raspberry Pi only…

JonathanKosh · June 3, 2014, 1:30am

Earth’s magnetic field ranges from 0.25 to 0.65 gauss, so the magnitude of those readings look like they are in the correct general range, but the exact values are probably not accurate because of the offset, which is probably what’s causing the heading to be off. That is what the calibration is for. You might try looking at this Raspberry Pi library, which has a feature to calibrate the compass.

Your acceleration values look just fine since the magnitude of all your acceleration vectors is close to 1g. Are you sure the board isn’t slightly tilted?

-Jon

codac · June 7, 2014, 10:02am

Thank you for the RasPi library Jon, that helps quite a lot.

Here is the comparison between the C++ RasPi Library and my python script (all values are without any calibration):

C++ RasPi Library:
Gyros (radians/s)
X: -0,045, Y: 0,083, Z: 0,070

Accelerometers (g)
X: 0,042, Y: -0,057, Z: 0,977

Magnetometers (uT)
X: 43.772, Y: 11.465, Z: 13.489

my python script:
Gyros (radians/s)
X: -0,012, Y: 0,010, Z: -0,002 <==== the Z axis has a negative sign (Hint: I’m calculating the degrees per second out of the Raw values and convert them to radians/s by multiplying the degrees per second by 0.017453293)

Accelerometers (g)
X: -0.042, Y: -0,057, Z: 0,978 <==== the X axis has a negative sign

Magnetometers (uT)
X: 34.727, Y: -10,182, Z: 34,818 <==== the Y axis has a negative sign and the Z axis differs quite a lot (Hint: I’m calculating the Gauss values out of the Raw values and convert them to uT by multiplying the Gauss value by 100)

Please find a screenshot attached for comparison.

Yes the MinIMU9 V2 is tilted, which is going to happen with my project anyhow, as its going to be attached to a high altitude balloon and therefore moving all the time in all directions (more or less).

kevin · June 9, 2014, 9:03pm

Hi, codac.

It looks like you’re doing fine with the gyro and accelerometer; the differences between your values and the ones from RTIMULib are very small, and that’s about the expected level of variation (noise) I would expect to see in the output. For the values that are close to zero, the sign of the number is insignificant (on a scale of -1 to +1, the difference between -0.05 and +0.05 is negligible). You can better confirm that your results are matching by rotating the board to see if the gyro and accelerometer outputs change in about the same way in both your program and RTIMULib.

For your magnetometer output, I found it suspicious that your X and Z values were so similar, and I went back and looked at the code you posted. It looks like you made a typo in readMagneticGauss():

return [(x_data_m_g),(y_data_m_g),(x_data_m_g)]

Also, are you compensating your program’s output with a magnetometer calibration? Did you calibrate the compass at any point in RTIMULibDemo? (It saves the calibration values in a settings file, so it will keep using them even if you restart the program.) If you did either of those, they could account for additional differences in the magnetometer outputs.

- Kevin

codac · July 4, 2014, 8:56am

Hi Kevin,

thank you for finding the typo. It’s corrected now.
With the posted values there are no compensations by calibration. It’s all raw so far. I took the measurements with RTIMULib the same way (without calibration).

Current status:

I’ve removed the temperature reading for the Gyro as tests showed that the temperature values wont change with the surrounding temperature. I’ve put the sensor in a freezer and ofer 1,5 hrs the values stayed at 22 degrees room temperature.
I still have a problem with dynamicly setting the magnetic gain (function setMagneticGain). It seems that the determined value is not getting written correctly to the register (using bus.write_byte_data (device_m,crb_reg_m,value_crb_reg_m)).
I’m having quite a hard time implementing an automatic calibration for the gyro, accel, magnet. The idea is that the automatic calibration measures the required values for each sensor, one after another, and then applies the offset values to the rest of the code. There are loads of different algorythms (even RTIMULib offers 3 different ones). If someone might be able to provide some code examples, this would be massively helpful.

The code is updated…

- codac

kevin · July 8, 2014, 7:13pm

I looked at your setMagneticGain code and it seemed reasonable to me. Could you try a simpler test of writing a given value to the register and immediately reading it back to see if it was written correctly?

I don’t have any specific suggestions for calibration, but you seem to have the right idea for the gyro (take some readings while the board isn’t moving to find the zero-rate offset, then subtract from your following readings); just make sure the board actually isn’t rotating while you’re calibrating. I’ve found that the accelerometer generally doesn’t need any calibrating to be useful, but if you do calibrate it the same way, you need to remove the acceleration due to gravity from your calibration offset (one way to do this is to require the board to be level during calibration, then subtract exactly 1 G from the Z axis offset).

The magnetometer is the hardest to calibrate: there usually seem to be significant offsets on all of the axes, and since there’s no practical way to put the magnetometer into a condition where all of its readings should be zero, you need to determine the offsets by finding the minimum and maximum possible readings on each axis and using the midpoint as the calibration offset, and this is what our Calibrate example for the Arduino does. There are also some more complex magnetometer calibration strategies that might be more robust and/or offer better results; one example is described in this article written by another customer.

- Kevin

codac · July 15, 2014, 11:00am

I’m gonna check the setMagneticGain code the way you suggested it. Unfortunately I dont have much time for development/testing…

I’m not a mathematician or physicist, but everywhere I’ve looked the way to go is to get the min and max values for x,y and z axis.
The big question I have is, once I’ve got those values, what am I supposed to do with it?
Here, it seems like an average value (mean) comes into place, and here in Pololu’s code there again is a different approach after getting the min/max values:

m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;

You said I’m supposed to subtract the zero-rate offset from your readings… what kind of value would that be? the min, max or average value?

Inthis code it is shown how to get the min/max/average but not how to use them in combination with the raw values.
Even the scale (sorry I’m German… what exactly does “scale” mean here?) comes into place.

kevin · July 16, 2014, 9:58pm

In your first link, the code is for the gyro (L3GD20), so it is not really applicable to the way we calibrate the magnetometer; the averaging is for filtering out noise so it doesn’t affect the offset.

For example, when the gyro isn’t rotating, we know the readings should be zero on all axes. In reality, it might give readings on each axis that look more like this: [1.9, 2.0, 2.1, 2.0, 1.9, 2.1]. So we take a few of these readings and average them to decide that the offset is about 2, and we subtract that offset from all future readings so that they are centered on 0 (although the noise will still be there).

For the magnetometer, it’s hard to know when the readings should be zero, so instead, we take readings while rotating it in as many directions as possible and then use the midpoint (average) between the min and max as our offset. In a magnetometer with no offset, we would expect the min and max readings on each axis to have the same absolute value; for example, if the magnetometer saw a max reading of +10 on one axis, it should see a min reading of -10 on the same axis when pointing the opposite direction. However, you might find that you get a max reading of +13 and a min of -7. So you would average the max and min to get an offset of 3, which you would subtract from future readings. Does that make sense?

In our code that you quoted, we don’t care about the absolute magnitude of the magnetometer readings, just the direction of the net magnetic field, so we remove the offset and scale each axis to the range [-1, 1] at the same time. If you do care about the magnitude, you can subtract the offset alone:

m.x = m.x - ((m_min.x + m_max.x) / 2);

- Kevin

codac · July 26, 2014, 10:43am

Hi Kevin,

ok this description makes sense.

Here are the calbirated values so far. Please notice that moving the magnetic sensor to all directions might not have been executed perfectly…

MagRawX: -3813 is still a bit high… I’ll have to take a look at it.
AccRawX: 4091 probably as well…

Here is the calibration code that reads the values and calculates the offset:

LSM303DLHC:

def calibrateMag():

  print "Calibrating Magnetometer, please move sensor to all directions..."
  time.sleep(15) #delay for 15 seconds
  print "Start moving now..."

  x_min_m = 999999
  y_min_m = 999999
  z_min_m = 999999
  x_max_m = 0
  y_max_m = 0
  z_max_m = 0

  for i in range (1,1000):

    x_value_m = readMagnetics(0,0,0)[0]
    y_value_m = readMagnetics(0,0,0)[1]
    z_value_m = readMagnetics(0,0,0)[2]

    if(x_value_m < x_min_m):
      x_min_m = x_value_m
    elif(x_value_m > x_max_m):
      x_max_m = x_value_m

    if(y_value_m < y_min_m):
      y_min_m = y_value_m
    elif(y_value_m > y_max_m):
      y_max_m = y_value_m

    if(z_value_m < z_min_m):
      z_min_m = z_value_m
    elif(z_value_m > z_max_m):
      z_max_m = z_value_m

  x_offset_m = (x_min_m + x_max_m) / 2
  y_offset_m = (y_min_m + y_max_m) / 2
  z_offset_m = (z_min_m + z_max_m) / 2

  print "Magnetometer minimum values are: {0}, {1}, {2}".format(x_min_m, y_min_m, z_min_m)
  print "Magnetometer maximum values are: {0}, {1}, {2}".format(x_max_m, y_max_m, z_max_m)
  print "Magnetometer average values are: {0}, {1}, {2}".format(x_offset_m, y_offset_m, z_offset_m)

  return [(x_offset_m),(y_offset_m),(z_offset_m)]

L3GD20

def calibrateGyro():

  x_value_g = 0
  y_value_g = 0
  z_value_g = 0
  x_offset_g = 0
  y_offset_g = 0
  z_offset_g = 0

  print "Calibrating Gyrometer, please do not move sensor..."
  time.sleep(15) #delay for 15 seconds

  for i in range (1,1000):

    #read the gyro values
    x_value_g = readGyro(0,0,0)[0]
    y_value_g = readGyro(0,0,0)[1]
    z_value_g = readGyro(0,0,0)[2]

    #sum up the readings
    x_offset_g += x_value_g
    y_offset_g += y_value_g
    z_offset_g += z_value_g

  #average the readings
  x_offset_g = x_offset_g / 1000
  y_offset_g = y_offset_g / 1000
  z_offset_g = z_offset_g / 1000

  print "Gyrometer offset values are: '{0}', '{1}', '{2}'".format(x_offset_g, y_offset_g, z_offset_g)

  return [(x_offset_g),(y_offset_g),(z_offset_g)]

What do you think?

kevin · July 29, 2014, 9:42pm

I think there is something wrong with the way you’re applying the offsets. For the magnetometer, I suspect it has to do with using unsigned numbers where they should be signed (all of your displayed readings are close to the negative end of the sensor’s range). For the gyro, it’s suspicious that you’re getting the same numbers for all three axes; did you make another typo and use the same variable multiple times like before?

Could you post the code that calculates the values you print, and could you also show output of your calibration code?

- Kevin

codac · July 30, 2014, 4:49pm

Here is the function for reading the magnetic values, followed by getting the Gauss and Heading based on the raw or calibrated values.
The parameter “offset” is filled either by (0,0,0) in case of calibration, so that i get the raw values. As soon as calibration is done, the offset is filled with the offset values (123, 456, 789).

def readMagnetics(*offset):                                <-----------offset

  #read magnetics
  try:
    val_out_x_h_m = bus.read_byte_data (device_m,out_x_h_m)
    val_out_x_l_m = bus.read_byte_data (device_m,out_x_l_m)
    val_out_z_h_m = bus.read_byte_data (device_m,out_z_h_m)
    val_out_z_l_m = bus.read_byte_data (device_m,out_z_l_m)
    val_out_y_h_m = bus.read_byte_data (device_m,out_y_h_m)
    val_out_y_l_m = bus.read_byte_data (device_m,out_y_l_m)
  except IOError:
    subprocess.call(['i2cdetect', '-y', '1'])
    flag = 1     #optional flag to signal your code to resend or something

  #magnetics bite slide
  x_data_m = (((val_out_x_h_m & 0x000F) << 8) + val_out_x_l_m)
  y_data_m = (((val_out_y_h_m & 0x000F) << 8) + val_out_y_l_m)
  z_data_m = (((val_out_z_h_m & 0x000F) << 8) + val_out_z_l_m)

  #remove the noise
  x_data_m = x_data_m - offset[0]                                <-----------offset
  y_data_m = y_data_m - offset[1]                                <-----------offset
  z_data_m = z_data_m - offset[2]                                <-----------offset

  return [(x_data_m),(y_data_m),(z_data_m)]

def readMagneticGauss(*offset):                                <-----------offset

  global lsb_xy_m_g
  global lsb_z_m_g

  #get the magnetic values
  magnetics = readMagnetics(*offset)                                <-----------offset
  x_data_m = magnetics[0]
  y_data_m = magnetics[1]
  z_data_m = magnetics[2]
  mag_factor = 1 / float(lsb_xy_m_g) #magnetic factor

  #adjust the magnetic gain value
  if (x_data_m >= 4090 or x_data_m <= -4090 or y_data_m >= 4090 or y_data_m <= -4090 or z_data_m >= 4090 or z_data_m <= -4090):
    magneticGain = setMagneticGain(gn_crb_reg_m)
    lsb_xy_m_g = magneticGain[0]
    lsb_z_m_g = magneticGain[1]
    mag_factor = magneticGain[2]

  #calculate magnetic Gauss unit
  x_data_m_g = twos_comp(x_data_m, 12) * mag_factor
  y_data_m_g = twos_comp(y_data_m, 12) * mag_factor
  z_data_m_g = twos_comp(z_data_m, 12) * mag_factor

  return [(x_data_m_g),(y_data_m_g),(z_data_m_g)]

def readMagneticHeading(*offset):                                <-----------offset

  #get the magnetic gauss values
  magnetic_gauss = readMagneticGauss(*offset)                                <-----------offset
  x_data_m_g = magnetic_gauss[0]
  y_data_m_g = magnetic_gauss[1]

  #calculate the heading
  heading_d_m = math.degrees(math.atan2(y_data_m_g, x_data_m_g))
  heading_wd_m = ''

  if (heading_d_m < 0):
    heading_d_m += 360

  #get the wind direction
  if (heading_d_m > 337.5 or heading_d_m <= 22.5):
    heading_wd_m = 'N'
  elif (heading_d_m > 22.5 and heading_d_m <= 67.5):
    heading_wd_m = 'NE'
  elif (heading_d_m > 67.5 and heading_d_m <= 110.5):
    heading_wd_m = 'E'
  elif (heading_d_m > 110.5 and heading_d_m <= 157.5):
    heading_wd_m = 'SE'
  elif (heading_d_m > 157.5 and heading_d_m <= 202.5):
    heading_wd_m = 'S'
  elif (heading_d_m > 202.5 and heading_d_m <= 247.5):
    heading_wd_m = 'SW'
  elif (heading_d_m > 247.5 and heading_d_m <= 292.5):
    heading_wd_m = 'W'
  elif (heading_d_m > 292.5 and heading_d_m <= 337.5):
    heading_wd_m = 'NW'
  else:
    heading_wd_m = heading_d_m

  return [(heading_d_m), (heading_wd_m)]

kevin · July 31, 2014, 9:38pm

I don’t think your magnetometer reading code contains any good hints in particular about what’s going wrong. I’d specifically like to see the output of your program when you call calibrateMag() and calibrateGyro(), as well as the code that calculates and prints the output you mentioned in your previous post:

If it’s easier to show your entire source file (or an archive of all your source files), you could post it as an attachment, though it will probably take me some time to look through it.

- Kevin

codac · August 5, 2014, 6:30pm

Hi Kevin,

please find my udpated source code on my first post.

Here you can see the measured offset values:

…and the values:

MagnetX: -1, MagnetY: 1, MagnetZ: -1 <-------- calibrated magnetic values
AccelX: 154, AccelY: 3614, AccelZ: 743 <-------- not calibrated acceleration values
MGaussX: -3.72454545455, MGaussY: 0.000909090909091, MGaussZ: -3.72454545455 <-------- Gauss calculated on basis of calibrated magnetic values
MuTX: -372.454545455, MuTY: 0.0909090909091, MuTZ: -372.454545455 <-------- Tesla calculated on basis of calibrated magnetic values
AgX: 0.154, AgY: -0.482, AgZ: 0.695 <-------- G-Force calculated on basis of not calibrated acceleration values
MHeading: 179.986015187 <-------- Heading calculated on basis of calibrated magnetic values
MWindDir: S <-------- Wind direction calculated on basis of calibrated magnetic values
MTemp(F): 28.0, MTemp©: 82.4
GyroX: 1, GyroY: 66, GyroZ: 66 <-------- calibrated gyrometer values
GyroDPSX: 0.00875, GyroDPSY: 0.5775, GyroDPSZ: 0.5775 <-------- degrees per second calculated on basis of calibrated gyro values
MagnetX: -60, MagnetY: -3700, MagnetZ: 241
AccelX: 143, AccelY: 3457, AccelZ: 675
MGaussX: -3.77818181818, MGaussY: -3.36363636364, MGaussZ: 0.219090909091
MuTX: -377.818181818, MuTY: -336.363636364, MuTZ: 21.9090909091
AgX: 0.143, AgY: -0.639, AgZ: 0.675
MHeading: 221.678005039
MWindDir: SW
MTemp(F): 28.0, MTemp©: 82.4
GyroX: 1, GyroY: 66, GyroZ: 66
GyroDPSX: 0.00875, GyroDPSY: 0.5775, GyroDPSZ: 0.5775

Thank you for your effort in helping me out!

kevin · August 19, 2014, 10:18pm

I just had a chance to take a look at your code and output again, and while I haven’t gone carefully through all of it, you definitely seem to be having problems with interpreting signed values as unsigned. This is apparent when you see values like 65460 in your output: the range of a 16-bit signed number is -32768 to 32767, while the range of an unsigned 16-bit number is 0 to 65535. The binary value in that case is 1111 1111 1011 0100, which is actually a decimal value of -76 when correctly interpreted as a 16-bit signed number in two’s complement.

You’re already calculating the two’s complement with your twos_comp() function, but you should use it in readMagnetics() immediately when you combine the bytes instead of in readMagneticGauss() (and likewise for the other sensors) . Otherwise, you use the (incorrect) unsigned interpretations of the raw values in your calibration calculations, which leads to strange results later on.

A possible related issue is that, in calibrateMag(), you initialize x_max_m, etc. with a value of 0. To be safe, you should initialize them with a more negative value (like -999999, though +32767 for min and -32768 for max should be sufficient) in the unlikely event that the magnetometer axis offset is extreme enough to make the readings always negative values.

- Kevin

codac · August 26, 2014, 6:58pm

Hi kevin,

looks much better now.

Here are my results for the calibration:

and here are the measured values:

MagnetX: 188, MagnetY: -2, MagnetZ: 10 <-------- calibrated magnetic values
AccelX: -493, AccelY: 233, AccelZ: 816 <-------- not calibrated acceleration values
MGaussX: 0.170909090909, MGaussY: -0.00181818181818, MGaussZ: 0.00909090909091 <-------- Gauss calculated on basis of calibrated magnetic values
MuTX: 17.0909090909, MuTY: -0.181818181818, MuTZ: 0.909090909091 <-------- Tesla calculated on basis of calibrated magnetic values
AgX: -0.493, AgY: 0.233, AgZ: 0.816 <-------- G-Force calculated on basis of not calibrated acceleration values
MHeading: 359.390493423 <-------- Heading calculated on basis of calibrated magnetic values
MWindDir: N <-------- Wind direction calculated on basis of calibrated magnetic values
MTemp(F): 24.25, MTemp©: 75.65
GyroX: 1, GyroY: 1, GyroZ: 0 <-------- calibrated gyrometer values
GyroDPSX: 0.00875, GyroDPSY: 0.00875, GyroDPSZ: 0.0 <-------- degrees per second calculated on basis of calibrated gyro values
GyroRadSX: 0.00015271631375, GyroRadSY: 0.00015271631375, GyroRadSZ: 0.0
MagnetX: 188, MagnetY: -2, MagnetZ: 10
AccelX: -494, AccelY: 231, AccelZ: 816
MGaussX: 0.170909090909, MGaussY: -0.00181818181818, MGaussZ: 0.00909090909091
MuTX: 17.0909090909, MuTY: -0.181818181818, MuTZ: 0.909090909091
AgX: -0.494, AgY: 0.231, AgZ: 0.816
MHeading: 359.390493423
MWindDir: N
MTemp(F): 24.25, MTemp©: 75.65
GyroX: 1, GyroY: 1, GyroZ: 0
GyroDPSX: 0.00875, GyroDPSY: 0.00875, GyroDPSZ: 0.0
GyroRadSX: 0.00015271631375, GyroRadSY: 0.00015271631375, GyroRadSZ: 0.0

I’ve updated the code.

Any more hints or wrong values?