Merge remote-tracking branch 'wokoeck/master'

.gitignore

@@ -1,4 +1,3 @@
-.vscode/c_cpp_properties.json
 .venv
 .vscode/settings.json
 

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),

platformio.ini

@@ -0,0 +1,19 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:uno]
+platform = atmelavr
+board = uno
+framework = arduino
+monitor_speed = 1000000
+board_build.f_cpu = 16000000L
+lib_deps = 
+	hideakitai/MPU9250@^0.2.3
+	martinl1/BMP280_DEV@^1.0.18

raspberry-pi/config.ini

@@ -37,6 +37,9 @@
   overhead_right = 20
 
 [mag_sensor]
+# Offsets for the Magnetometer
+mag_offset_x = 0
+mag_offset_y = 0
 
 [opt_sensor]
   capture_device = -1

raspberry-pi/gui/mainWindow.py

@@ -7,18 +7,21 @@ import numpy as np
 from gui.popup import CalibrationPopUp
 from gui.graph import Graph
 from gui.logScreen import LogScreen
+from sensors.opticalSensor import OpticalSensor
 import logHandler
 
 
 class MainWindow(tk.Frame):
-  def __init__(self, root, conf, ac_sensor, opt_sensor):
-    self.root        = root
-    self.conf        = conf
-    self.ac_sensor   = ac_sensor
-    self.opt_sensor  = opt_sensor
-    self.log_handler = logHandler.get_log_handler()
-    self.popup_window       = None
-    self.log_window  = None
+  def __init__(self, root, conf, ac_sensor, opt_sensor, mag_sensor):
+    self.root         = root
+    self.conf         = conf
+    self.ac_sensor    = ac_sensor
+    self.opt_sensor   = opt_sensor
+    self.mag_sensor   = mag_sensor
+    self.log_handler  = logHandler.get_log_handler()
+    self.popup_window = None
+    self.log_window   = None
+    self.mainWindow   = None
 
     tk.Frame.__init__(self, root)
     # data plot at left side
@@ -55,17 +58,48 @@ class MainWindow(tk.Frame):
     tk.Label(self.controls, textvariable=self.opt_dro_offset, anchor="nw").pack(side="top", fill="both", expand=False)
     tk.Label(self.controls, textvariable=self.opt_dro_size, anchor="nw").pack(side="top", fill="both", expand=False)
 
-    quit_button = tk.Button(self.controls, text="Quit", command=self.root.destroy, height=2, foreground="red")
-    quit_button.pack(side="bottom", fill="both")
+    self.mag_dro_val_sums = np.ndarray((4), dtype=np.float)
+    self.mag_dro_val_count = 0
+    self.mag_label = tk.Label(self.controls, text="Magnetic Sensor", anchor="c", font=("Helvatica", 10, 'bold'))
+    self.mag_label.pack(side="top", fill="both", expand=False)
+    self.mag_dro_x = tk.StringVar()
+    self.mag_dro_y = tk.StringVar()
+    self.mag_dro_offset_x = tk.StringVar()
+    self.mag_dro_offset_y = tk.StringVar()
+    tk.Label(self.controls, textvariable=self.mag_dro_x, anchor = "nw").pack(side = "top", fill = "both", expand = False)
+    tk.Label(self.controls, textvariable=self.mag_dro_y, anchor = "nw").pack(side = "top", fill = "both", expand = False)
+    tk.Label(self.controls, textvariable=self.mag_dro_x, anchor = "nw").pack(side = "top", fill = "both", expand = False)
+    tk.Label(self.controls, textvariable=self.mag_dro_y, anchor = "nw").pack(side = "top", fill = "both", expand = False)
 
-    calibrate_button = tk.Button(self.controls, text="Calibrate AC", command=self.calibrate_ac, height=4)
-    calibrate_button.pack(side="bottom", fill="both")
+    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")
 
-    clear_button = tk.Button(self.controls, text="Clear graph", command=self.graph.clear, height=4)
-    clear_button.pack(side="bottom", fill="both")
+    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.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()
 
-    logscreen_button = tk.Button(self.controls, text="Log", command=self.open_log, height=4)
-    logscreen_button.pack(side="bottom", fill="both")
+    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=2)
+    self.logscreen_button.pack_forget()
+
+    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=2)
+    self.menu_back_button.pack_forget()
 
   def update(self):
     if not self.root.winfo_exists():
@@ -87,8 +121,17 @@ class MainWindow(tk.Frame):
         opt_positions.append(data[0:2])
         self.opt_dro_val_sums += data
         self.opt_dro_val_count += 1
+    
+    mag_positions = []
+    while self.mag_sensor.queue.qsize() > 0:
+      name, data = self.mag_sensor.queue.get()
+      if name == "data":
+        mag_positions.append(data[0:2])
+        self.mag_dro_val_sums += data
+        self.mag_dro_val_count += 1
+    
     # graph shows all values as a line
-    self.graph.update([ac_positions, opt_positions])
+    self.graph.update([ac_positions, opt_positions, mag_positions])
 
     # update status color
     if self.ac_sensor.dummyActive:
@@ -106,6 +149,14 @@ class MainWindow(tk.Frame):
     else:
       self.opt_label.config(fg="black", bg="yellow")
 
+    if not self.mag_sensor.success:
+     self.mag_label.config(fg="white", bg="red")
+    elif len(mag_positions) > 0:
+     self.mag_label.config(fg="white", bg="green")
+    else:
+     self.mag_label.config(fg="black", bg="yellow")
+
+
     # readouts will only be updated so often
     if self.controlsUpdateTime + 0.4 < time.time():
       self.controlsUpdateTime = time.time()
@@ -120,7 +171,7 @@ class MainWindow(tk.Frame):
       self.ac_dro_t1.set("t1: {:.3f} ms".format(self.ac_dro_val_sums[2]/1000))
       self.ac_dro_t2.set("t2: {:.3f} ms".format(self.ac_dro_val_sums[3]/1000))
 
-      self.ac_dro_val_sums.fill(0)
+      self.ac_dro_val_sums.fill(0)    
       self.ac_dro_val_count = 0
 
       if self.opt_dro_val_count > 0:
@@ -130,12 +181,22 @@ class MainWindow(tk.Frame):
 
       self.opt_dro_x.set("X: {:.1f} mm".format(self.opt_dro_val_sums[0]))
       self.opt_dro_y.set("Y: {:.1f} mm".format(self.opt_dro_val_sums[1]))
-      self.opt_dro_offset.set("offset: {:.1f} %".format(self.opt_dro_val_sums[2]*100))
-      self.opt_dro_size.set("size: {:.1f} %".format(self.opt_dro_val_sums[3]*100))
+      self.opt_dro_offset.set("X Offset: {:.1f} %".format(self.opt_dro_val_sums[2]*100))
+      self.opt_dro_size.set("Y Offset: {:.1f} %".format(self.opt_dro_val_sums[3]*100))
 
       self.opt_dro_val_sums.fill(0)
       self.opt_dro_val_count = 0
 
+      if self.mag_dro_val_count > 0:
+        self.mag_dro_val_sums /= self.mag_dro_val_count
+      else:
+        self.mag_dro_val_sums.fill(0)
+
+      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()
     
@@ -144,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():
@@ -160,6 +236,25 @@ 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_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
     self.log_root = tk.Toplevel(self.root)
@@ -174,3 +269,22 @@ class MainWindow(tk.Frame):
     self.log_window = LogScreen(self.log_root)
     self.log_window.pack(side="top", fill="both", expand=True)
 
+  
+  # Menu Button
+  def menu(self):
+    self.menu_back_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.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/main.py

@@ -2,6 +2,7 @@
 
 from sensors.acousticSensor import AcousticSensor
 from sensors.opticalSensor import OpticalSensor
+from sensors.magneticSensor import MagneticSensor
 from gui.mainWindow import MainWindow
 
 import configparser
@@ -11,19 +12,22 @@ import logHandler
 
 conf = configparser.ConfigParser()
 conf.read('config.ini')
+print(conf.sections())
 
 def main():
   log_handler = logHandler.get_log_handler(int(conf['gui']['log_lines']))
   ac_sensor = AcousticSensor(conf)
   opt_sensor = OpticalSensor(conf)
+  mag_sensor = MagneticSensor(conf)
 
   try:
     ac_sensor.start()
     opt_sensor.start()
+    mag_sensor.start()
     root = tk.Tk()
     root.title("Tracking System")
     root.attributes('-fullscreen', conf['gui']['fullscreen'] == "yes")
-    view = MainWindow(root, conf, ac_sensor, opt_sensor)
+    view = MainWindow(root, conf, ac_sensor, opt_sensor, mag_sensor)
     view.pack(side="top", fill="both", expand=True)
     view.update()
     root.mainloop()
@@ -36,8 +40,6 @@ def main():
   finally:
     ac_sensor.stop()
     opt_sensor.stop()
+    mag_sensor.stop()
 
-main()
-
-
-
+main()

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
+      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,24 +1,79 @@
+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
 
 conn = globalArduinoSlave()
 
 
 class MagneticSensor:
-  def __init__(self):
-    pass
-
+  def __init__(self, conf):
+    self.conf = conf
+    self.queue = queue.Queue()
+    self.calibration_state = CalibrationStateMashine()
+    self.success = False
+    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)
 
   def _readCb(self, raw):
-    print("mag: ", conn.getMagneticField())
+    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 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 calibrate(self, x, y):
-    pass
 
   def read(self):
-    return conn.getMagneticField()
+    return conn.getMagneticField()
+
+  def stop(self):
+    self.log_handler.log_and_print("stop magnetic sensor")
+    conn.close()
+
+  def pass_to_gui(self, data):
+    self.queue.put(("data", data))

start.sh

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