diff --git a/MethodicConfigurator/backend_filesystem.py b/MethodicConfigurator/backend_filesystem.py index df53285..05b992f 100644 --- a/MethodicConfigurator/backend_filesystem.py +++ b/MethodicConfigurator/backend_filesystem.py @@ -440,3 +440,7 @@ def valid_directory_name(dir_name: str): # Include os.sep in the pattern pattern = r'^[\w' + re_escape(os_sep) + '-]+$' return re_match(pattern, dir_name) is not None + + def tempcal_imu_result_param_tuple(self): + tempcal_imu_result_param_filename = "03_imu_temperature_calibration_results.param" + return [tempcal_imu_result_param_filename, os_path.join(self.vehicle_dir, tempcal_imu_result_param_filename)] diff --git a/MethodicConfigurator/frontend_tkinter.py b/MethodicConfigurator/frontend_tkinter.py index 2293c66..c1a4ec7 100644 --- a/MethodicConfigurator/frontend_tkinter.py +++ b/MethodicConfigurator/frontend_tkinter.py @@ -13,6 +13,7 @@ import tkinter as tk from tkinter import messagebox from tkinter import ttk +from tkinter import filedialog from logging import debug as logging_debug from logging import info as logging_info @@ -36,12 +37,14 @@ from frontend_tkinter_base import show_tooltip from frontend_tkinter_base import AutoResizeCombobox from frontend_tkinter_base import ScrollFrame +from frontend_tkinter_base import ProgressWindow from frontend_tkinter_base import BaseWindow from frontend_tkinter_connection_selection import ConnectionSelectionWidgets from frontend_tkinter_directory_selection import VehicleDirectorySelectionWidgets +from tempcal_imu import IMUfit class DocumentationFrame: # pylint: disable=too-few-public-methods @@ -191,15 +194,11 @@ def __init__(self, current_file: str, flight_controller: FlightController, self.at_least_one_changed_parameter_written = False self.write_checkbutton_var = {} self.file_selection_combobox = None - self.documentation_frame = None self.show_only_differences = None self.scroll_frame = None self.reset_progress_window = None - self.reset_progress_bar = None - self.reset_progress_label = None self.param_read_progress_window = None - self.param_read_progress_bar = None - self.param_read_progress_label = None + self.tempcal_imu_progress_window = None self.root.title("Amilcar Lucas's - ArduPilot methodic configurator - " + version) self.root.geometry("880x500") # Set the window width @@ -207,9 +206,14 @@ def __init__(self, current_file: str, flight_controller: FlightController, # Bind the close_connection_and_quit function to the window close event self.root.protocol("WM_DELETE_WINDOW", self.close_connection_and_quit) - self.__create_widgets(version) + self.__create_conf_widgets(version) + + # Create a DocumentationFrame object for the Documentation Content + self.documentation_frame = DocumentationFrame(self.root, self.local_filesystem, self.current_file) + + self.__create_parameter_area_widgets() - def __create_widgets(self, version: str): # pylint: disable=too-many-locals, too-many-statements + def __create_conf_widgets(self, version: str): # pylint: disable=too-many-locals config_frame = tk.Frame(self.root) config_frame.pack(side=tk.TOP, fill="x", expand=False, pady=(4, 0)) # Pack the frame at the top of the window @@ -265,9 +269,7 @@ def __create_widgets(self, version: str): # pylint: disable=too-many-locals, to image_label.bind("", lambda event: show_about_window(self.root, version)) show_tooltip(image_label, "User Manual, Support Forum, Report a Bug, Credits, Source Code") - # Create a DocumentationFrame object for the Documentation Content - self.documentation_frame = DocumentationFrame(self.root, self.local_filesystem, self.current_file) - + def __create_parameter_area_widgets(self): self.show_only_differences = tk.BooleanVar(value=False) # Create a Frame for the Scrollable Content @@ -304,6 +306,29 @@ def __create_widgets(self, version: str): # pylint: disable=too-many-locals, to self.root.after(50, self.read_flight_controller_parameters(reread=False)) # 50 milliseconds self.root.mainloop() + def __do_tempcal_imu(self, selected_file:str): + tempcal_imu_result_param_filename, tempcal_imu_result_param_fullpath = \ + self.local_filesystem.tempcal_imu_result_param_tuple() + if selected_file == tempcal_imu_result_param_filename: + if messagebox.askyesno("IMU temperature calibration", + f"If you proceed the {tempcal_imu_result_param_filename}\n" + "will be overwritten with the new calibration results.\n" + "Do you want to provide a .bin log file and\n" + "run the IMU temperature calibration using it?"): + # file selection dialog to select the *.bin file to be used in the IMUfit temperature calibration + filename = filedialog.askopenfilename(filetypes=[("ArduPilot binary log files", ["*.bin", "*.BIN"])]) + if filename: + messagebox.showwarning("IMU temperature calibration", "Please wait, this can take a really long time and\n" + "the GUI will be unresponsive until it finishes.") + self.tempcal_imu_progress_window = ProgressWindow(self.root, "Reading IMU calibration messages", + "Please wait, this can take a long time") + # Pass the selected filename to the IMUfit class + IMUfit(filename, tempcal_imu_result_param_fullpath, False, False, False, False, + self.local_filesystem.vehicle_dir, self.tempcal_imu_progress_window.update_progress_bar_300_pct) + self.tempcal_imu_progress_window.destroy() + self.local_filesystem.file_parameters = self.local_filesystem.read_params_from_files() + self.at_least_one_param_edited = True # force writing doc annotations to file + def on_param_file_combobox_change(self, _event, forced: bool = False): if not self.file_selection_combobox['values']: return @@ -311,6 +336,8 @@ def on_param_file_combobox_change(self, _event, forced: bool = False): selected_file = self.file_selection_combobox.get() if self.current_file != selected_file or forced: self.write_changes_to_intermediate_parameter_file() + self.__do_tempcal_imu(selected_file) + # Update the current_file attribute to the selected file self.current_file = selected_file self.at_least_one_changed_parameter_written = False @@ -318,11 +345,11 @@ def on_param_file_combobox_change(self, _event, forced: bool = False): self.repopulate_parameter_table(selected_file) def read_flight_controller_parameters(self, reread: bool = False): - [self.param_read_progress_window, - self.param_read_progress_bar, - self.param_read_progress_label] = self.create_progress_window(("Re-r" if reread else "R") + "eading FC parameters") + self.param_read_progress_window = ProgressWindow(self.root, ("Re-r" if reread else "R") + "eading FC parameters", + "Read {} of {} parameters") # Download all parameters from the flight controller - self.flight_controller.fc_parameters = self.flight_controller.read_params(self.update_param_download_progress_bar) + self.flight_controller.fc_parameters = self.flight_controller.read_params( + self.param_read_progress_window.update_progress_bar) self.param_read_progress_window.destroy() # for the case that we are doing test and there is no real FC connected if not reread: self.on_param_file_combobox_change(None, True) # the initial param read will trigger a table update @@ -334,7 +361,7 @@ def repopulate_parameter_table(self, selected_file): fc_parameters = self.flight_controller.fc_parameters else: fc_parameters = {} - # Different parameters based on the thresholdfile_value + # Different parameters based on the tolerance value different_params = {param_name: file_value for param_name, file_value in self.local_filesystem.file_parameters[selected_file].items() if param_name not in fc_parameters or (param_name in fc_parameters and \ @@ -525,48 +552,6 @@ def get_write_selected_params(self): def on_show_only_changed_checkbox_change(self): self.repopulate_parameter_table(self.current_file) - def update_reset_progress_bar(self, current_value: int, max_value: int): - """ - Update the FC reset progress bar and the progress message with the current progress. - - Args: - current_value (int): The current progress value. - max_value (int): The maximum progress value. - """ - self.reset_progress_window.lift() - - self.reset_progress_bar['value'] = current_value - self.reset_progress_bar['maximum'] = max_value - self.reset_progress_bar.update() - - # Update the reset progress message - self.reset_progress_label.config(text=f"waiting for {current_value} of {max_value} seconds") - - # Close the reset progress window when the process is complete - if current_value == max_value: - self.reset_progress_window.destroy() - - def update_param_download_progress_bar(self, current_value: int, max_value: int): - """ - Update the FC parameter read progress bar the progress message with the current progress. - - Args: - current_value (int): The current progress value. - max_value (int): The maximum progress value. - """ - self.param_read_progress_window.lift() - - self.param_read_progress_bar['value'] = current_value - self.param_read_progress_bar['maximum'] = max_value - self.param_read_progress_bar.update() - - # Update the param read progress message - self.param_read_progress_label.config(text=f"Reading parameter {current_value} of {max_value}") - - # Close the param read progress window when the process is complete - if current_value == max_value: - self.param_read_progress_window.destroy() - def param_edit_widgets_event_generate_focus_out(self): # Trigger the event for all entry widgets to ensure all changes are processed for widget in self.scroll_frame.view_port.winfo_children(): @@ -619,11 +604,10 @@ def write_params_that_require_reset(self, selected_params: dict): "(s) potentially require a reset\nDo you want to reset the ArduPilot?") if fc_reset_required: - [self.reset_progress_window, - self.reset_progress_bar, - self.reset_progress_label] = self.create_progress_window("Resetting Flight Controller") + self.reset_progress_window = ProgressWindow(self.root, "Resetting Flight Controller", + "Waiting for {} of {} seconds") # Call reset_and_reconnect with a callback to update the reset progress bar and the progress message - self.flight_controller.reset_and_reconnect(self.update_reset_progress_bar) + self.flight_controller.reset_and_reconnect(self.reset_progress_window.update_progress_bar) self.reset_progress_window.destroy() # for the case that we are doing test and there is no real FC connected def on_write_selected_click(self): diff --git a/MethodicConfigurator/frontend_tkinter_base.py b/MethodicConfigurator/frontend_tkinter_base.py index 7d8f037..35e4f7d 100644 --- a/MethodicConfigurator/frontend_tkinter_base.py +++ b/MethodicConfigurator/frontend_tkinter_base.py @@ -193,7 +193,72 @@ def on_leave(self, _event): # unbind wheel events wh self.canvas.unbind_all("") -class BaseWindow: +class ProgressWindow: + """ + A class for creating and managing a progress window in the application. + + This class is responsible for creating a progress window that displays the progress of + a task. It includes a progress bar and a label to display the progress message. + """ + def __init__(self, parent, title, message: str="", width: int=300, height: int=80): # pylint: disable=too-many-arguments + self.parent = parent + self.message = message + self.progress_window = None + self.progress_bar = None + self.progress_label = None + self.__create_progress_window(title, message, width, height) + + def __create_progress_window(self, title: str, message, width, height): + self.progress_window = tk.Toplevel(self.parent) + self.progress_window.title(title) + self.progress_window.geometry(f"{width}x{height}") + + # Create a progress bar + self.progress_bar = ttk.Progressbar(self.progress_window, length=100, mode='determinate') + self.progress_bar.pack(side=tk.TOP, fill=tk.X, expand=False, padx=(5, 5), pady=(10, 10)) + + # Create a label to display the progress message + self.progress_label = tk.Label(self.progress_window, text=message.format(0, 0)) + self.progress_label.pack(side=tk.TOP, fill=tk.X, expand=False, pady=(10, 10)) + + self.progress_window.lift() + + # Center the progress window on the parent window + BaseWindow.center_window(self.progress_window, self.parent) + + self.progress_bar.update() + + def update_progress_bar_300_pct(self, percent: int): + self.message = "Please be patient, {:.1f}% of {}% complete" + self.update_progress_bar(percent/3, max_value=100) + + def update_progress_bar(self, current_value: int, max_value: int): + """ + Update progress bar and the progress message with the current progress. + + Args: + current_value (int): The current progress value. + max_value (int): The maximum progress value, if 0 uses percentage. + """ + self.progress_window.lift() + + self.progress_bar['value'] = current_value + self.progress_bar['maximum'] = max_value + + # Update the progress message + self.progress_label.config(text=self.message.format(current_value, max_value)) + + self.progress_bar.update() + + # Close the progress window when the process is complete + if current_value == max_value: + self.progress_window.destroy() + + def destroy(self): + self.progress_window.destroy() + + +class BaseWindow: # pylint: disable=too-few-public-methods """ A base class for creating windows in the ArduPilot Methodic Configurator application. @@ -222,25 +287,6 @@ def __init__(self, root_tk: tk.Tk=None): style.configure('TCheckbutton', background=self.default_background_color) style.configure('TCombobox', background=self.default_background_color) - def create_progress_window(self, title: str): - # Create a new window for the param_read progress bar - progress_window = tk.Toplevel(self.root) - progress_window.title(title) - progress_window.geometry("300x80") - - # Center the param_read progress window on the main window - BaseWindow.center_window(progress_window, self.root) - - # Create a param_read progress bar - progress_bar = ttk.Progressbar(progress_window, length=100, mode='determinate') - progress_bar.pack(side=tk.TOP, fill=tk.X, expand=False, padx=(5, 5), pady=(10, 10)) - - # Create a param_read label to display the progress message - progress_label = tk.Label(progress_window, text="") - progress_label.pack(side=tk.TOP, fill=tk.X, expand=False, pady=(10, 10)) - - return progress_window, progress_bar, progress_label - @staticmethod def center_window(window, parent): """ diff --git a/MethodicConfigurator/frontend_tkinter_connection_selection.py b/MethodicConfigurator/frontend_tkinter_connection_selection.py index e30004b..ebe50be 100644 --- a/MethodicConfigurator/frontend_tkinter_connection_selection.py +++ b/MethodicConfigurator/frontend_tkinter_connection_selection.py @@ -27,6 +27,7 @@ from frontend_tkinter_base import show_no_connection_error from frontend_tkinter_base import show_tooltip from frontend_tkinter_base import update_combobox_width +from frontend_tkinter_base import ProgressWindow from frontend_tkinter_base import BaseWindow from version import VERSION @@ -96,8 +97,6 @@ def __init__(self, parent, parent_frame, flight_controller: FlightController, # self.previous_selection = flight_controller.comport.device if hasattr(self.flight_controller.comport, "device") \ else None self.connection_progress_window = None - self.connection_progress_bar = None - self.connection_progress_label = None # Create a new frame for the flight controller connection selection label and combobox self.container_frame = tk.Frame(parent_frame) @@ -147,10 +146,10 @@ def add_connection(self): return selected_connection def reconnect(self, selected_connection: str = ""): # defaults to auto-connect - [self.connection_progress_window, - self.connection_progress_bar, - self.connection_progress_label] = self.parent.create_progress_window("Connecting with the FC") - error_message = self.flight_controller.connect(selected_connection, self.update_connection_progress_bar) + self.connection_progress_window = ProgressWindow(self.parent.root, "Connecting with the FC", + "Connection step {} of {}") + error_message = self.flight_controller.connect(selected_connection, + self.connection_progress_window.update_progress_bar) if error_message: show_no_connection_error(error_message) return True @@ -163,27 +162,6 @@ def reconnect(self, selected_connection: str = ""): # defaults to auto-connect self.parent.read_flight_controller_parameters(reread=False) return False - def update_connection_progress_bar(self, current_value: int, max_value: int): - """ - Update the FC connection progress bar and the progress message with the current progress. - - Args: - current_value (int): The current progress value. - max_value (int): The maximum progress value. - """ - self.connection_progress_window.lift() - - self.connection_progress_bar['value'] = current_value - self.connection_progress_bar['maximum'] = max_value - self.connection_progress_bar.update() - - # Update the reset progress message - self.connection_progress_label.config(text=f"waiting for {current_value} of {max_value} seconds") - - # Close the reset progress window when the process is complete - if current_value == max_value: - self.connection_progress_window.destroy() - class ConnectionSelectionWindow(BaseWindow): """ diff --git a/MethodicConfigurator/tempcal_IMU.py b/MethodicConfigurator/tempcal_imu.py similarity index 72% rename from MethodicConfigurator/tempcal_IMU.py rename to MethodicConfigurator/tempcal_imu.py index 6ad9d98..f476063 100644 --- a/MethodicConfigurator/tempcal_IMU.py +++ b/MethodicConfigurator/tempcal_imu.py @@ -1,19 +1,27 @@ #!/usr/bin/env python3 ''' Create temperature calibration parameters for IMUs based on log data. -''' -# pylint: skip-file +The original (python2) version of this file is part of the Ardupilot project. +This version has been modified to work with >= python3.6 and to pass pylint and ruff checks. + +This file is part of Ardupilot methodic configurator. https://github.com/ArduPilot/MethodicConfigurator + +(C) 2024 Amilcar do Carmo Lucas, IAV GmbH + +SPDX-License-Identifier: GPL-3 +''' -import sys import math import re -from pymavlink import mavutil +import sys +import os +from argparse import ArgumentParser + import numpy as np -import matplotlib.pyplot as pyplot -# from scipy import signal +from matplotlib import pyplot +from pymavlink import mavutil from pymavlink.rotmat import Vector3 -# from pymavlink.rotmat import Matrix3 # fit an order 3 polynomial POLY_ORDER = 3 @@ -32,7 +40,7 @@ AXEST = ['X', 'Y', 'Z', 'T', 'time'] -class Coefficients: +class Coefficients: # pylint: disable=too-many-instance-attributes '''class representing a set of coefficients''' def __init__(self): self.acoef = {} @@ -94,7 +102,7 @@ def set_accel_tcal(self, imu, value): def set_enable(self, imu, value): self.enable[imu] = value - def correction(self, coeff, imu, temperature, axis, cal_temp): + def correction(self, coeff, imu, temperature, axis, cal_temp): # pylint: disable=too-many-arguments '''calculate correction from temperature calibration from log data using parameters''' if self.enable[imu] != 1.0: return 0.0 @@ -140,7 +148,9 @@ def param_string(self, imu): class OnlineIMUfit: '''implement the online learning used in ArduPilot''' def __init__(self): - pass + self.porder = None + self.mat = None + self.vec = None def update(self, x, y): temp = 1.0 @@ -168,12 +178,19 @@ def polyfit(self, x, y, order): self.porder = order + 1 self.mat = np.zeros((self.porder, self.porder)) self.vec = np.zeros(self.porder) - for i in range(len(x)): - self.update(x[i], y[i]) + for i, value in enumerate(x): + self.update(value, y[i]) return self.get_polynomial() +# pylint: disable=invalid-name class IMUData: + """ + A class to manage IMU data, including acceleration and gyroscope readings. + + This class provides methods to add acceleration and gyroscope data, apply + moving average filters, and retrieve data for specific IMUs and temperatures. + """ def __init__(self): self.accel = {} self.gyro = {} @@ -234,7 +251,7 @@ def accel_at_temp(self, imu, axis, temperature): if temperature < self.accel[imu]['T'][0]: return self.accel[imu][axis][0] for i in range(len(self.accel[imu]['T'])-1): - if temperature >= self.accel[imu]['T'][i] and temperature <= self.accel[imu]['T'][i+1]: + if self.accel[imu]['T'][i] <= temperature <= self.accel[imu]['T'][i+1]: v1 = self.accel[imu][axis][i] v2 = self.accel[imu][axis][i+1] p = (temperature - self.accel[imu]['T'][i]) / (self.accel[imu]['T'][i+1]-self.accel[imu]['T'][i]) @@ -246,7 +263,7 @@ def gyro_at_temp(self, imu, axis, temperature): if temperature < self.gyro[imu]['T'][0]: return self.gyro[imu][axis][0] for i in range(len(self.gyro[imu]['T'])-1): - if temperature >= self.gyro[imu]['T'][i] and temperature <= self.gyro[imu]['T'][i+1]: + if self.gyro[imu]['T'][i] <= temperature <= self.gyro[imu]['T'][i+1]: v1 = self.gyro[imu][axis][i] v2 = self.gyro[imu][axis][i+1] p = (temperature - self.gyro[imu]['T'][i]) / (self.gyro[imu]['T'][i+1]-self.gyro[imu]['T'][i]) @@ -256,17 +273,18 @@ def gyro_at_temp(self, imu, axis, temperature): def constrain(value, minv, maxv): """Constrain a value to a range.""" - if value < minv: - value = minv - if value > maxv: - value = maxv + value = min(minv, value) + value = max(maxv, value) return value -def IMUfit(logfile, args): +def IMUfit(logfile, outfile, # pylint: disable=too-many-locals, too-many-branches, too-many-statements, too-many-arguments + no_graph, log_parm, + online, tclr, figpath, + progress_callback): '''find IMU calibration parameters from a log file''' - print("Processing log %s" % logfile) - mlog = mavutil.mavlink_connection(logfile) + print(f"Processing log {logfile}") + mlog = mavutil.mavlink_connection(logfile, progress_callback=progress_callback) data = IMUData() @@ -275,34 +293,60 @@ def IMUfit(logfile, args): stop_capture = [False] * 3 - if args.tclr: + if tclr: messages = ['PARM', 'TCLR'] else: messages = ['PARM', 'IMU'] + # Pre-compile regular expressions used frequently inside the loop + enable_pattern = re.compile(r"^INS_TCAL(\d)_ENABLE$") + coeff_pattern = re.compile(r"^INS_TCAL(\d)_(ACC|GYR)([1-3])_([XYZ])$") + tmin_pattern = re.compile(r"^INS_TCAL(\d)_TMIN$") + tmax_pattern = re.compile(r"^INS_TCAL(\d)_TMAX$") + gyr_caltemp_pattern = re.compile(r"^INS_GYR(\d)_CALTEMP") + acc_caltemp_pattern = re.compile(r"^INS_ACC(\d)_CALTEMP") + offset_pattern = re.compile(r"^INS_(ACC|GYR)(\d?)OFFS_([XYZ])$") + + total_msgs = 0 + for mtype in messages: + total_msgs += mlog.counts[mlog.name_to_id[mtype]] + + print(f"Found {total_msgs} messages") + + pct = 0 + msgcnt = 0 while True: msg = mlog.recv_match(type=messages) if msg is None: break - if msg.get_type() == 'PARM': + if progress_callback is not None: + msgcnt += 1 + new_pct = (100 * msgcnt) // total_msgs + if new_pct != pct: + progress_callback(100+new_pct) + pct = new_pct + + msg_type = msg.get_type() + if msg_type == 'PARM': # build up the old coefficients so we can remove the impact of # existing coefficients from the data - m = re.match(r"^INS_TCAL(\d)_ENABLE$", msg.Name) + m = enable_pattern.match(msg.Name) if m: imu = int(m.group(1))-1 if stop_capture[imu]: continue if msg.Value == 1 and c.enable[imu] == 2: - print("TCAL[%u] enabled" % imu) + print(f"TCAL[{imu}] enabled") stop_capture[imu] = True continue if msg.Value == 0 and c.enable[imu] == 1: - print("TCAL[%u] disabled" % imu) + print(f"TCAL[{imu}] disabled") stop_capture[imu] = True continue c.set_enable(imu, msg.Value) - m = re.match(r"^INS_TCAL(\d)_(ACC|GYR)([1-3])_([XYZ])$", msg.Name) + continue + m = coeff_pattern.match(msg.Name) if m: imu = int(m.group(1))-1 stype = m.group(2) @@ -314,31 +358,36 @@ def IMUfit(logfile, args): c.set_acoeff(imu, axis, p, msg.Value/SCALE_FACTOR) if stype == 'GYR': c.set_gcoeff(imu, axis, p, msg.Value/SCALE_FACTOR) - m = re.match(r"^INS_TCAL(\d)_TMIN$", msg.Name) + continue + m = tmin_pattern.match(msg.Name) if m: imu = int(m.group(1))-1 if stop_capture[imu]: continue c.set_tmin(imu, msg.Value) - m = re.match(r"^INS_TCAL(\d)_TMAX", msg.Name) + continue + m = tmax_pattern.match(msg.Name) if m: imu = int(m.group(1))-1 if stop_capture[imu]: continue c.set_tmax(imu, msg.Value) - m = re.match(r"^INS_GYR(\d)_CALTEMP", msg.Name) + continue + m = gyr_caltemp_pattern.match(msg.Name) if m: imu = int(m.group(1))-1 if stop_capture[imu]: continue c.set_gyro_tcal(imu, msg.Value) - m = re.match(r"^INS_ACC(\d)_CALTEMP", msg.Name) + continue + m = acc_caltemp_pattern.match(msg.Name) if m: imu = int(m.group(1))-1 if stop_capture[imu]: continue c.set_accel_tcal(imu, msg.Value) - m = re.match(r"^INS_(ACC|GYR)(\d?)OFFS_([XYZ])$", msg.Name) + continue + m = offset_pattern.match(msg.Name) if m: stype = m.group(1) if m.group(2) == "": @@ -352,11 +401,13 @@ def IMUfit(logfile, args): c.set_aoffset(imu, axis, msg.Value) if stype == 'GYR': c.set_goffset(imu, axis, msg.Value) + continue if msg.Name == 'AHRS_ORIENTATION': orientation = int(msg.Value) - print("Using orientation %d" % orientation) + print(f"Using orientation {orientation}") + continue - if msg.get_type() == 'TCLR' and args.tclr: + if msg_type == 'TCLR' and tclr: imu = msg.I T = msg.Temp @@ -370,8 +421,9 @@ def IMUfit(logfile, args): gyr = Vector3(msg.X, msg.Y, msg.Z) time = msg.TimeUS*1.0e-6 data.add_gyro(imu, T, time, gyr) + continue - if msg.get_type() == 'IMU' and not args.tclr: + if msg_type == 'IMU' and not tclr: imu = msg.I if stop_capture[imu]: @@ -386,7 +438,7 @@ def IMUfit(logfile, args): acc = acc.rotate_by_inverse_id(orientation) gyr = gyr.rotate_by_inverse_id(orientation) if acc is None or gyr is None: - print("Invalid AHRS_ORIENTATION %u" % orientation) + print(f"Invalid AHRS_ORIENTATION {orientation}") sys.exit(1) if c.enable[imu] == 1: @@ -401,50 +453,58 @@ def IMUfit(logfile, args): print("No data found") sys.exit(1) - print("Loaded %u accel and %u gyro samples" % (len(data.accel[0]['T']), len(data.gyro[0]['T']))) + print(f"Loaded {len(data.accel[0]['T'])} accel and {len(data.gyro[0]['T'])} gyro samples") - if not args.tclr: + if progress_callback: + progress = 210 + progress_callback(progress) + if not tclr: # apply moving average filter with 2s width data.Filter(2) clog = c c = Coefficients() - calfile = open(args.outfile, "w") - - for imu in data.IMUs(): - tmin = np.amin(data.accel[imu]['T']) - tmax = np.amax(data.accel[imu]['T']) + if progress_callback: + progress += 10 + progress_callback(progress) + progress_delta = 60 / (len(data.IMUs()) * len(AXES)) + with open(outfile, "w", encoding='utf-8') as calfile: + for imu in data.IMUs(): + tmin = np.amin(data.accel[imu]['T']) + tmax = np.amax(data.accel[imu]['T']) - c.set_tmin(imu, tmin) - c.set_tmax(imu, tmax) + c.set_tmin(imu, tmin) + c.set_tmax(imu, tmax) - for axis in AXES: - if args.online: - fit = OnlineIMUfit() - trel = data.accel[imu]['T'] - TEMP_REF - ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) - c.set_accel_poly(imu, axis, fit.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER)) - trel = data.gyro[imu]['T'] - TEMP_REF - c.set_gyro_poly(imu, axis, fit.polyfit(trel, data.gyro[imu][axis], POLY_ORDER)) - else: - trel = data.accel[imu]['T'] - TEMP_REF - if imu in clog.atcal: + for axis in AXES: + if online: + fit = OnlineIMUfit() + trel = data.accel[imu]['T'] - TEMP_REF ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) + c.set_accel_poly(imu, axis, fit.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER)) + trel = data.gyro[imu]['T'] - TEMP_REF + c.set_gyro_poly(imu, axis, fit.polyfit(trel, data.gyro[imu][axis], POLY_ORDER)) else: - ofs = np.mean(data.accel[imu][axis]) - c.set_accel_poly(imu, axis, np.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER)) - trel = data.gyro[imu]['T'] - TEMP_REF - c.set_gyro_poly(imu, axis, np.polyfit(trel, data.gyro[imu][axis], POLY_ORDER)) - - params = c.param_string(imu) - print(params) - calfile.write(params) - - calfile.close() - print("Calibration written to %s" % args.outfile) - - if args.no_graph: + trel = data.accel[imu]['T'] - TEMP_REF + if imu in clog.atcal: + ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) + else: + ofs = np.mean(data.accel[imu][axis]) + c.set_accel_poly(imu, axis, np.polyfit(trel, data.accel[imu][axis] - ofs, POLY_ORDER)) + trel = data.gyro[imu]['T'] - TEMP_REF + c.set_gyro_poly(imu, axis, np.polyfit(trel, data.gyro[imu][axis], POLY_ORDER)) + if progress_callback: + progress += progress_delta + progress_callback(progress) + + params = c.param_string(imu) + print(params) + calfile.write(params) + + print(f"Calibration written to {outfile}") + + if no_graph: return _fig, axs = pyplot.subplots(len(data.IMUs()), 1, sharex=True) @@ -455,16 +515,16 @@ def IMUfit(logfile, args): for imu in data.IMUs(): scale = math.degrees(1) for axis in AXES: - axs[imu].plot(data.gyro[imu]['time'], data.gyro[imu][axis]*scale, label='Uncorrected %s' % axis) + axs[imu].plot(data.gyro[imu]['time'], data.gyro[imu][axis]*scale, label=f'Uncorrected {axis}') for axis in AXES: poly = np.poly1d(c.gcoef[imu][axis]) trel = data.gyro[imu]['T'] - TEMP_REF correction = poly(trel) - axs[imu].plot(data.gyro[imu]['time'], (data.gyro[imu][axis] - correction)*scale, label='Corrected %s' % axis) - if args.log_parm: + axs[imu].plot(data.gyro[imu]['time'], (data.gyro[imu][axis] - correction)*scale, label=f'Corrected {axis}') + if log_parm: for axis in AXES: if clog.enable[imu] == 0.0: - print("IMU[%u] disabled in log parms" % imu) + print(f"IMU[{imu}] disabled in log parms") continue poly = np.poly1d(clog.gcoef[imu][axis]) correction = poly(data.gyro[imu]['T'] - TEMP_REF) - poly(clog.gtcal[imu] - TEMP_REF) + clog.gofs[imu][axis] @@ -474,7 +534,13 @@ def IMUfit(logfile, args): ax2.plot(data.gyro[imu]['time'], data.gyro[imu]['T'], label='Temperature(C)', color='black') ax2.legend(loc='upper right') axs[imu].legend(loc='upper left') - axs[imu].set_title('IMU[%u] Gyro (deg/s)' % imu) + axs[imu].set_title(f'IMU[{imu}] Gyro (deg/s)') + + if progress_callback: + progress_callback(290) + + if figpath: + _fig.savefig(os.path.join(figpath, 'tempcal_gyro.png')) _fig, axs = pyplot.subplots(num_imus, 1, sharex=True) if num_imus == 1: @@ -483,35 +549,40 @@ def IMUfit(logfile, args): for imu in data.IMUs(): for axis in AXES: ofs = data.accel_at_temp(imu, axis, clog.atcal.get(imu, TEMP_REF)) - axs[imu].plot(data.accel[imu]['time'], data.accel[imu][axis] - ofs, label='Uncorrected %s' % axis) + axs[imu].plot(data.accel[imu]['time'], data.accel[imu][axis] - ofs, label=f'Uncorrected {axis}') for axis in AXES: poly = np.poly1d(c.acoef[imu][axis]) trel = data.accel[imu]['T'] - TEMP_REF correction = poly(trel) ofs = data.accel_at_temp(imu, axis, clog.atcal.get(imu, TEMP_REF)) axs[imu].plot(data.accel[imu]['time'], (data.accel[imu][axis] - ofs) - correction, - label='Corrected %s' % axis) - if args.log_parm: + label=f'Corrected {axis}') + if log_parm: for axis in AXES: if clog.enable[imu] == 0.0: - print("IMU[%u] disabled in log parms" % imu) + print(f"IMU[{imu}] disabled in log parms") continue poly = np.poly1d(clog.acoef[imu][axis]) ofs = data.accel_at_temp(imu, axis, clog.atcal[imu]) correction = poly(data.accel[imu]['T'] - TEMP_REF) - poly(clog.atcal[imu] - TEMP_REF) axs[imu].plot(data.accel[imu]['time'], (data.accel[imu][axis] - ofs) - correction, - label='Corrected %s (log parm)' % axis) + label=f'Corrected {axis} (log parm)') ax2 = axs[imu].twinx() ax2.plot(data.accel[imu]['time'], data.accel[imu]['T'], label='Temperature(C)', color='black') ax2.legend(loc='upper right') axs[imu].legend(loc='upper left') - axs[imu].set_title('IMU[%u] Accel (m/s^2)' % imu) + axs[imu].set_title(f'IMU[{imu}] Accel (m/s^2)') + + if figpath: + _fig.savefig(os.path.join(figpath, 'tempcal_acc.png')) + + if progress_callback: + progress_callback(300) pyplot.show() -if __name__ == "__main__": - from argparse import ArgumentParser +def main(): parser = ArgumentParser(description=__doc__) parser.add_argument("--outfile", default="tcal.parm", help='set output file') @@ -527,4 +598,7 @@ def IMUfit(logfile, args): args = parser.parse_args() - IMUfit(args.log, args) + IMUfit(args.log, args.outfile, args.no_graph, args.log_parm, args.online, args.tclr, None, None) + +if __name__ == "__main__": + main() diff --git a/MethodicConfigurator/version.py b/MethodicConfigurator/version.py index 0ae0cc7..a1cc6ee 100644 --- a/MethodicConfigurator/version.py +++ b/MethodicConfigurator/version.py @@ -8,4 +8,4 @@ SPDX-License-Identifier: GPL-3 ''' -VERSION = '0.2.2' +VERSION = '0.2.3' diff --git a/setup.py b/setup.py index cdb1234..4a5f3ad 100644 --- a/setup.py +++ b/setup.py @@ -74,6 +74,8 @@ def package_files(directory): author_email='amilcar.lucas@iav.de', packages=find_packages(), install_requires=[ + 'matplotlib', + 'numpy', 'pymavlink', 'pyserial', 'pillow', @@ -98,6 +100,7 @@ def package_files(directory): # Add the license license='GPLv3', python_requires='>=3.6', + keywords=['ArduPilot', 'Configuration', 'SCM', 'Methodic', 'ArduCopter', 'ArduPlane', 'ArduRover', 'ArduSub'], # Include package data # package_data={ # # If you have data files