feat: Add Magnetometer Calibration

CMakeLists.txt

@@ -57,9 +57,11 @@ target_sources(score_addon_puara
 
 target_include_directories(score_addon_puara
   PRIVATE
-  3rdparty/puara-gestures/include/
-  3rdparty/puara-gestures/3rdparty/
+    3rdparty/puara-gestures/include/
+    3rdparty/puara-gestures/3rdparty/
+    "${3RDPARTY_FOLDER}/eigen"
 )
+
 target_link_libraries(score_addon_puara PUBLIC BioData)
 
 avnd_score_plugin_add(
@@ -91,6 +93,16 @@ avnd_score_plugin_add(
   MAIN_CLASS BioData_Heart
   NAMESPACE puara_gestures::objects
 )
+avnd_score_plugin_add(
+  BASE_TARGET score_addon_puara
+  SOURCES
+    Puara/Calibration.hpp
+    Puara/Calibration.cpp
+  TARGET puara_calibration
+  MAIN_CLASS Calibration
+  NAMESPACE puara_gestures::objects
+)
+
 
 avnd_score_plugin_add(
   BASE_TARGET score_addon_puara

Puara/Calibration.cpp

@@ -0,0 +1,57 @@
+#include "Calibration.hpp"
+
+namespace puara_gestures::objects
+{
+
+void Calibration::operator()(tick t)
+{
+  if (inputs.record_data.value != m_last_record_state)
+  {
+    m_is_recording = inputs.record_data.value;
+    if (m_is_recording)
+    {
+      m_calibration_logic.rawMagData.clear();
+      outputs.points_collected.value = 0;
+    }
+    m_last_record_state = m_is_recording;
+  }
+
+  if (m_is_recording)
+  {
+    m_calibration_logic.recordRawMagData(inputs.raw_magnetometer.value);
+    outputs.points_collected.value = m_calibration_logic.rawMagData.size();
+  }
+
+  if (!inputs.generate_calibration.values.empty())
+  {
+    if(m_is_recording)
+    {
+      m_is_recording = false;
+      inputs.record_data.value = false;
+      m_last_record_state = false;
+    }
+
+    if (!m_calibration_logic.rawMagData.empty())
+    {
+      m_calibration_logic.gravitationField = inputs.gravitation_field.value;
+      m_calibration_logic.generateMagnetometerMatrices();
+
+      const auto& bias = m_calibration_logic.hardIronBias;
+      outputs.hard_iron_bias.value = { (float)bias(0), (float)bias(1), (float)bias(2) };
+
+      const auto& matrix = m_calibration_logic.softIronMatrix;
+      outputs.soft_iron_row1.value = { (float)matrix(0,0), (float)matrix(0,1), (float)matrix(0,2) };
+      outputs.soft_iron_row2.value = { (float)matrix(1,0), (float)matrix(1,1), (float)matrix(1,2) };
+      outputs.soft_iron_row3.value = { (float)matrix(2,0), (float)matrix(2,1), (float)matrix(2,2) };
+    }
+  }
+
+  puara_gestures::Imu9Axis raw_imu;
+  raw_imu.magn = inputs.raw_magnetometer.value;
+
+  m_calibration_logic.applyMagnetometerCalibration(raw_imu);
+
+  outputs.calibrated_magnetometer.value = m_calibration_logic.myCalIMU.magn;
+}
+
+}

Puara/Calibration.hpp

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <halp/audio.hpp>
+#include <halp/controls.hpp>
+#include <halp/meta.hpp>
+#include <halp/sample_accurate_controls.hpp>
+#include <puara/utils/calibration.h>
+#include <puara/structs.h>
+
+namespace puara_gestures::objects
+{
+
+class Calibration
+{
+public:
+  halp_meta(name, "Magnetometer Calibration")
+  halp_meta(category, "Gestures")
+  halp_meta(c_name, "puara_calibration")
+  halp_meta(description, "Calibrates a raw magnetometer stream to correct for hard and soft-iron distortions.")
+  halp_meta(manual_url, "https://github.com/Puara/puara-gestures/")
+  halp_meta(uuid, "0b46699b-5e17-4742-a585-e7d7ad9b22ca")
+
+  struct ins
+  {
+    halp::val_port<"Raw Magnetometer", puara_gestures::Coord3D> raw_magnetometer;
+    halp::toggle<"Record Data"> record_data;
+    halp::accurate<halp::impulse_button<"Generate Calibration">> generate_calibration;
+    halp::knob_f32<"Gravitation Field", halp::range{0.0, 1000.0, 234.0f}> gravitation_field;
+  } inputs;
+
+  struct outs
+  {
+    halp::val_port<"Calibrated Magnetometer", puara_gestures::Coord3D> calibrated_magnetometer;
+    halp::val_port<"Points Collected", int> points_collected;
+    halp::val_port<"Hard-Iron Bias", puara_gestures::Coord3D> hard_iron_bias;
+    halp::val_port<"Soft-Iron Row 1", puara_gestures::Coord3D> soft_iron_row1;
+    halp::val_port<"Soft-Iron Row 2", puara_gestures::Coord3D> soft_iron_row2;
+    halp::val_port<"Soft-Iron Row 3", puara_gestures::Coord3D> soft_iron_row3;
+  } outputs;
+
+  halp::setup setup;
+  void prepare(halp::setup info) { setup = info; }
+
+  using tick = halp::tick;
+  void operator()(tick t);
+
+private:
+  puara_gestures::utils::Calibration m_calibration_logic;
+  bool m_is_recording = false;
+  bool m_last_record_state = false;
+};
+
+}