diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 00000000..13566b81
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/editor.xml b/.idea/editor.xml
new file mode 100644
index 00000000..ec902243
--- /dev/null
+++ b/.idea/editor.xml
@@ -0,0 +1,247 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml
new file mode 100644
index 00000000..1b3bdef8
--- /dev/null
+++ b/.idea/inspectionProfiles/Project_Default.xml
@@ -0,0 +1,52 @@
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 00000000..35eb1ddf
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Dockerfile b/Dockerfile
new file mode 100644
index 00000000..47577650
--- /dev/null
+++ b/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
diff --git a/binding/python/CMakeLists.txt b/binding/python/CMakeLists.txt
index 2d8fa499..6fc4e6e2 100644
--- a/binding/python/CMakeLists.txt
+++ b/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)
diff --git a/build.sh b/build.sh
new file mode 100755
index 00000000..1aff2d95
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/deploy.sh b/deploy.sh
new file mode 100755
index 00000000..13d57e1f
--- /dev/null
+++ b/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!"
diff --git a/module/accel/public/kipr/accel/accel.h b/module/accel/public/kipr/accel/accel.h
index 235ac7ae..72cb5613 100644
--- a/module/accel/public/kipr/accel/accel.h
+++ b/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
*/
/**
* \page Accelerometer
@@ -18,10 +19,10 @@ 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
*/
@@ -29,10 +30,10 @@ 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
*/
@@ -40,11 +41,10 @@ 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
+
diff --git a/module/accel/src/accel_p.cpp b/module/accel/src/accel_p.cpp
index 7814fb56..62e33ef0 100644
--- a/module/accel/src/accel_p.cpp
+++ b/module/accel/src/accel_p.cpp
@@ -1,8 +1,15 @@
+/*
+* Modified by: Tobias Madlberger*, 2025
+* * Corresponding author's email: tobias.madlberger@gmail.com
+*/
+
#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;
@@ -10,48 +17,115 @@ 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(Platform::instance()->readRegister16b(REG_RW_ACCEL_X_H)) / 16 - Biasx;
+ return static_cast(-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(Platform::instance()->readRegister16b(REG_RW_ACCEL_Y_H)) / 16 - Biasy;
+ return static_cast(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(Platform::instance()->readRegister16b(REG_RW_ACCEL_Z_H)) / 16 - Biasz;
+ return static_cast(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
+}
\ No newline at end of file
diff --git a/module/botball/src/botball_c.cpp b/module/botball/src/botball_c.cpp
index 84379b47..f9657895 100644
--- a/module/botball/src/botball_c.cpp
+++ b/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);
}
diff --git a/module/core/protected/kipr/core/platform.hpp b/module/core/protected/kipr/core/platform.hpp
index 57b925ce..771719ce 100644
--- a/module/core/protected/kipr/core/platform.hpp
+++ b/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
void submit(Args &&...args)
{
diff --git a/module/core/protected/kipr/core/registers.hpp b/module/core/protected/kipr/core/registers.hpp
index 7a33f927..78303a35 100644
--- a/module/core/protected/kipr/core/registers.hpp
+++ b/module/core/protected/kipr/core/registers.hpp
@@ -1,5 +1,13 @@
-#ifndef _KIPR_CORE_REGISTERS_HPP_
-#define _KIPR_CORE_REGISTERS_HPP_
+#ifndef KIPR_CORE_REGISTERS_HPP_
+#define KIPR_CORE_REGISTERS_HPP_
+
+#define WALLABY_SPI_VERSION 4
+
+// Author: Joshua Southerland (2015)
+
+#ifndef WALLABY_SPI_R1_H_
+#define WALLABY_SPI_R1_H_
+
#define WALLABY_SPI_VERSION 4
@@ -10,7 +18,6 @@
#define REG_R_VERSION_L 2
-
// READ/Write Registers --------------------------------------------------------
#define REG_RW_DIG_IN_H 3
@@ -36,176 +43,206 @@
#define REG_RW_ADC_5_L 22
#define REG_RW_ADC_PE 23 // low 6 bits used
-#define REG_RW_MAG_X_H 24
-#define REG_RW_MAG_X_L 25
-#define REG_RW_MAG_Y_H 26
-#define REG_RW_MAG_Y_L 27
-#define REG_RW_MAG_Z_H 28
-#define REG_RW_MAG_Z_L 29
-
-#define REG_RW_ACCEL_X_H 30
-#define REG_RW_ACCEL_X_L 31
-#define REG_RW_ACCEL_Y_H 32
-#define REG_RW_ACCEL_Y_L 33
-#define REG_RW_ACCEL_Z_H 34
-#define REG_RW_ACCEL_Z_L 35
-
-#define REG_RW_GYRO_X_H 36
-#define REG_RW_GYRO_X_L 37
-#define REG_RW_GYRO_Y_H 38
-#define REG_RW_GYRO_Y_L 39
-#define REG_RW_GYRO_Z_H 40
-#define REG_RW_GYRO_Z_L 41
+// Magnetometer (Float Values)
+#define REG_RW_MAG_X_0 24
+#define REG_RW_MAG_X_1 25
+#define REG_RW_MAG_X_2 26
+#define REG_RW_MAG_X_3 27
+
+#define REG_RW_MAG_Y_0 28
+#define REG_RW_MAG_Y_1 29
+#define REG_RW_MAG_Y_2 30
+#define REG_RW_MAG_Y_3 31
+
+#define REG_RW_MAG_Z_0 32
+#define REG_RW_MAG_Z_1 33
+#define REG_RW_MAG_Z_2 34
+#define REG_RW_MAG_Z_3 35
+
+// Accelerometer (Float Values)
+#define REG_RW_ACCEL_X_0 36
+#define REG_RW_ACCEL_X_1 37
+#define REG_RW_ACCEL_X_2 38
+#define REG_RW_ACCEL_X_3 39
+
+#define REG_RW_ACCEL_Y_0 40
+#define REG_RW_ACCEL_Y_1 41
+#define REG_RW_ACCEL_Y_2 42
+#define REG_RW_ACCEL_Y_3 43
+
+#define REG_RW_ACCEL_Z_0 44
+#define REG_RW_ACCEL_Z_1 45
+#define REG_RW_ACCEL_Z_2 46
+#define REG_RW_ACCEL_Z_3 47
+
+// Gyroscope (Float Values)
+#define REG_RW_GYRO_X_0 48
+#define REG_RW_GYRO_X_1 49
+#define REG_RW_GYRO_X_2 50
+#define REG_RW_GYRO_X_3 51
+
+#define REG_RW_GYRO_Y_0 52
+#define REG_RW_GYRO_Y_1 53
+#define REG_RW_GYRO_Y_2 54
+#define REG_RW_GYRO_Y_3 55
+
+#define REG_RW_GYRO_Z_0 56
+#define REG_RW_GYRO_Z_1 57
+#define REG_RW_GYRO_Z_2 58
+#define REG_RW_GYRO_Z_3 59
// Motor 0 position
-#define REG_RW_MOT_0_B3 42
-#define REG_RW_MOT_0_B2 43
-#define REG_RW_MOT_0_B1 44
-#define REG_RW_MOT_0_B0 45
+#define REG_RW_MOT_0_B3 63
+#define REG_RW_MOT_0_B2 64
+#define REG_RW_MOT_0_B1 65
+#define REG_RW_MOT_0_B0 66
// Motor 1 position
-#define REG_RW_MOT_1_B3 46
-#define REG_Rw_MOT_1_B2 47
-#define REG_Rw_MOT_1_B1 48
-#define REG_RW_MOT_1_B0 49
+#define REG_RW_MOT_1_B3 67
+#define REG_RW_MOT_1_B2 68
+#define REG_RW_MOT_1_B1 69
+#define REG_RW_MOT_1_B0 70
// Motor 2 position
-#define REG_RW_MOT_2_B3 50
-#define REG_RW_MOT_2_B2 51
-#define REG_RW_MOT_2_B1 52
-#define REG_RW_MOT_2_B0 53
+#define REG_RW_MOT_2_B3 71
+#define REG_RW_MOT_2_B2 72
+#define REG_RW_MOT_2_B1 73
+#define REG_RW_MOT_2_B0 74
// Motor 3 position
-#define REG_RW_MOT_3_B3 54
-#define REG_RW_MOT_3_B2 55
-#define REG_RW_MOT_3_B1 56
-#define REG_RW_MOT_3_B0 57
-
-#define REG_RW_MOT_MODES 58 // Normal PWM, MTP, MAV, MRP, 2 bits per motor
-#define REG_RW_MOT_DIRS 59 // IDLE, FORWARD, REVERSE, BREAK, 2 bits per motor
-#define REG_RW_MOT_DONE 60 // 4 lowest bit used: 0000 (chan0) (chan1) (chan2) (chan3)
-#define REG_RW_MOT_SRV_ALLSTOP 61 // 2nd lowest bit is motor all stop, lowest bit is servo allstop
-
-// 16 bit signed speed goals
-#define REG_RW_MOT_0_SP_H 62
-#define REG_RW_MOT_0_SP_L 63
-#define REG_RW_MOT_1_SP_H 64
-#define REG_RW_MOT_1_SP_L 65
-#define REG_RW_MOT_2_SP_H 66
-#define REG_RW_MOT_2_SP_L 67
-#define REG_RW_MOT_3_SP_H 68
-#define REG_RW_MOT_3_SP_L 69
-
-// 16 bit unsigned pwms, from the user or PID controller
-#define REG_RW_MOT_0_PWM_H 70
-#define REG_RW_MOT_0_PWM_L 71
-#define REG_RW_MOT_1_PWM_H 72
-#define REG_RW_MOT_1_PWM_L 73
-#define REG_RW_MOT_2_PWM_H 74
-#define REG_RW_MOT_2_PWM_L 75
-#define REG_RW_MOT_3_PWM_H 76
-#define REG_RW_MOT_3_PWM_L 77
-
-// 16 bit unsigned servo goals
-// microsecond servo pulse, where 1500 is neutral
-// +/- about 10 per degree from neutral
-#define REG_RW_SERVO_0_H 78
-#define REG_RW_SERVO_0_L 79
-#define REG_RW_SERVO_1_H 80
-#define REG_RW_SERVO_1_L 81
-#define REG_RW_SERVO_2_H 82
-#define REG_RW_SERVO_2_L 83
-#define REG_RW_SERVO_3_H 84
-#define REG_RW_SERVO_3_L 85
-
-// 12 bit unsigned int adc result
-#define REG_RW_BATT_H 86
-#define REG_RW_BATT_L 87
-
-// msb is "extra show", then a non used bit
-// then 6 virtual button bits
-// E x 5 4 3 2 1 0
-#define REG_RW_BUTTONS 88
-
-#define REG_READABLE_COUNT 89
-
-
-
-// WRITE ONLY Registers---------------------------------------------------------
-#define REG_W_PID_0_P_H 89
-#define REG_W_PID_0_P_L 90
-#define REG_W_PID_0_PD_H 91
-#define REG_W_PID_0_PD_L 92
-#define REG_W_PID_0_I_H 93
-#define REG_W_PID_0_I_L 94
-#define REG_W_PID_0_ID_H 95
-#define REG_W_PID_0_ID_L 96
-#define REG_W_PID_0_D_H 97
-#define REG_W_PID_0_D_L 98
-#define REG_W_PID_0_DD_H 99
-#define REG_W_PID_0_DD_L 100
-#define REG_W_PID_1_P_H 101
-#define REG_W_PID_1_P_L 102
-#define REG_W_PID_1_PD_H 103
-#define REG_W_PID_1_PD_L 104
-#define REG_W_PID_1_I_H 105
-#define REG_W_PID_1_I_L 106
-#define REG_W_PID_1_ID_H 107
-#define REG_W_PID_1_ID_L 108
-#define REG_W_PID_1_D_H 119
-#define REG_W_PID_1_D_L 110
-#define REG_W_PID_1_DD_H 111
-#define REG_W_PID_1_DD_L 112
-#define REG_W_PID_2_P_H 113
-#define REG_W_PID_2_P_L 114
-#define REG_W_PID_2_PD_H 115
-#define REG_W_PID_2_PD_L 116
-#define REG_W_PID_2_I_H 117
-#define REG_W_PID_2_I_L 118
-#define REG_W_PID_2_ID_H 119
-#define REG_W_PID_2_ID_L 120
-#define REG_W_PID_2_D_H 121
-#define REG_W_PID_2_D_L 122
-#define REG_W_PID_2_DD_H 123
-#define REG_W_PID_2_DD_L 124
-
-#define REG_W_PID_3_P_H 125
-#define REG_W_PID_3_P_L 126
-#define REG_W_PID_3_PD_H 127
-#define REG_W_PID_3_PD_L 128
-#define REG_W_PID_3_I_H 129
-#define REG_W_PID_3_I_L 130
-#define REG_W_PID_3_ID_H 131
-#define REG_W_PID_3_ID_L 132
-#define REG_W_PID_3_D_H 133
-#define REG_W_PID_3_D_L 134
-#define REG_W_PID_3_DD_H 135
-#define REG_W_PID_3_DD_L 136
-
-// Motor 0 position goal
-#define REG_W_MOT_0_GOAL_B3 137
-#define REG_W_MOT_0_GOAL_B2 138
-#define REG_W_MOT_0_GOAL_B1 139
-#define REG_W_MOT_0_GOAL_B0 140
-
-// Motor 1 position goal
-#define REG_W_MOT_1_GOAL_B3 141
-#define REG_w_MOT_1_GOAL_B2 142
-#define REG_w_MOT_1_GOAL_B1 143
-#define REG_W_MOT_1_GOAL_B0 144
-
-// Motor 2 position goal
-#define REG_W_MOT_2_GOAL_B3 145
-#define REG_W_MOT_2_GOAL_B2 146
-#define REG_W_MOT_2_GOAL_B1 147
-#define REG_W_MOT_2_GOAL_B0 148
-
-// Motor 3 position goal
-#define REG_W_MOT_3_GOAL_B3 149
-#define REG_W_MOT_3_GOAL_B2 150
-#define REG_W_MOT_3_GOAL_B1 151
-#define REG_W_MOT_3_GOAL_B0 152
-
-#define REG_ALL_COUNT 153
-
-#endif
\ No newline at end of file
+#define REG_RW_MOT_3_B3 75
+#define REG_RW_MOT_3_B2 76
+#define REG_RW_MOT_3_B1 77
+#define REG_RW_MOT_3_B0 78
+
+#define REG_RW_MOT_MODES 79
+#define REG_RW_MOT_DIRS 80
+#define REG_RW_MOT_DONE 81
+#define REG_RW_MOT_SRV_ALLSTOP 82
+
+// 16-bit signed speed goals
+#define REG_RW_MOT_0_SP_H 83
+#define REG_RW_MOT_0_SP_L 84
+#define REG_RW_MOT_1_SP_H 85
+#define REG_RW_MOT_1_SP_L 86
+#define REG_RW_MOT_2_SP_H 87
+#define REG_RW_MOT_2_SP_L 88
+#define REG_RW_MOT_3_SP_H 89
+#define REG_RW_MOT_3_SP_L 90
+
+// 16-bit unsigned PWMs (from user or PID controller)
+#define REG_RW_MOT_0_PWM_H 91
+#define REG_RW_MOT_0_PWM_L 92
+#define REG_RW_MOT_1_PWM_H 93
+#define REG_RW_MOT_1_PWM_L 94
+#define REG_RW_MOT_2_PWM_H 95
+#define REG_RW_MOT_2_PWM_L 96
+#define REG_RW_MOT_3_PWM_H 97
+#define REG_RW_MOT_3_PWM_L 98
+
+// 16-bit unsigned servo goals
+#define REG_RW_SERVO_0_H 99
+#define REG_RW_SERVO_0_L 100
+#define REG_RW_SERVO_1_H 101
+#define REG_RW_SERVO_1_L 102
+#define REG_RW_SERVO_2_H 103
+#define REG_RW_SERVO_2_L 104
+#define REG_RW_SERVO_3_H 105
+#define REG_RW_SERVO_3_L 106
+
+// 12-bit unsigned ADC result
+#define REG_RW_BATT_H 107
+#define REG_RW_BATT_L 108
+
+// Virtual button bits
+#define REG_RW_BUTTONS 109
+
+#define REG_READABLE_COUNT 110
+
+// WRITE ONLY Registers ---------------------------------------------------------
+
+// Sensitivity Registers
+#define REG_W_ACCEL_SENSITIVITY 60
+#define REG_W_GYRO_SENSITIVITY 61
+#define REG_W_MAG_SENSITIVITY 62
+
+#define REG_W_PID_0_P_H 111
+#define REG_W_PID_0_P_L 112
+#define REG_W_PID_0_PD_H 113
+#define REG_W_PID_0_PD_L 114
+#define REG_W_PID_0_I_H 115
+#define REG_W_PID_0_I_L 116
+#define REG_W_PID_0_ID_H 117
+#define REG_W_PID_0_ID_L 118
+#define REG_W_PID_0_D_H 119
+#define REG_W_PID_0_D_L 120
+#define REG_W_PID_0_DD_H 121
+#define REG_W_PID_0_DD_L 122
+
+#define REG_W_PID_1_P_H 123
+#define REG_W_PID_1_P_L 124
+#define REG_W_PID_1_PD_H 125
+#define REG_W_PID_1_PD_L 126
+#define REG_W_PID_1_I_H 127
+#define REG_W_PID_1_I_L 128
+#define REG_W_PID_1_ID_H 129
+#define REG_W_PID_1_ID_L 130
+#define REG_W_PID_1_D_H 131
+#define REG_W_PID_1_D_L 132
+#define REG_W_PID_1_DD_H 133
+#define REG_W_PID_1_DD_L 134
+
+#define REG_W_PID_2_P_H 135
+#define REG_W_PID_2_P_L 136
+#define REG_W_PID_2_PD_H 137
+#define REG_W_PID_2_PD_L 138
+#define REG_W_PID_2_I_H 139
+#define REG_W_PID_2_I_L 140
+#define REG_W_PID_2_ID_H 141
+#define REG_W_PID_2_ID_L 142
+#define REG_W_PID_2_D_H 143
+#define REG_W_PID_2_D_L 144
+#define REG_W_PID_2_DD_H 145
+#define REG_W_PID_2_DD_L 146
+
+#define REG_W_PID_3_P_H 147
+#define REG_W_PID_3_P_L 148
+#define REG_W_PID_3_PD_H 149
+#define REG_W_PID_3_PD_L 150
+#define REG_W_PID_3_I_H 151
+#define REG_W_PID_3_I_L 152
+#define REG_W_PID_3_ID_H 153
+#define REG_W_PID_3_ID_L 154
+#define REG_W_PID_3_D_H 155
+#define REG_W_PID_3_D_L 156
+#define REG_W_PID_3_DD_H 157
+#define REG_W_PID_3_DD_L 158
+
+// Motor position goals
+#define REG_W_MOT_0_GOAL_B3 159
+#define REG_W_MOT_0_GOAL_B2 160
+#define REG_W_MOT_0_GOAL_B1 161
+#define REG_W_MOT_0_GOAL_B0 162
+
+#define REG_W_MOT_1_GOAL_B3 163
+#define REG_W_MOT_1_GOAL_B2 164
+#define REG_W_MOT_1_GOAL_B1 165
+#define REG_W_MOT_1_GOAL_B0 166
+
+#define REG_W_MOT_2_GOAL_B3 167
+#define REG_W_MOT_2_GOAL_B2 168
+#define REG_W_MOT_2_GOAL_B1 169
+#define REG_W_MOT_2_GOAL_B0 170
+
+#define REG_W_MOT_3_GOAL_B3 171
+#define REG_W_MOT_3_GOAL_B2 172
+#define REG_W_MOT_3_GOAL_B1 173
+#define REG_W_MOT_3_GOAL_B0 174
+
+#define REG_ALL_COUNT 175
+
+
+#endif // WALLABY_SPI_R1_H_
+
+
+#endif
diff --git a/module/core/src/platform.cpp b/module/core/src/platform.cpp
index d59bafd7..bc686f01 100644
--- a/module/core/src/platform.cpp
+++ b/module/core/src/platform.cpp
@@ -7,6 +7,7 @@
#include "device.hpp"
#include
+#include
#include
@@ -125,7 +126,26 @@ void Platform::writeRegister32b(unsigned char address, unsigned int value)
return DEVICE->w32(address, value);
}
+float Platform::readRegisterFloat(const unsigned char address)
+{
+ if (address >= REG_ALL_COUNT - 3) // Ensure we don't read out of bounds
+ {
+ logger.error() << "Float address " << static_cast(address) << " is invalid";
+ return 0.0f;
+ }
+
+ // Read 4 bytes from the register as a 32-bit integer
+ const uint32_t rawData = DEVICE->r32(address);
+
+ // Convert raw integer data back to float
+ float result;
+ memcpy(&result, &rawData, sizeof(float)); // Safe conversion
+
+ return result;
+}
+
+
void Platform::submit_(const Command *const buffer, const std::size_t size)
{
DEVICE->submit(buffer, size);
-}
\ No newline at end of file
+}
diff --git a/module/gyro/public/kipr/gyro/gyro.h b/module/gyro/public/kipr/gyro/gyro.h
index 6cb33bf2..ced631f8 100644
--- a/module/gyro/public/kipr/gyro/gyro.h
+++ b/module/gyro/public/kipr/gyro/gyro.h
@@ -2,6 +2,7 @@
* \file gyro.h
* \copyright KISS Institute for Practical Robotics
* \defgroup gyro Gyrometer
+ * \authors Tobias Madlberger
*/
#ifndef _KIPR_GYRO_GYRO_H_
@@ -12,36 +13,37 @@ extern "C" {
#endif
/*!
- * Gets the sensed x rotation
- * \return The latest signed x rotation value
- * \description Wallaby: +/- 250 degree/sec range, 1024 per 250 degrees
- * \description Wombat: +/- 250 degree/sec range, 512 per 250 degrees
+ * \brief Gets the sensed x rotation in degrees per second (deg/s) in the NED frame (Side with ports is defined as North)
+ * \description Wombat: +/- 2000 deg/s range
+ * \description Measures rotation around the x-axis in the NED reference frame
+ * \return The latest signed x rotation value in deg/s
* \ingroup gyro
*/
signed short gyro_x();
/*!
- * Gets the sensed y rotation
- * \return The latest signed y rotation value
- * \description Wallaby: +/- 250 degree/sec range, 1024 per 250 degrees
- * \description Wombat: +/- 250 degree/sec range, 512 per 250 degrees
+ * \brief Gets the sensed y rotation in degrees per second (deg/s) in the NED frame (Side with ports is defined as North)
+ * \description Wombat: +/- 2000 deg/s range
+ * \description Measures rotation around the y-axis in the NED reference frame
+ * \return The latest signed y rotation value in deg/s
* \ingroup gyro
*/
signed short gyro_y();
/*!
- * Gets the sensed z rotation
- * \return The latest signed z rotation value
- * \description Wallaby: +/- 250 degree/sec range, 1024 per 250 degrees
- * \description Wombat: +/- 250 degree/sec range, 512 per 250 degrees
+ * \brief Gets the sensed z rotation in degrees per second (deg/s) in the NED frame (Side with ports is defined as North)
+ * \description Wombat: +/- 2000 deg/s range
+ * \description Measures rotation around the z-axis in the NED reference frame
+ * \return The latest signed z rotation value in deg/s
* \ingroup gyro
*/
signed short gyro_z();
/*!
- * \description Calibrates gyroscope
+ * \brief Calibrates gyroscope
* \description Sets a low-pass filter. Put at beginning of your program or before you use gyroscope commands if you want calibrated output.
* \description This function will block for around 500ms taking samples of the gyroscope at standstill.
+ * \return 1: success 0: failure
* \ingroup gyro
*/
int gyro_calibrate();
diff --git a/module/gyro/src/gyro_p.cpp b/module/gyro/src/gyro_p.cpp
index caf41f93..05decb5c 100644
--- a/module/gyro/src/gyro_p.cpp
+++ b/module/gyro/src/gyro_p.cpp
@@ -1,3 +1,8 @@
+/*
+* Modified by: Tobias Madlberger*, 2025
+* * Corresponding author's email: tobias.madlberger@gmail.com
+*/
+
#include "gyro_p.hpp"
#include "kipr/core/platform.hpp"
#include "kipr/core/registers.hpp"
@@ -9,6 +14,7 @@ namespace
int biasx = 0;
int biasy = 0;
int biasz = 0;
+ bool calibrated = false;
}
using namespace kipr;
@@ -16,25 +22,58 @@ using namespace kipr::gyro;
using kipr::core::Platform;
-// Pulls gyroscope data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+/*
+ * Outputs the internal gyro x reading
+ * Pulls gyroscope data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_gyro_x()
+{
+ return Platform::instance()->readRegisterFloat(REG_RW_GYRO_X_0);
+}
+
+/*
+ * Outputs the internal gyro y reading
+ * Pulls gyroscope data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_gyro_y()
+{
+ return Platform::instance()->readRegisterFloat(REG_RW_GYRO_Y_0);
+}
+
+/*
+ * Outputs the internal gyro z reading
+ * Pulls gyroscope data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_gyro_z()
+{
+ return Platform::instance()->readRegisterFloat(REG_RW_GYRO_Z_0);
+}
+
+
+/*
+ * Returns the gyro_x in the NED frame. +x is forward (North). -x is backward (South).
+ */
short kipr::gyro::gyro_x()
{
- return static_cast(Platform::instance()->readRegister16b(REG_RW_GYRO_X_H)) / 16 - biasx;
+ return static_cast(-local_gyro_y());
}
-// Pulls gyroscope data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+/*
+ * Returns the gyro_y in the NED frame. +y is right (East). -y is left (West).
+ */
short kipr::gyro::gyro_y()
{
- return static_cast(Platform::instance()->readRegister16b(REG_RW_GYRO_Y_H)) / 16 - biasy;
+ return static_cast(local_gyro_x());
}
-// Pulls gyroscope data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+/*
+ * Returns the gyro_z in the NED frame. +z is up. -z is down.
+ */
short kipr::gyro::gyro_z()
{
- return static_cast(Platform::instance()->readRegister16b(REG_RW_GYRO_Z_H)) / 16 - biasz;
+ return static_cast(local_gyro_z());
}
-// Simple low-pass filter for gyroscope
bool kipr::gyro::gyro_calibrate()
{
int samples = 50;
@@ -56,12 +95,12 @@ bool kipr::gyro::gyro_calibrate()
biasz = sumZ / samples;
printf("[Gyro Calibrate]: Bias Z: %d | Bias Y: %d | Bias X: %d \n", biasz, biasy, biasx);
- return 0;
+ calibrated = true;
+ return true;
}
// This function is currently unused and meaningless...
bool kipr::gyro::gyro_calibrated()
{
- // TODO
- return true;
+ return calibrated;
}
\ No newline at end of file
diff --git a/module/magneto/public/kipr/magneto/magneto.h b/module/magneto/public/kipr/magneto/magneto.h
index c1e0b2bd..9626fb25 100644
--- a/module/magneto/public/kipr/magneto/magneto.h
+++ b/module/magneto/public/kipr/magneto/magneto.h
@@ -2,6 +2,7 @@
* \file magneto.h
* \copyright KISS Institute for Practical Robotics
* \defgroup magneto Magnetometer
+ * \authors Tobias Madlberger
*/
#ifndef _KIPR_MAGNETO_MAGNETO_H_
@@ -12,22 +13,28 @@ extern "C" {
#endif
/*!
- * Gets the sensed x magneto value
- * \return The latest signed x magneto value
+ * \brief Gets the sensed x magnetometer value in micro Tesla (μT) in the NED frame (Side with ports is defined as North)
+ * \description Measures the magnetic field strength along the x-axis in the NED reference frame
+ * \description Wombat: Used for compass and magnetic field detection
+ * \return The latest signed x magnetometer value in μT
* \ingroup magneto
*/
signed short magneto_x();
/*!
- * Gets the sensed x magneto value
- * \return The latest signed y magneto value
+ * \brief Gets the sensed y magnetometer value in micro Tesla (μT) in the NED frame (Side with ports is defined as North)
+ * \description Measures the magnetic field strength along the y-axis in the NED reference frame
+ * \description Wombat: Used for compass and magnetic field detection
+ * \return The latest signed y magnetometer value in μT
* \ingroup magneto
*/
signed short magneto_y();
/*!
- * Gets the sensed x magneto value
- * \return The latest signed z magneto value
+ * \brief Gets the sensed z magnetometer value in micro Tesla (μT) in the NED frame (Side with ports is defined as North)
+ * \description Measures the magnetic field strength along the z-axis in the NED reference frame
+ * \description Wombat: Used for compass and magnetic field detection
+ * \return The latest signed z magnetometer value in μT
* \ingroup magneto
*/
signed short magneto_z();
@@ -36,4 +43,4 @@ signed short magneto_z();
}
#endif
-#endif
\ No newline at end of file
+#endif
diff --git a/module/magneto/src/magneto_p.cpp b/module/magneto/src/magneto_p.cpp
index 57d29c44..b28e7070 100644
--- a/module/magneto/src/magneto_p.cpp
+++ b/module/magneto/src/magneto_p.cpp
@@ -1,3 +1,8 @@
+/*
+* Modified by: Tobias Madlberger*, 2025
+* * Corresponding author's email: tobias.madlberger@gmail.com
+*/
+
#include "magneto_p.hpp"
#include "kipr/core/platform.hpp"
@@ -8,17 +13,53 @@ using namespace kipr::magneto;
using kipr::core::Platform;
+/*
+ * Outputs the internal magneto x reading
+ * Pulls magnetometer data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_magneto_x()
+{
+ return Platform::instance()->readRegisterFloat(REG_RW_MAG_X_0);
+}
+
+/*
+ * Outputs the internal magneto y reading
+ * Pulls magnetometer data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_magneto_y()
+{
+ return Platform::instance()->readRegisterFloat(REG_RW_MAG_Y_0);
+}
+
+/*
+ * Outputs the internal magneto z reading
+ * Pulls magnetometer data from the I2C registers. The MPU 9250 outputs high and low registers that need to be combined.
+ */
+float local_magneto_z()
+{
+ return Platform::instance()->readRegisterFloat(REG_RW_MAG_Z_0);
+}
+
+/*
+ * Returns the magneto_x in the NED frame. +x is forward (North). -x is backward (South).
+ */
short kipr::magneto::magneto_x()
{
- return static_cast(Platform::instance()->readRegister16b(REG_RW_MAG_X_H))/16;
+ return static_cast(local_magneto_x());
}
+/*
+ * Returns the magneto_y in the NED frame. +y is right (East). -y is left (West).
+ */
short kipr::magneto::magneto_y()
{
- return static_cast(Platform::instance()->readRegister16b(REG_RW_MAG_Y_H))/16;
+ return static_cast(local_magneto_y());
}
+/*
+ * Returns the magneto_z in the NED frame. +z is up. -z is down.
+ */
short kipr::magneto::magneto_z()
{
- return static_cast(Platform::instance()->readRegister16b(REG_RW_MAG_Z_H))/16;
+ return static_cast(-local_magneto_z());
}