Menu updated; Magnetic sensor offsets groundwork & shell script for easy program start

.pio/libdeps/uno/MPU9250/MPU9250.h

@@ -54,7 +54,7 @@ class MPU9250_ {
 
     QuaternionFilter qFilter;
 
-    float magnetic_declination = -7.51;  // Japan, 24th June
+    float magnetic_declination = 4.6;  // Köthen, 26th November
 
     bool b_verbose{false};
 

arduino/src/main.cpp

@@ -27,10 +27,13 @@ void setup() {
   mpu.setup(0x68); 
   delay(1000);
 
-  Serial.println(F("calibrate accel/gyro"));
+  Serial.println(F("Calibrate accel/gyro"));
   mpu.calibrateAccelGyro();
 
-  Serial.println(F("FIELDS:\tUS_0\tUS_1\tMAG_X\tMAG_Y\tMAG_Z\tACCEL_X\tACCEL_Y\tACCEL_Z\tGYRO_X\tGYRO_Y\tGYRO_Z\tTEMP\tEXEC_TIME"));
+  Serial.println(F("Calibrate Magnetometer"));
+  mpu.calibrateMag();
+  
+  Serial.println(F("FIELDS:\tUS_0\tUS_1\tMAG_X\tMAG_Y\tMAG_Z\tMAG_Off_X\tMAG_Off_Y\tMAG_Off_Z\tACCEL_X\tACCEL_Y\tACCEL_Z\tGYRO_X\tGYRO_Y\tGYRO_Z\tTEMP\tEXEC_TIME"));
 
 }
 
@@ -45,13 +48,17 @@ void loop() {
     //get mpu values
     mpu.update();
 
-    snprintf(outputBuffer, sizeof(outputBuffer), "DATA:\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\r\n",
+    snprintf(outputBuffer, sizeof(outputBuffer), "DATA:\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\r\n",
       //acoustic RTT 
       us_get_duration(0), us_get_duration(1),
       //magnetic field
       (long)(mpu.getMagX()*1000),
       (long)(mpu.getMagY()*1000),
       (long)(mpu.getMagZ()*1000),
+      // magnetic bias (offsets) ###
+      (long) (mpu.getMagBiasX()*1000),
+      (long) (mpu.getMagBiasY()*1000),
+      (long) (mpu.getMagBiasZ()*1000),
       //accelerometer
       (long)(mpu.getAccX()*1000),
       (long)(mpu.getAccY()*1000),

raspberry-pi/config.ini

@@ -37,10 +37,9 @@
   overhead_right = 20
 
 [mag_sensor]
-# amount of measurements used for the calibration of the Gyroscope and the Accelerometer
-measurements_gyro = 500
-measurements_accel = 1000
-
+# Offsets for the Magnetometer
+mag_offset_x = 0
+mag_offset_y = 0
 
 [opt_sensor]
   capture_device = -1

raspberry-pi/gui/mainWindow.py

@@ -74,22 +74,31 @@ class MainWindow(tk.Frame):
     self.quit_button = tk.Button(self.controls, text="Quit", command=self.root.destroy, height=2, foreground="red")
     self.quit_button.pack(side="bottom", fill="both")
 
-    self.calibrateac_button = tk.Button(self.controls, text="Calibrate AC", command=self.calibrate_ac, height=4)
+    self.calibrate_submenu_button = tk.Button(self.controls, text="Calibrate", command=self.calibrate_submenu, height=2)
+    self.calibrate_submenu_button.pack_forget()
+    
+    self.calibrate_all_button = tk.Button(self.controls, text="Calibrate All", command=self.calibrate_all, height=2)
+    self.calibrate_all_button.pack_forget()
+    
+    self.calibrateac_button = tk.Button(self.controls, text="Calibrate AC", command=self.calibrate_ac, height=2)
     self.calibrateac_button.pack_forget()
 
-    self.calibratemc_button = tk.Button(self.controls, text = "Calibrate MC", command = self.calibrate_mc,height = 4)
-    self.calibratemc_button.pack_forget()
+    self.calibrateopt_button = tk.Button(self.controls, text = "Calibrate OPT", command = self.calibrate_opt,height = 2)
+    self.calibrateopt_button.pack_forget()
+    
+    self.calibratemag_button = tk.Button(self.controls, text = "Calibrate MAG", command = self.calibrate_mag,height = 2)
+    self.calibratemag_button.pack_forget()
 
-    self.clear_button = tk.Button(self.controls, text="Clear graph", command=self.graph.clear, height=4)
+    self.clear_button = tk.Button(self.controls, text="Clear graph", command=self.graph.clear, height=2)
     self.clear_button.pack_forget()
 
-    self.logscreen_button = tk.Button(self.controls, text="Log", command=self.open_log, height=4)
+    self.logscreen_button = tk.Button(self.controls, text="Log", command=self.open_log, height=2)
     self.logscreen_button.pack_forget()
 
-    self.menu_button = tk.Button(self.controls, text="Menu", command=self.menu, height=4)
+    self.menu_button = tk.Button(self.controls, text="Menu", command=self.menu, height=2)
     self.menu_button.pack(side="bottom", fill="both")
     
-    self.menu_back_button = tk.Button(self.controls, text="Back", command=self.back, height=4)
+    self.menu_back_button = tk.Button(self.controls, text="Back", command=self.back, height=2)
     self.menu_back_button.pack_forget()
 
   def update(self):
@@ -183,10 +192,10 @@ class MainWindow(tk.Frame):
       else:
         self.mag_dro_val_sums.fill(0)
 
-      self.mag_dro_x.set("X: {:.1f} mm".format(self.mag_dro_val_sums[0]))
-      self.mag_dro_y.set("Y: {:.1f} mm".format(self.mag_dro_val_sums[1]))
-      #self.mag_dro_offset_x.set("offset: {:.1f} %".format(self.mag_dro_val_sums[2]*100))
-      #self.mag_dro_offset_y.set("size: {:.1f} %".format(self.mag_dro_val_sums[3]*100))
+      self.mag_dro_x.set("X: {:.1f} mT".format(self.mag_dro_val_sums[0]))
+      self.mag_dro_y.set("Y: {:.1f} mT".format(self.mag_dro_val_sums[1]))
+      self.mag_dro_offset_x.set("X Offset: {:.1f} %".format(self.mag_dro_val_sums[2]*100))
+      self.mag_dro_offset_y.set("Y: {:.1f} %".format(self.mag_dro_val_sums[3]*100))
 
     if self.popup_window:
       self.popup_window.update()
@@ -196,6 +205,21 @@ class MainWindow(tk.Frame):
 
     self.root.after(30, self.update)
 
+  def calibrate_submenu(self):
+    
+    self.calibrate_submenu_button.pack_forget()
+    self.menu_button.pack_forget()
+    self.clear_button.pack_forget()
+    self.logscreen_button.pack_forget()
+    self.menu_back_button.pack(side="bottom", fill="both")
+    self.calibratemag_button.pack(side="bottom", fill="both")
+    self.calibrateopt_button.pack(side="bottom", fill="both")
+    self.calibrateac_button.pack(side="bottom", fill="both")
+    self.calibrat_all_button.pack(side="bottom", fill="both")
+  
+  def calibrate_all(self):
+    pass
+  
   def calibrate_ac(self):
     self.ac_sensor.start_calibration()
     if not self.popup_window or not self.pu_root.winfo_exists():
@@ -212,8 +236,24 @@ class MainWindow(tk.Frame):
       self.popup_window = CalibrationPopUp(self.pu_root, self.ac_sensor.calibration_state, self.conf)
       self.popup_window.pack(side="top", fill="both", expand=True)
 
-  def calibrate_mc(self):
+  def calibrate_opt(self):
     pass
+  
+  def calibrate_mag(self): ###
+    self.mag_sensor.start_calibration()
+    if not self.popup_window or not self.pu_root.winfo_exists():
+      # create new window
+      self.pu_root = tk.Toplevel(self.root)
+      self.pu_root.wm_transient(self.root)
+      self.pu_root.wm_title("Magnetic Sensor Calibration")
+      # make it centered
+      x = (self.pu_root.winfo_screenwidth()  - 500) / 2
+      y = (self.pu_root.winfo_screenheight() - 200) / 2
+      self.pu_root.geometry(f'500x200+{int(x)}+{int(y)}')
+      # deactivate mainWindow
+      self.pu_root.grab_set()
+      self.popup_window = CalibrationPopUp(self.pu_root, self.mag_sensor.calibration_state, self.conf)
+      self.popup_window.pack(side="top", fill="both", expand=True)
 
   def open_log(self):
     #create new window
@@ -233,17 +273,18 @@ class MainWindow(tk.Frame):
   # Menu Button
   def menu(self):
     self.menu_back_button.pack(side="bottom", fill="both")
-    self.calibrateac_button.pack(side="bottom", fill="both")
-    self.calibratemc_button.pack(side="bottom", fill="both")
+    self.calibrate_submenu_button.pack(side="bottom", fill="both")
     self.clear_button.pack(side="bottom", fill="both")
     self.logscreen_button.pack(side="bottom", fill="both")
     self.menu_button.pack_forget()
 
   #Back Button
   def back(self):
+    self.calibratemag_button.pack_forget()
+    self.calibrateopt_button.pack_forget()
     self.calibrateac_button.pack_forget()
-    self.calibratemc_button.pack_forget()
     self.clear_button.pack_forget()
     self.logscreen_button.pack_forget()
+    self.calibrate_submenu_button.pack_forget()
     self.menu_button.pack(side="bottom", fill="both")
     self.menu_back_button.pack_forget()

raspberry-pi/sensors/connection.py

@@ -122,22 +122,29 @@ class ArduinoSlave(SerialConnection):
       int(self.sensorData[4]) / 1000
     )
   
-  def getAccelValues(self):
+  def getMagneticOffsets(self):
     return (
       int(self.sensorData[5]) / 1000,
       int(self.sensorData[6]) / 1000,
       int(self.sensorData[7]) / 1000,
     )
   
+  def getAccelValues(self):
+    return (
+      int(self.sensorData[8]) / 1000,
+      int(self.sensorData[9]) / 1000,
+      int(self.sensorData[10]) / 1000,
+    )
+  
   def getGyroValues(self):
     return (
-      int(self.sensorData[8])  / 1000, #
-      int(self.sensorData[9])  / 1000, #
-      int(self.sensorData[10]) / 1000  #
+      int(self.sensorData[11])  / 1000, #
+      int(self.sensorData[12])  / 1000, #
+      int(self.sensorData[13]) / 1000  #
     )
   
   def getTemperature(self): # in °C
-    return int(self.sensorData[11]) / 1000
+    return int(self.sensorData[14]) / 1000
 
   def addRecvCallback(self, cb):
     self._recvCbs.append(cb)

raspberry-pi/sensors/magneticSensor.py

@@ -1,10 +1,13 @@
 import queue
+import statistics
 from struct import calcsize
 import numpy as np
 import time
 import threading # ?
 import random # ?
+from configparser import ConfigParser
 
+from sensors.calibration import CalibrationStateMashine
 from sensors.connection import globalArduinoSlave
 import logHandler
 
@@ -15,52 +18,55 @@ class MagneticSensor:
   def __init__(self, conf):
     self.conf = conf
     self.queue = queue.Queue()
+    self.calibration_state = CalibrationStateMashine()
     self.success = False
-    self.mpu_array = []
-    self.measurements_gyro = float(conf["mag_sensor"]["measurements_gyro"])
-    self.measurements_accel = float(conf["mag_sensor"]["measurements_accel"])
-    self.mpu_gyro_offsets = [0.0,0.0,0.0]
+    self.mag_offset_x = float(conf["mag_sensor"]["mag_offset_x"])
+    self.mag_offset_y = float(conf["mag_sensor"]["mag_offset_y"])
     self.log_handler = logHandler.get_log_handler()
   
   def start(self):
     if not conn.isConnected():
       conn.open()
     conn.addRecvCallback(self._readCb)
-    #self.dummyActive = True
-    #dummyThread = threading.Thread(target=self._readCb_dummy)
-    #dummyThread.start()
 
   def _readCb(self, raw):
-    #print("mag: ", conn.getMagneticField())
-    #print("accel: ", conn.getAccelValues())
-    print("gyro: ", conn.getGyroValues())
+    mag_values = conn.getMagneticField()
+    print("Magnetic Offsets:", conn.getMagneticOffsets())
+    if mag_values[0] >= 0 and mag_values[1] >= 0:
+      pass
+      #position = self.calculate_position(mag_values) ### MUSS AUF MAG. SENSOR ANGEPASST WERDEN!!!
+      #if position != None:
+      #  self.pass_to_gui(position + value)
 
-  def calibrate(self):
-    # Gyroscope Calibration (TESTING!)
-    self.mpu_array = [] # clear Array for next calibration
-    while True:
-      try:
-        gyro_x, gyro_y, gyro_z = conn.getGyroValues()
-      except:  
-        continue
-      
-      self.mpu_array.append([gyro_x,gyro_y,gyro_z])
-      
-      if np.shape(self.mpu_array)[0] == self.measurements_gyro:
-            for qq in range(0,3):
-                self.mpu_gyro_offsets[qq] = np.mean(np.array(self.mpu_array)[:,qq]) # average
-            break
-      print('Gyro Calibration Complete')
-      print(self.mpu_gyro_offsets)
-      return self.mpu_gyro_offsets
-    
-    # Accelerometer Calibration
-    self.mpu_array = [] # clear array for next calibration
-    accel_x, accel_y, accel_z = conn.getAccelValues()
 
-    # Magnetometer Calibration
-    self.mpu_array = [] # clear array for next calibration
-    magneto_x, magneto_y, magneto_z = conn.getMagneticField()
+  def setOffsets(self):
+    # Read config file
+    config_object = ConfigParser()
+    config_object("config.ini")
+    # Get mag_sensor section
+    mag_sensor = config_object["mag_sensor"]
+    # Update Offsets
+    mag_sensor["offset_x"] = conn.getMagneticOffsets(0)
+    mag_sensor["offset_y"] = conn.getMagneticOffsets(1)
+     
+
+  def start_calibration(self): ###
+    self.calibration_state.reset_state()
+    self.time_vals = [[],[]]
+    self.calibration_state.next_state()
+  
+  def calibrate(self, value): ### öffnet erstmal popup :)
+    #pass
+     if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
+      self.time_vals[0].append(value[0])
+      self.time_vals[1].append(value[1])
+      self.calibration_state.progress = len(self.time_vals[0]) / 2
+      if len(self.time_vals[0]) >= 100:
+        self.cal_values["front"][0]  = statistics.mean(self.time_vals[0])
+        self.cal_values["front"][1] = statistics.mean(self.time_vals[1])
+        self.time_vals = [[],[]]
+        self.calibration_state.next_state() # signal gui to get next position
+
 
   def read(self):
     return conn.getMagneticField()

start.sh

@@ -0,0 +1,3 @@
+#!/usr/bin/bash
+cd ./raspberry-pi
+python3 main.py