IMU fix and rewrite

Dockerfile

@@ -0,0 +1,13 @@
+#FROM arm64v8/python:3.11-bookworm
+FROM python:3.9-bullseye
+LABEL authors="Tobias Madlberger"
+
+WORKDIR /app
+
+RUN apt-get update && apt-get install -y build-essential cmake git gcc-aarch64-linux-gnu g++-aarch64-linux-gnu gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf swig doxygen
+
+COPY . .
+
+RUN chmod +x build.sh
+
+RUN ./build.sh

binding/python/CMakeLists.txt

@@ -2,8 +2,8 @@ option(with_python_binding "Build python bindings" ON)
 
 if(with_python_binding)
   find_package(SWIG REQUIRED)
-  find_package(PythonLibs 3 REQUIRED)
-
+  find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
+  
   set(KIPR_PYTHON_WRAPPER ${CMAKE_CURRENT_BINARY_DIR}/kipr_python.cpp)
 
   set(PACKAGE_DIR ${CMAKE_CURRENT_BINARY_DIR}/package)

build.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+set -e
+
+# Create a build directory
+mkdir -p build
+cd build
+
+# Run CMake to configure the project
+cmake .. \
+    -DCMAKE_TOOLCHAIN_FILE=../toolchain/arm-linux-gnueabihf.cmake \
+    -Dpackage_debian=ON \
+    -DCPACK_DEBIAN_PACKAGE_ARCHITECTURE=armhf
+
+# Build the project and create the .deb package
+make -j$(nproc --ignore=1) package

deploy.sh

@@ -0,0 +1,21 @@
+#!/bin/bash
+set -e
+
+# --- Configuration ---
+PI_IP="192.168.125.1"
+DEB_FILE="build/libkipr-1.1.0-Linux.deb"
+PI_USER="pi"
+
+# --- Build ---
+docker buildx build --platform linux/arm64 -t wallaby-build --load .
+
+# --- Deployment ---
+echo "Deploying to Raspberry Pi at $PI_IP..."
+
+# Copy the .deb file to the Raspberry Pi
+scp "$DEB_FILE" "$PI_USER@$PI_IP:/tmp/"
+
+# Install the .deb file on the Raspberry Pi
+ssh "$PI_USER@$PI_IP" "sudo dpkg -i /tmp/$(basename $DEB_FILE)"
+
+echo "Deployment complete!"

module/accel/public/kipr/accel/accel.h

@@ -1,7 +1,8 @@
 /*!
- * \file analog.h
+ * \file accel.h
  * \copyright KISS Institute for Practical Robotics
  * \defgroup accel Accelerometer
+ * \authors Tobias Madlberger <[email protected]>
  */
 /**
  * \page Accelerometer
@@ -18,33 +19,32 @@ extern "C" {
 #endif
 
 /*!
- * \brief Gets the sensed x acceleration
- * \description Wallaby: +/- 2G range,  1024 per G
- * \description Wombat: +/- 2G range,  1024 per G
- * \description This should be approximately 0 when at rest and flat on a table
+ * \brief Gets the sensed x acceleration in the NED frame (Side with ports is defined as North)
+ * \description Wombat: +/- 2G range
+ * \description This should be approximately 0 when at rest and flat on a table.
+ * The measured unit is in m/s²
  * \return The latest signed x acceleration value
  * \ingroup accel
  */
 signed short accel_x();
 
 
 /*!
- * \brief Gets the sensed y acceleration
- * \description Wallaby: +/- 2G range,  1024 per G
- * \description Wombat: +/- 2G range,  1024 per G
+ * \brief Gets the sensed y acceleration in the NED frame (Side with ports is defined as North)
+ * \description Wombat: +/- 2G range
  * \description This should be approximately 0 when at rest and flat on a table
+ * The measured unit is in m/s²
  * \return The latest signed y acceleration value
  * \ingroup accel
  */
 signed short accel_y();
 
 
 /*!
- * \brief Gets the sensed z acceleration
- * \description Wallaby: +/- 2G range,  1024 per G
- * \description Wallaby: This should be approximately -1024 when at rest and flat on a table
- * \description Wombat: +/- 2G range,  1024 per G
- * \description Wombat: This should be approximately -512 when at rest and flat on a table
+ * \brief Gets the sensed z acceleration in the NED frame (Side with ports is defined as North)
+ * \description Wombat: +/- 2G range
+ * \description Wombat: This should be approximately -9.8 when at rest and flat on a table
+ * The measured unit is in m/s²
  * \return The latest signed z acceleration value
  * \ingroup accel
  */
@@ -55,6 +55,8 @@ signed short accel_z();
  * \brief Calibrates a lowpass filter for the accelerometer
  * \description Sets a low-pass filter. Put at beginning of your program or before you use accelerometer commands if you want calibrated output.
  * \description This function will block for around 500ms taking samples of the accelerometer at standstill.
+ * \description This function will determine the accelerometer bias as well as the gravity direction.
+ * \description When the wombat is flat at rest, the reading should be calibrated to be approximately 0, 0, -9.8.
  * \return 1: success 0: failure
  * \ingroup accel
  */
@@ -66,3 +68,4 @@ int accel_calibrate();
 
 
 #endif
+

module/accel/src/accel_p.cpp

@@ -1,57 +1,131 @@
+/*
+*  Modified by: Tobias Madlberger*, 2025
+*  * Corresponding author's email: [email protected]
+*/
+
 #include "accel_p.hpp"
 #include "kipr/core/platform.hpp"
 #include "kipr/core/registers.hpp"
 #include "kipr/time/time.h"
 
+constexpr float gravity = 9.80665;
+
 using namespace kipr;
 using namespace kipr::accel;
 
 using kipr::core::Platform;
 
 namespace
 {
-  double Biasx = 0;
-  double Biasy = 0;
-  double Biasz = 0;
+    double biasX = 0;
+    double biasY = 0;
+    double biasZ = 0;
 }
 
+/*
+ * Outputs the internal accel x reading
+ * Pulls accelerometer data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_accel_x()
+{
+    return Platform::instance()->readRegisterFloat(REG_RW_ACCEL_X_0);
+}
+
+/*
+ * Outputs the internal accel y reading
+ * Pulls accelerometer data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_accel_y()
+{
+    return Platform::instance()->readRegisterFloat(REG_RW_ACCEL_Y_0);
+}
+
+/*
+ * Outputs the internal accel z reading
+ * Pulls accelerometer data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_accel_z()
+{
+    return Platform::instance()->readRegisterFloat(REG_RW_ACCEL_Z_0);
+}
+
+/*
+ * Returns the accel_x in the NED frame. +x is forward (North). -x is backward (South).
+ */
 short kipr::accel::accel_x()
 {
-  return static_cast<signed short>(Platform::instance()->readRegister16b(REG_RW_ACCEL_X_H)) / 16 - Biasx;
+    return static_cast<short>(-local_accel_y());
 }
 
+/*
+ * Returns the accel_y in the NED frame. +y is right (East). -y is left (West).
+ */
 short kipr::accel::accel_y()
 {
-  return static_cast<signed short>(Platform::instance()->readRegister16b(REG_RW_ACCEL_Y_H)) / 16 - Biasy;
+    return static_cast<short>(local_accel_x());
 }
 
+/*
+ * Returns the accel_z in the NED frame. +z is up. -z is down.
+ */
 short kipr::accel::accel_z()
 {
-  return static_cast<signed short>(Platform::instance()->readRegister16b(REG_RW_ACCEL_Z_H)) / 16 - Biasz;
+    return static_cast<short>(local_accel_z());
 }
 
-// Simple low-pass filter for accelerometer
 bool kipr::accel::accel_calibrate()
 {
-  int samples = 50;
-
-  // Find the average noise
-  int i = 0;
-  double sumX = 0, sumY = 0, sumZ = 0;
-  while (i < samples)
-  {
-    sumX += accel_z();
-    sumY += accel_y();
-    sumZ += accel_x();
+    constexpr int samples = 50;
+    constexpr int delay_ms = 10;
 
-    msleep(10);
-    i++;
-  }
+    // Find the average readings when device is stationary
+    double sumX = 0, sumY = 0, sumZ = 0;
+
+    // Collect samples
+    for (int i = 0; i < samples; ++i)
+    {
+        sumX += local_accel_x();
+        sumY += local_accel_y();
+        sumZ += local_accel_z();
+        msleep(delay_ms);
+    }
 
-  Biasx = sumX / samples;
-  Biasy = sumY / samples;
-  Biasz = sumZ / samples + 512; // Z axis should be detecting gravity
+    // Calculate average raw values
+    const double avgX = sumX / samples;
+    const double avgY = sumY / samples;
+    const double avgZ = sumZ / samples;
+
+    // Determine which axis has the largest absolute value (likely pointing toward/away from gravity)
+    double absX = std::abs(avgX);
+    double absY = std::abs(avgY);
+    double absZ = std::abs(avgZ);
+
+    // Autodetect gravity direction and calibrate
+    if (absZ >= absX && absZ >= absY) {
+        const double gravitySign = avgZ >= 0 ? 1.0 : -1.0;
+
+        biasX = avgX;
+        biasY = avgY;
+        biasZ = avgZ - gravitySign * gravitySign;
+        printf("[Accel Calibration]: Z axis is aligned with gravity (%f)\n", gravitySign);
+    } 
+    else if (absX >= absY && absX >= absZ) {
+        const double gravitySign = avgX >= 0 ? 1.0 : -1.0;
+
+        biasX = avgX - gravitySign * gravitySign;
+        biasY = avgY;
+        biasZ = avgZ;
+        printf("[Accel Calibration]: X axis is aligned with gravity (%f)\n", gravitySign);
+    }
+    else {
+        const double gravitySign = avgY >= 0 ? 1.0 : -1.0;
+
+        biasX = avgX;
+        biasY = avgY - gravitySign * gravitySign;
+        biasZ = avgZ;
+        printf("[Accel Calibration]: Y axis is aligned with gravity (%f)\n", gravitySign);
+    }
 
-  printf("[Accel Calibration]: Bias Z: %f | Bias Y: %f | Bias X: %f \n", Biasz, Biasy, Biasx);
-  return 0;
-}
+    printf("[Accel Calibration]: Bias X: %f | Bias Y: %f | Bias Z: %f\n", biasX, biasY, biasZ);
+    return true;  // Return true to indicate successful calibration
+}

module/botball/src/botball_c.cpp

@@ -41,6 +41,7 @@ class ShutDownIn : public kipr::thread::Thread
 		std::cout << std::endl << "Shutdown after " << (end - start) << " seconds" << std::endl;
 		// Note: Might want to move this to botui in the future.
 		// Create::instance()->stop();
+		// ToDo: Shutdown all threads
 		kipr::core::cleanup(true);
 		_exit(0);
 	}

module/core/protected/kipr/core/platform.hpp

@@ -34,6 +34,8 @@ namespace kipr
       unsigned int readRegister32b(unsigned char address);
       void writeRegister32b(unsigned char address, unsigned int value);
 
+      float readRegisterFloat(unsigned char address);
+
       template <typename... Args>
       void submit(Args &&...args)
       {