Update Magnetische kalib.

.gitignore

@@ -1,4 +1,9 @@
 .venv
 .vscode/settings.json
 
-__pycache__
+__pycache__
+.vscode/c_cpp_properties.json
+.pio/build/project.checksum
+.pio/build/uno/idedata.json
+.vscode/c_cpp_properties.json
+.vscode/launch.json

.pio/build/project.checksum

@@ -1 +1 @@
-641ec06da10d5b04d4b357a2410525e3ddd8a2d7
+fb7487a38c33ab8f5ddac2d3de9ed44f3bd3ef87

.vscode/c_cpp_properties.json

@@ -8,10 +8,10 @@
         {
             "name": "PlatformIO",
             "includePath": [
-                "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/arduino/include",
-                "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/arduino/src",
-                "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/libdeps/uno/BMP280_DEV",
-                "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/libdeps/uno/MPU9250",
+                "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/include",
+                "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/src",
+                "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/libdeps/uno/BMP280_DEV",
+                "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/libdeps/uno/MPU9250",
                 "/home/user/.platformio/packages/framework-arduino-avr/cores/arduino",
                 "/home/user/.platformio/packages/framework-arduino-avr/variants/standard",
                 "/home/user/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
@@ -24,10 +24,10 @@
             "browse": {
                 "limitSymbolsToIncludedHeaders": true,
                 "path": [
-                    "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/arduino/include",
-                    "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/arduino/src",
-                    "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/libdeps/uno/BMP280_DEV",
-                    "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/libdeps/uno/MPU9250",
+                    "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/include",
+                    "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/src",
+                    "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/libdeps/uno/BMP280_DEV",
+                    "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/libdeps/uno/MPU9250",
                     "/home/user/.platformio/packages/framework-arduino-avr/cores/arduino",
                     "/home/user/.platformio/packages/framework-arduino-avr/variants/standard",
                     "/home/user/.platformio/packages/framework-arduino-avr/libraries/SPI/src",
@@ -39,7 +39,7 @@
                 ]
             },
             "defines": [
-                "PLATFORMIO=50203",
+                "PLATFORMIO=50205",
                 "ARDUINO_AVR_UNO",
                 "F_CPU=16000000L",
                 "ARDUINO_ARCH_AVR",

.vscode/extensions.json

@@ -1,7 +1,10 @@
-{
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
-    "recommendations": [
-        "platformio.platformio-ide"
-    ]
-}
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

.vscode/launch.json

@@ -12,7 +12,7 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug",
-            "executable": "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/build/uno/firmware.elf",
+            "executable": "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/build/uno/firmware.elf",
             "projectEnvName": "uno",
             "toolchainBinDir": "/home/user/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",
@@ -25,7 +25,7 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (skip Pre-Debug)",
-            "executable": "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/build/uno/firmware.elf",
+            "executable": "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/build/uno/firmware.elf",
             "projectEnvName": "uno",
             "toolchainBinDir": "/home/user/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart"
@@ -34,7 +34,7 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (without uploading)",
-            "executable": "/home/user/Schreibtisch/Hochschule/WPM_Projekt/lern-tracking-system/.pio/build/uno/firmware.elf",
+            "executable": "/home/user/Schreibtisch/Projektarbeit/lern-tracking-system/.pio/build/uno/firmware.elf",
             "projectEnvName": "uno",
             "toolchainBinDir": "/home/user/.platformio/packages/toolchain-atmelavr/bin",
             "internalConsoleOptions": "openOnSessionStart",

arduino/src/main.cpp

@@ -31,9 +31,9 @@ void setup() {
   mpu.calibrateAccelGyro();
 
   Serial.println(F("Calibrate Magnetometer"));
-  mpu.calibrateMag();
+  //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"));
+  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"));
 
 }
 
@@ -44,29 +44,31 @@ void loop() {
 
   if(micros() >= loopStart + LOOP_INTERVAL_US) {
     loopStart = micros();
+    /*if (Serial.available() > 0){
+      char c = Serial.read(1);
+      if (c == 'c'){
+        mpu.calibrateMag();
+      }
+    }*/
 
     //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\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\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),
       (long)(mpu.getAccZ()*1000),
-      //gyroscope
+      /*gyroscope (values not needed)
       (long)(mpu.getGyroX()*1000),
       (long)(mpu.getGyroY()*1000),
-      (long)(mpu.getGyroZ()*1000),
+      (long)(mpu.getGyroZ()*1000),*/
       //temperature
       (long)(mpu.getTemperature()*1000),
       //execution time in microseconds

raspberry-pi/config.ini

@@ -36,11 +36,6 @@
   overhead_left  = 20
   overhead_right = 20
 
-[mag_sensor]
-# Offsets for the Magnetometer
-mag_offset_x = 0
-mag_offset_y = 0
-
 [opt_sensor]
   capture_device = -1
   debug_image = yes
@@ -55,6 +50,16 @@ mag_offset_y = 0
   # RaspiCam datasheet: https://www.raspberrypi.org/documentation/hardware/camera/
   fov = 53.50
 
+[mag_sensor]
+max_x = 800
+max_y = 800
+off_x = 0
+off_y = 0
+off_z = 0
+scale_x = 0
+scale_y = 0
+scale_z = 0
+
 [gui]
-  fullscreen = yes
+  fullscreen = no
   log_lines  = 100

raspberry-pi/gui/mainWindow.py

@@ -60,14 +60,10 @@ class MainWindow(tk.Frame):
 
     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)
+    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)
     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)
 
@@ -192,12 +188,12 @@ class MainWindow(tk.Frame):
       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))
+      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("X Offset: {:.1f} %".format(self.mag_dro_val_sums[2]*100))
+      #self.mag_dro_offset_y.set("Y Offset: {:.1f} %".format(self.mag_dro_val_sums[3]*100))
 
-    if self.popup_window:
+    if self.popup_window: # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!~~~~~~~+++*
       self.popup_window.update()
     
     if self.log_window:
@@ -215,7 +211,7 @@ class MainWindow(tk.Frame):
     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")
+    self.calibrate_all_button.pack(side="bottom", fill="both")
   
   def calibrate_all(self):
     pass

raspberry-pi/sensors/acousticSensor.py

@@ -30,6 +30,7 @@ class AcousticSensor:
     self.sonic_speed            = float(conf["ac_sensor"]["sonicspeed"])
     self.overhead_left          = float(conf["ac_sensor"]["overhead_left"])
     self.overhead_right         = float(conf["ac_sensor"]["overhead_right"])
+    self.check = 1
 
     self.log_handler = logHandler.get_log_handler()
 

raspberry-pi/sensors/connection.py

@@ -122,13 +122,6 @@ class ArduinoSlave(SerialConnection):
       int(self.sensorData[4]) / 1000
     )
   
-  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,
@@ -136,15 +129,15 @@ class ArduinoSlave(SerialConnection):
       int(self.sensorData[10]) / 1000,
     )
   
-  def getGyroValues(self):
-    return (
-      int(self.sensorData[11])  / 1000, #
-      int(self.sensorData[12])  / 1000, #
-      int(self.sensorData[13]) / 1000  #
-    )
+  #def getGyroValues(self):
+    #return (
+    #  int(self.sensorData[11])  / 1000, #
+    #  int(self.sensorData[12])  / 1000, #
+    #  int(self.sensorData[13]) / 1000  #
+    #)
   
   def getTemperature(self): # in °C
-    return int(self.sensorData[14]) / 1000
+    return int(self.sensorData[11]) / 1000
 
   def addRecvCallback(self, cb):
     self._recvCbs.append(cb)

raspberry-pi/sensors/magneticSensor.py

@@ -1,12 +1,10 @@
+from distutils.command.config import config
 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
@@ -20,59 +18,95 @@ class MagneticSensor:
     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.offset_x = float(conf["mag_sensor"]["off_x"])
+    self.offset_y = float(conf["mag_sensor"]["off_y"])
+    self.scale_x = float(conf["mag_sensor"]["scale_x"])
+    self.scale_y = float(conf["mag_sensor"]["scale_y"])
+    self.max_x = float(conf["mag_sensor"]["max_x"])
+    self.max_y = float(conf["mag_sensor"]["max_y"])
     self.log_handler = logHandler.get_log_handler()
   
   def start(self):
     if not conn.isConnected():
       conn.open()
+    self.success = True
     conn.addRecvCallback(self._readCb)
 
   def _readCb(self, raw):
-    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)
-     
-
+    value = conn.getMagneticField()
+    if value[0] >= 0 and value[1] >= 0:
+      self.calibrate(value)
+      position = self.calculate_position(value)
+      if position != None:
+        self.pass_to_gui(position + value)
+  
   def start_calibration(self): ###
     self.calibration_state.reset_state()
-    self.time_vals = [[],[]]
+    self.time_vals = [[],[],[]]
     self.calibration_state.next_state()
   
-  def calibrate(self, value): ### öffnet erstmal popup :)
-    #pass
+  def calibrate(self, value): ### öffnet erstmal popup :) # how?
      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.time_vals[2].append(value[2])
       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.cal_values["front"][2] = statistics.mean(self.time_vals[2])
+        self.time_vals = [[],[],[]]
         self.calibration_state.next_state() # signal gui to get next position
 
+      elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2:
+        self.time_vals[0].append(value[0])
+        self.time_vals[1].append(value[1])
+        self.time_vals[2].append(value[2])
+        self.calibration_state.progress = 50 + len(self.time_vals[0]) / 2
+        if len(self.time_vals[0]) >= 100:
+          self.cal_values["back"][0] = statistics.mean(self.time_vals[0])
+          self.cal_values["back"][1] = statistics.mean(self.time_vals[1])
+          self.cal_values["back"][2] = statistics.mean(self.time_vals[2])
+        
+          # all values have been captured
+          
+          # Hard iron distortion & Soft iron distortion 
+          self.offset_x = (self.cal_values["back"][0] - self.cal_values["front"][0]) / 2
+          self.offset_y = (self.cal_values["back"][1] - self.cal_values["front"][1]) / 2
+
+          avg_delta_x = (self.cal_values["back"][0] - self.cal_values["front"][0]) / 2
+          avg_delta_y = (self.cal_values["back"][1] - self.cal_values["front"][1]) / 2
+          avg_delta_z = (self.cal_values["back"][2] - self.cal_values["front"][2]) / 2
+
+          avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3
+
+          self.scale_x = avg_delta / avg_delta_x
+          self.scale_y = avg_delta / avg_delta_y
+          self.scale_z = avg_delta / avg_delta_z
+
+          # max values for placeholder algorithm
+          self.max_x = (self.cal_values["back"][0] - self.offset_x) * self.scale_x
+          self.max_y = (self.cal_values["back"][1] - self.offset_y) * self.scale_y
+        
+          self.calibration_state.next_state()
+
+          return self.offset_x, self.offset_y, self.scale_x, self.scale_y, self.max_x, self.max_y
 
   def read(self):
-    return conn.getMagneticField()
+    value = conn.getMagneticField()
+    return value
+  
+  def calculate_position(self): ###
+    corrected_x = (conn.getMagneticField[0] - self.offset_x) * self.scale_x
+    corrected_y = (conn.getMagneticField[1] - self.offset_y) * self.scale_y
+    # placeholder algorithm (to see if the sensor even works)
+    x = (corrected_x * 400) / self.max_x
+    y = (corrected_y * 400) / self.max_y
+    return (x, y)
 
   def stop(self):
     self.log_handler.log_and_print("stop magnetic sensor")
+    self.success = False
     conn.close()
 
   def pass_to_gui(self, data):

raspberry-pi/sensors/workspace.code-workspace

@@ -0,0 +1,12 @@
+{
+	"folders": [
+		{
+			"path": "../.."
+		},
+		{
+			"name": "arduino",
+			"path": "../../arduino"
+		}
+	],
+	"settings": {}
+}