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