Created
April 29, 2018 20:29
-
-
Save ViennaMike/d8b8f9636694c7edf4f115b28c9378c0 to your computer and use it in GitHub Desktop.
Calibration Program for LMS303 3-axis accelerometer and magnetometer, Written in python 2.7. Uses adafruit_lsm303.mpy module to interface with the sensot.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# 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!') |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
[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