diff --git a/src/calibration.py b/src/calibration.py index a501302..ac20bc8 100644 --- a/src/calibration.py +++ b/src/calibration.py @@ -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, diff --git a/src/socket_control.py b/src/socket_control.py index d0a02c4..fb045f9 100644 --- a/src/socket_control.py +++ b/src/socket_control.py @@ -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). """ diff --git a/src/user_interface.py b/src/user_interface.py index 689f750..56db0b8 100644 --- a/src/user_interface.py +++ b/src/user_interface.py @@ -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: