Instantly share code, notes, and snippets.

Embed
What would you like to do?
Calibration Program for LMS303 3-axis accelerometer and magnetometer, Written in python 2.7. Uses adafruit_lsm303.mpy module to interface with the sensot.
# Simple calibration program using the LSM303 accelerometer & magnetometer library.
# Will print the max accelerometer & magnetometer X, Y, Z axis values every
# 1/10th of a second.
# Author: Mike McGurrin, combining code from Tony DiCola and Pololu,
# with additional concepts from https://appelsiini.net/2018/calibrate-magnetometer/
# License: Follows Pololu license (other code was public domain):
# Portions Copyright (c) 2013 Pololu Corporation. For more information, see
#http://www.pololu.com/
#http://forum.pololu.com/
#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.
import time
import ConfigParser
# Import the LSM303 module.
import Adafruit_LSM303
# Create a LSM303 instance.
lsm303 = Adafruit_LSM303.LSM303()
# Alternatively you can specify the I2C bus with a bus parameter:
#lsm303 = Adafruit_LSM303.LSM303(busum=2)
running_mmin = (32767, 32767, 32767)
running_mmax = (-32768, -32768, -32768)
running_amin = (32767, 32767, 32767)
running_amax = (-32768, -32768, -32768)
measuring_duration = 15.0 # units are seconds
print('Calibrating magnetometer X, Y, Z axis values, move slowly in figure 8 and rotate...')
start_time = time.time()
while (time.time() < start_time + measuring_duration):
# Read the X, Y, Z axis magnetometer and acceleration values
accel, mag = lsm303.read()
# Grab the X, Y, Z components from the reading for printing.
mag_x, mag_z, mag_y = mag
# set lowest and highest values seen so far
running_mmin = tuple(map(lambda x, y: min(x,y), running_mmin, mag))
running_mmax = tuple(map(lambda x, y: max(x,y), running_mmax, mag))
print('Mag X={0}, Mag Y={1}, Mag Z={2}'.format(
mag_x, mag_y, mag_z))
print('mag minimums: ',running_mmin)
print('mag maximums: ',running_mmax)
# Wait 1/10th of a second and repeat.
time.sleep(0.1)
print('Calibrating accelerometer X, Y, Z axis values, slowly position all six faces of board facing up...')
raw_input('hit any key when ready')
start_time = time.time()
while (time.time() < start_time + measuring_duration):
# Read the X, Y, Z axis magnetometer and acceleration values
accel, mag = lsm303.read()
# Grab the X, Y, Z components for the accelerometer
accel_x, accel_y, accel_z = accel
running_amin = tuple(map(lambda x, y: min(x,y), running_amin, accel))
running_amax = tuple(map(lambda x, y: max(x,y), running_amax, accel))
print('Accel X={0}, Accel Y={1}, Accel Z={2}'.format(
accel_x, accel_y, accel_z))
print('accel minimums: ',running_amin)
print('accel maximums: ',running_amax)
# Wait 1/10th of a second and repeat.
time.sleep(0.1)
# Compute Magnetometer and Accelerometer Corrections
# Corrects for "soft iron" errors (bias) and approximate correction for "hard iron" through scaling along 3 axes
print('computing, printing, and saving calibration factors')
# Magnetometer Corrections
moffset = tuple(map(lambda x1, x2: (x1+x2) / 2., running_mmin, running_mmax))
avg_mdelta = tuple(map(lambda x1, x2: (x2-x1)/2., running_mmin, running_mmax))
combined_avg_mdelta = (avg_mdelta[0] + avg_mdelta[1] + avg_mdelta[2])/3.
scale_mx = combined_avg_mdelta / avg_mdelta[0]
scale_my = combined_avg_mdelta / avg_mdelta[1]
scale_mz = combined_avg_mdelta / avg_mdelta[2]
print('magnetometer offsets: ',moffset)
print('magentometer scaling: ',scale_mx, scale_my, scale_mz)
# Accelerometer Corrections
aoffset = tuple(map(lambda x1, x2: (x1+x2) / 2., running_amin, running_amax))
avg_adelta = tuple(map(lambda x1, x2: (x2-x1)/2., running_amin, running_amax))
combined_avg_adelta = (avg_adelta[0] + avg_adelta[1] + avg_adelta[2])/3.
scale_ax = combined_avg_adelta / avg_adelta[0]
scale_ay = combined_avg_adelta / avg_adelta[1]
scale_az = combined_avg_adelta / avg_adelta[2]
print('accel offsets: ',aoffset)
print('accel scaling: ',scale_ax, scale_ay, scale_az)
# Write values to .ini file
config = ConfigParser.ConfigParser()
config.add_section('Calibration')
config.set('Calibration', 'moffsetx', str(moffset[0]))
config.set('Calibration', 'moffsety', str(moffset[1]))
config.set('Calibration', 'moffsetz', str(moffset[2]))
config.set('Calibration', 'scale_mx', str(scale_mx))
config.set('Calibration', 'scale_my', str(scale_my))
config.set('Calibration', 'scale_mz', str(scale_mz))
config.set('Calibration', 'aoffsetx', str(aoffset[0]))
config.set('Calibration', 'aoffsety', str(aoffset[1]))
config.set('Calibration', 'aoffsetz', str(aoffset[2]))
config.set('Calibration', 'scale_ax', str(scale_ax))
config.set('Calibration', 'scale_ay', str(scale_ay))
config.set('Calibration', 'scale_az', str(scale_az))
with open('lms_calibrate.ini', 'w') as configfile:
config.write(configfile)
# To use offset in program
#corrected_mx = (sensor_mx - moffsetx) * scale_mx
#corrected_my = (sensor_my - moffsety) * scale_my
#corrected_mz = (sensor_mz - moffsetz) * scale_mz
#corrected_ax = (sensor_ax - aoffsetx) * scale_a
#corrected_ay = (sensor_ay - aoffsety) * scale_ay
#corrected_az = (sensor_az - aoffsetz) * scale_az
print('Finished!')
[Calibration]
moffsetx = 121.0
moffsety = -94.5
moffsetz = -75.0
scale_mx = 1.0753058104
scale_my = 0.992239858907
scale_mz = 0.941432396252
aoffsetx = 51.0
aoffsety = 34.5
aoffsetz = -21.5
scale_ax = 1.0003003003
scale_ay = 1.06099697404
scale_az = 0.945366822761
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment