Demo entry 6636997

calibration of magnetometer

   

Submitted by anonymous on Aug 30, 2017 at 05:56
Language: Python 3. Code size: 9.0 kB.

### This program is only for calibration ###

import socket,traceback
import csv
from StringIO import StringIO
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.gridspec as sp
import numpy as np
# Set up UDP connection
host  = '192.168.191.3'                                                        
port = 5555
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind((host,port))

sample_num = 5000                                                                
count = 0
flag = 0

raw_gyx_st = []
raw_gyy_st = []
raw_gyz_st = []

raw_accx_st = []
raw_accy_st = []
raw_accz_st = []

raw_magx_st = []
raw_magy_st = []
raw_magz_st = []

raw_magx_samst = []
raw_magy_samst = []
raw_magz_samst = []

mag_max = [0,0,0]
mag_min = [0,0,0]
mag_ave = [0,0,0]

mag_scale = [1,1,1]
mag_bias = [0,0,0]
####################################################
while 1:
        
        message, address = s.recvfrom(1024)
        f = StringIO(message)
        reader = csv.reader(f,delimiter=',')
        
        #for row in reader:
        for row in reader:
                if ((len(row) == 17) & (flag == 0)):
                        if(count <= sample_num):
                                print(count)

                                ### Raw Data ###
                                # Get raw data of gyroscope
                                gyx_raw = float(row[6])
                                gyy_raw = float(row[7])
                                gyz_raw = float(row[8])
                                
                                # Change the angle from radian to degree
                                c_gyx = gyx_raw*rtod
                                c_gyy = gyy_raw*rtod
                                c_gyz = gyz_raw*rtod
                     
                                # Get raw data of magnetometer
                                magx_raw = float(row[10])
                                magy_raw = float(row[11])
                                magz_raw = float(row[12])

                                mx = magx_raw
                                my = magy_raw
                                mz = magz_raw                              

                                # Get raw data of accelerator
                                acx = float(row[2])
                                acy = float(row[3])
                                acz = float(row[4])

                                # Get raw data of gravity
                                gx = float(row[14])
                                gy = float(row[15])
                                gz = float(row[16])

                                ## Package alignment
                                #Package alignment of gyroscope
                                cc_gyx = c_gyx
                                cc_gyy = -c_gyz
                                cc_gyz = c_gyy

                                #Package alignment of magnetometer 
                                c_mx = my
                                c_my = -mz
                                c_mz = mx

                                #Package alignment of gravity
                                cc_gx = -gy
                                cc_gy = gz
                                cc_gz = gx
                                
                                raw_gyx_st.append(cc_gyx)
                                raw_gyy_st.append(cc_gyy)
                                raw_gyz_st.append(cc_gyz)

                                raw_accx_st.append(cc_gx)
                                raw_accy_st.append(cc_gy)
                                raw_accz_st.append(cc_gz)

                                raw_magx_st.append(c_mx)
                                raw_magy_st.append(c_my)
                                raw_magz_st.append(c_mz)

                                if(sample_count < sample_num):
                                        
                                        raw_magx_samst.append(c_mx)
                                        raw_magy_samst.append(c_my)
                                        raw_magz_samst.append(c_mz)
                                        sample_count += 1

                                if(sample_count == sample_num):
                                        print('Sample collection done!!')
                                        mag_max[0] = max(raw_magx_samst)
                                        mag_max[1] = max(raw_magy_samst)
                                        mag_max[2] = max(raw_magz_samst)

                                        mag_min[0] = min(raw_magx_samst)
                                        mag_min[1] = min(raw_magy_samst)
                                        mag_min[2] = min(raw_magz_samst)

                                        mag_bias[0] = (mag_max[0]+mag_min[0])/2.0
                                        mag_bias[1] = (mag_max[1]+mag_min[1])/2.0
                                        mag_bias[2] = (mag_max[2]+mag_min[2])/2.0

                                        mag_scale[0] = (mag_max[0]-mag_min[0])/2.0
                                        mag_scale[1] = (mag_max[1]-mag_min[1])/2.0
                                        mag_scale[2] = (mag_max[2]-mag_min[2])/2.0

                                        mag_scale_ave = (mag_scale[0]+mag_scale[1]+mag_scale[2])/3.0

                                        mag_kx = mag_scale_ave/mag_scale[0]
                                        mag_ky = mag_scale_ave/mag_scale[1]
                                        mag_kz = mag_scale_ave/mag_scale[2]
                                        
                                        print('max for magxyz:')
                                        print(mag_max[0],mag_max[1],mag_max[2])
                                        print('min for magxyz:')
                                        print(mag_min[0],mag_min[1],mag_min[2])
                                        print('Calibration bias collected')                                     
                                        print('Start plotting scatter of uncalibrated data...')

                                        #2D plotting

                                        plt.title('Uncalibrated magnetic field')
                                        plt.xlim(-60,60)
                                        plt.ylim(-60,60)
                                        plt.xticks(range(-60,60,5),rotation = 45)
                                        plt.yticks(range(-60,60,5),rotation = 45)
                                        plt.grid(True)
                                        plt.scatter(raw_magx_samst,raw_magy_samst,c = 'royalblue',label = 'x-y plane',edgecolors = 'royalblue',s = si,alpha = 0.5)
                                        plt.scatter(raw_magy_samst,raw_magz_samst,c = 'green',marker = 's',label = 'y-z plane',edgecolors = 'green',s = si,alpha = 0.5)
                                        plt.scatter(raw_magz_samst,raw_magx_samst,c ='orange', marker = '^',label = 'z-x plane',edgecolors = 'orange',s = si+10,alpha = 0.5)
                                            
                                        plt.xlabel('strength of magnetic field')
                                        plt.ylabel('strength of magnetic field')
                                        plt.legend()
                                        
                                        # 3D plotting
                                        fig = plt.figure()
                                        ax1 = fig.add_subplot(111,projection = '3d')
                                        ax1.scatter(raw_magx_samst,raw_magy_samst,raw_magz_samst,c ='royalblue',edgecolors = 'royalblue',s= si, alpha = 0.7)

                                        ax1.set_xlabel('strength of magnetic field')
                                        ax1.set_ylabel('strength of magnetic field')
                                        ax1.set_title('Uncalibrated magnetic field (3D)')

                                        plt.show()
                                        sample_count += 1
                                        print('#################################')
                                        print('mag bias xyz are:')
                                        print(mag_bias[0],mag_bias[1],mag_bias[2])
                                        print('mag scale xyz are:')
                                        print(mag_kx,mag_ky,mag_kz)
                                        print('#################################')                               
                                                                    
                                        break
                                count += 1
                   
                        else:
                                flag = 1
                                

                

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).