eive/Helmholtz_Test_Bench
16
0
Fork 0
forked from zietzm/Helmholtz_Test_Bench
Code Issues Pull Requests Projects Releases 1 Wiki Activity

Tested magnetometer calibration method.

Browse Source
This commit is contained in:
Robin Müller
2021-08-24 16:46:20 +02:00
parent bcadc3f273
commit 4bb1bad731
3 changed files with 37 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/calibration.py
+8 -6
View File
@@ -281,6 +281,7 @@ class MagnetometerCalibration(Thread):
reading = g.MAGNETOMETER.field - offsets
for i in range(3):
row = {'m': reading[i], 'b_x': applied_vec[0], 'b_y': applied_vec[1], 'b_z': applied_vec[2]}
# print("[Axis {}] {}".format(i, row))
samples[i].append(row)
# Set new progress indicator for UI
@@ -290,7 +291,7 @@ class MagnetometerCalibration(Thread):
self.cage_dev.idle()
# Use collected data to build and solve system of equations
sensor_parameters = self.solve_system(samples)
sensor_parameters = self.solve_system(samples, offsets)
# Pass results to UI
self.put_message('calibration_data', sensor_parameters)
@@ -299,20 +300,21 @@ class MagnetometerCalibration(Thread):
progress = int(offset_complete) * 0.2 + (test_vec_index / self.calibration_points) * 0.8
self.put_message('progress', progress)
def solve_system(self, samples):
def solve_system(self, samples, offset_data):
# Calculate magnitude of ambient field
b_e_x = g.CAGE_DEVICE.axes[0].ambient_field
b_e_y = g.CAGE_DEVICE.axes[0].ambient_field
b_e_z = g.CAGE_DEVICE.axes[0].ambient_field
b_e_y = g.CAGE_DEVICE.axes[1].ambient_field
b_e_z = g.CAGE_DEVICE.axes[2].ambient_field
b_e = sqrt(b_e_x**2 + b_e_y**2 + b_e_z**2)
# Perform least squares optimization on all magnetometer axes
sensor_parameters = []
for axis, axis_samples in enumerate(samples):
result = scipy.optimize.least_squares(self.residual_function, (0, 0, 0, 0), args=(b_e, axis_samples))
result = scipy.optimize.least_squares(self.residual_function, (1.0, pi/4, pi/4, pi/4), args=(b_e, axis_samples), gtol=1e-13)
s, alpha_e, alpha, beta = result.x
residual = result.cost
residual = np.max(np.abs(result.fun))
sensor_parameters.append({'sensitivity': s,
'offset': offset_data[axis],
'alpha_e': alpha_e,
'alpha': alpha,
'beta': beta,
src/socket_control.py
+6 -1
View File
@@ -98,8 +98,13 @@ class ClientConnectionThread(Thread):
msg = ''
else:
msg += char
except ConnectionResetError:
except ConnectionResetError as e:
ui_print("A connection was closed by the client.")
self.client_socket.close()
if self._cage_dev:
self._cage_dev.close()
g.MAGNETOMETER.connected = False
return
def handle_msg(self, message):
""" Executes command logic and returns string response (for client). """
src/user_interface.py
+23 -10
View File
@@ -825,6 +825,7 @@ class CalibrateMagnetometer(Frame):
self.calibration_interval_var = DoubleVar(value=5)
# Calibration results
self.sensitivity_result_vars = [StringVar(), StringVar(), StringVar()]
self.offset_result_vars = [StringVar(), StringVar(), StringVar()]
self.angle_to_plane_result_vars = [StringVar(), StringVar(), StringVar()]
self.angle_in_plane_result_vars = [StringVar(), StringVar(), StringVar()]
self.residual_result_vars = [StringVar(), StringVar(), StringVar()]
@@ -913,39 +914,50 @@ class CalibrateMagnetometer(Frame):
axis_data.grid(row=1, column=i + 1, padx=5, pady=5, sticky="nw")
sensitivity_results_unit = Label(calibration_results_frame, text="-")
sensitivity_results_unit.grid(row=1, column=4, padx=5, pady=5, sticky="nw")
# Axis offsets
offset_results_label = Label(calibration_results_frame, text="Offset:")
offset_results_label.grid(row=2, column=0, padx=5, pady=5, sticky="nw")
for i in range(3):
axis_data = Entry(calibration_results_frame,
textvariable=self.offset_result_vars[i],
width=15,
state='readonly')
axis_data.grid(row=2, column=i + 1, padx=5, pady=5, sticky="nw")
offset_results_unit = Label(calibration_results_frame, text="\u03BCT")
offset_results_unit.grid(row=2, column=4, padx=5, pady=5, sticky="nw")
# Angle to XY coil plane
angle_to_plane_label = Label(calibration_results_frame, text="Angle to XY plane:")
angle_to_plane_label.grid(row=2, column=0, padx=5, pady=5, sticky="nw")
angle_to_plane_label.grid(row=3, column=0, padx=5, pady=5, sticky="nw")
for i in range(3):
axis_data = Entry(calibration_results_frame,
textvariable=self.angle_to_plane_result_vars[i],
width=15,
state='readonly')
axis_data.grid(row=2, column=i + 1, padx=5, pady=5, sticky="nw")
axis_data.grid(row=3, column=i + 1, padx=5, pady=5, sticky="nw")
angle_to_plane_unit = Label(calibration_results_frame, text="°")
angle_to_plane_unit.grid(row=2, column=4, padx=5, pady=5, sticky="nw")
angle_to_plane_unit.grid(row=3, column=4, padx=5, pady=5, sticky="nw")
# Angle in XY coil plane
angle_in_plane_label = Label(calibration_results_frame, text="Angle in XY plane:")
angle_in_plane_label.grid(row=3, column=0, padx=5, pady=5, sticky="nw")
angle_in_plane_label.grid(row=4, column=0, padx=5, pady=5, sticky="nw")
for i in range(3):
axis_data = Entry(calibration_results_frame,
textvariable=self.angle_in_plane_result_vars[i],
width=15,
state='readonly')
axis_data.grid(row=3, column=i + 1, padx=5, pady=5, sticky="nw")
axis_data.grid(row=4, column=i + 1, padx=5, pady=5, sticky="nw")
angle_in_plane_unit = Label(calibration_results_frame, text="°")
angle_in_plane_unit.grid(row=3, column=4, padx=5, pady=5, sticky="nw")
angle_in_plane_unit.grid(row=4, column=4, padx=5, pady=5, sticky="nw")
# Residual in system of equations
residual_label = Label(calibration_results_frame, text="Residual:")
residual_label.grid(row=4, column=0, padx=5, pady=5, sticky="nw")
residual_label.grid(row=5, column=0, padx=5, pady=5, sticky="nw")
for i in range(3):
axis_data = Entry(calibration_results_frame,
textvariable=self.residual_result_vars[i],
width=15,
state='readonly')
axis_data.grid(row=4, column=i + 1, padx=5, pady=5, sticky="nw")
axis_data.grid(row=5, column=i + 1, padx=5, pady=5, sticky="nw")
residual_unit = Label(calibration_results_frame, text="\u03BCT")
residual_unit.grid(row=4, column=4, padx=5, pady=5, sticky="nw")
residual_unit.grid(row=5, column=4, padx=5, pady=5, sticky="nw")
row_counter += 1
# This starts an endless polling loop
@@ -1002,9 +1014,10 @@ class CalibrateMagnetometer(Frame):
def display_calibration_results(self, results):
for i in range(3):
self.sensitivity_result_vars[i].set("{:.3f}".format(results[i]['sensitivity']))
self.offset_result_vars[i].set("{:.3f}".format(results[i]['offset'] * 1e6))
self.angle_to_plane_result_vars[i].set("{:.3f}".format(results[i]['alpha'] * 180/pi))
self.angle_in_plane_result_vars[i].set("{:.3f}".format(results[i]['beta'] * 180/pi))
self.residual_result_vars[i].set("{:.3f}".format(results[i]['residual'] * 1e6))
self.residual_result_vars[i].set("{:.3e}".format(results[i]['residual'] * 1e6))
def start_calibration_procedure(self):
try: