From dbe5bac427642b3cd3cec031e2c52d9b975f216f Mon Sep 17 00:00:00 2001 From: Porter Fencl Date: Mon, 20 Oct 2025 19:28:52 -0500 Subject: [PATCH 1/3] initial design --- include/gyro.h | 22 ++++++++++++++++++++++ src/gyro.cpp | 41 +++++++++++++++++++++++++++++++++++++++++ src/main.cpp | 22 ++++++++++++---------- 3 files changed, 75 insertions(+), 10 deletions(-) create mode 100644 include/gyro.h create mode 100644 src/gyro.cpp diff --git a/include/gyro.h b/include/gyro.h new file mode 100644 index 0000000..9c34626 --- /dev/null +++ b/include/gyro.h @@ -0,0 +1,22 @@ +#ifndef __gyro__h__ +#define __gyro__h__ +#include +#include +#include + +#define LSM6DS3_OUTX_L_G 0X22 //gyroscope +#define LSM6DS3_OUTX_L_XL 0x28 //accelerometer +#define LSM6DS3_OUT_TEMP_L 0x20 //temperature + +class LSM6DS3 { + public: + void readRegisters(uint8_t address, uint8_t* data, size_t length); + + void readGyroData(float* x, float* y, float* z); + void readAccelData(float* x, float* y, float* z); + void readTempData(float* t); +}; + + + +#endif \ No newline at end of file diff --git a/src/gyro.cpp b/src/gyro.cpp new file mode 100644 index 0000000..642b3b0 --- /dev/null +++ b/src/gyro.cpp @@ -0,0 +1,41 @@ +#include "gyro.h" + +void LSM6DS3::readRegisters(uint8_t address, uint8_t * data, size_t length){ + + SPI.beginTransaction(SPISettings(14000000, MSBFIRST, SPI_MODE0)); + SPI.transfer(address | 0x80); + SPI.transfer(data,length); + SPI.endTransaction(); + +} + +void LSM6DS3::readGyroData(float* x, float* y, float* z){ + + int16_t data[3]; + readRegisters(LSM6DS3_OUTX_L_G, (uint8_t*)data, sizeof(data)); + + *x = data[0] * 2000.0 / 32768.0; + *y = data[1] * 2000.0 / 32768.0; + *z = data[2] * 2000.0 / 32768.0; + +} + +void LSM6DS3::readAccelData(float* x, float* y, float* z){ + + int16_t data[3]; + readRegisters(LSM6DS3_OUTX_L_XL, (uint8_t*)data, sizeof(data)); + + *x = data[0] * 4.0 / 32768.0; + *y = data[1] * 4.0 / 32768.0; + *z = data[2] * 4.0 / 32768.0; + +} + +void LSM6DS3::readTempData(float* t){ + + int16_t data[1]; + + readRegisters(LSM6DS3_OUT_TEMP_L, (uint8_t*)data, sizeof(data)); + *t = data[0] / 16.0 + 2; + +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index cb9fbba..547f7ec 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,18 +1,20 @@ -#include +#include "gyro.h" + -// put function declarations here: -int myFunction(int, int); void setup() { - // put your setup code here, to run once: - int result = myFunction(2, 3); + SPI.begin(); + Serial.begin(115200); + Wire.begin(); + + float x; + float y; + float z; } void loop() { - // put your main code here, to run repeatedly: + LSM6DS3.readGyroData(x, y, z); + Serial.println(); + } -// put function definitions here: -int myFunction(int x, int y) { - return x + y; -} \ No newline at end of file From 120b71c0f803f6436e2d5ea1b56d2997e75fa0f7 Mon Sep 17 00:00:00 2001 From: Porter Fencl Date: Mon, 20 Oct 2025 19:29:06 -0500 Subject: [PATCH 2/3] fixed errors --- src/main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 547f7ec..3994cf1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,8 +13,8 @@ void setup() { } void loop() { - LSM6DS3.readGyroData(x, y, z); - Serial.println(); + // LSM6DS3.readGyroData(x, y, z); + // Serial.println(); } From 777531d36ae728c92bf134f01521f865a52697e0 Mon Sep 17 00:00:00 2001 From: ThomasDerl <145520479+ThomasDerl@users.noreply.github.com> Date: Mon, 10 Nov 2025 19:40:45 -0600 Subject: [PATCH 3/3] Initial version of sensor fusion --- .vscode/settings.json | 12 +++++ include/gyro.h | 2 - include/sensfuse.h | 0 platformio.ini | 1 + src/main.cpp | 109 +++++++++++++++++++++++++++++++++++++++--- src/sensfuse.cpp | 0 6 files changed, 115 insertions(+), 9 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 include/sensfuse.h create mode 100644 src/sensfuse.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..9618221 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "C_Cpp.errorSquiggles": "disabled", + "files.associations": { + "array": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "string_view": "cpp", + "initializer_list": "cpp" + } +} \ No newline at end of file diff --git a/include/gyro.h b/include/gyro.h index 9c34626..5601433 100644 --- a/include/gyro.h +++ b/include/gyro.h @@ -17,6 +17,4 @@ class LSM6DS3 { void readTempData(float* t); }; - - #endif \ No newline at end of file diff --git a/include/sensfuse.h b/include/sensfuse.h new file mode 100644 index 0000000..e69de29 diff --git a/platformio.ini b/platformio.ini index 345e039..b60e17d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -13,3 +13,4 @@ platform = ststm32 board = nucleo_l432kc framework = arduino lib_extra_dirs = ./embedded-pio +lib_deps = adafruit/Adafruit LSM6DS@^4.7.4 diff --git a/src/main.cpp b/src/main.cpp index 3994cf1..e605c31 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,20 +1,115 @@ #include "gyro.h" +#include +// For SPI mode we need a CS pin +#define LSM_CS PA4 +// For software-SPI mode we need SCK/MOSI/MISO pins +#define LSM_SCK D13 +#define LSM_MISO D12 +#define LSM_MOSI D11 +Adafruit_ISM330DHCX ism330dhcx; // Create an instance of the gyro/accelerometer class + +// Variables to store the running angles +float ptch_accum = 0.0f; +float roll_accum = 0.0f; + +// Fusion correct terms, used to track how much to adjust gyro integration with the accelerometer +float fusion_correct_pitch = 0.0f; +float fusion_correct_roll = 0.0f; + +// Variables to store the small error adjustments for gyro bias drift +float gyro_bias_x = 0.0f; +float gyro_bias_y = 0.0f; +float gyro_drift_x = 0.0f; // Drift values to be used later if need be +float gyro_drift_y = 0.0f; + +unsigned long last_micros = 0; // Tracks time between loop cycles + +// Fusion correct constant +const float FUSION_STEP = 0.05f; void setup() { - SPI.begin(); Serial.begin(115200); - Wire.begin(); + while (!Serial) + delay(10); // will pause Zero, Leonardo, etc until serial console opens + + Serial.println("Adafruit ISM330DHCX test!"); + + if (!ism330dhcx.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { + Serial.println("Failed to find ISM330DHCX chip1"); + while (1) { + delay(10); + } + } + + // Calibrate gyro bias while the board/car is not moving + // Take readings to find the average bias error + float sumX = 0.0f, sumY = 0.0f; + for (int i = 0; i < 500; i++) { + float gx, gy, gz; + ism330dhcx.readGyroscope(&gx, &gy, &gz); + sumX += gx; + sumY += gy; + delay(2); + } + + // Calculate average bias + gyro_bias_x = sumX / 500.0f; + gyro_bias_y = sumY / 500.0f; - float x; - float y; - float z; + // Read in accelerometer data for initial angle calculation + float ax, ay, az; + ism330dhcx.readAcceleration(&ax, &ay, &az); + + // Figure out the current tilt of the board (in degrees) + // atan2 gives the angle in radians; multiply by 57.2958 (180/pi) to turn it into degrees + ptch_accum = atan2f(-ax, sqrtf(ay * ay + az * az)) * 57.2958f; + roll_accum = atan2f(ay, az) * 57.2958f; + + // Store the start time to calculate time difference next loop + last_micros = micros(); + + ism330dhcx.configInt1(false, false, true); // accelerometer DRDY on INT1 + ism330dhcx.configInt2(false, true, false); // gyro DRDY on INT2 } void loop() { - // LSM6DS3.readGyroData(x, y, z); - // Serial.println(); + // Calculate time step (dt) in seconds for integration + unsigned long now = micros(); // Current time in microseconds + float dt = (now - last_micros) * 1e-6f; // Convert microseconds to seconds + last_micros = now; // Update last_micros for next loop + + // Read sensor data + float gx, gy, gz; + float ax, ay, az; + ism330dhcx.readGyroscope(&gx, &gy, &gz); // Read in the gyro data + ism330dhcx.readAcceleration(&ax, &ay, &az); // Read in the accelerometer data + + // Gyro bias correction + gx -= gyro_bias_x; + gy -= gyro_bias_y; + + // Integrate gyro data to get running angles + // gx/y * dt = change in angle since last loop + ptch_accum += (gx * dt) + fusion_correct_pitch; + roll_accum += (gy * dt) + fusion_correct_roll; + + // Compute accelerometer-based angles (gravity reference) + float ptch_acc_only = atan2f(-ax, sqrtf(ay * ay + az * az)) * 57.2958f; + float roll_acc_only = atan2f(ay, az) * 57.2958f; + + // Sensor fusion correction loops + // If the accelerometer says the angle should be bigger, bump fusion up. + // If it says smaller, bump it down. This slowly keeps gyro drift in check. + if (ptch_acc_only > ptch_accum) + fusion_correct_pitch += FUSION_STEP; + else if (ptch_acc_only < ptch_accum) + fusion_correct_pitch -= FUSION_STEP; + if (roll_acc_only > roll_accum) + fusion_correct_roll += FUSION_STEP; + else if (roll_acc_only < roll_accum) + fusion_correct_roll -= FUSION_STEP; } diff --git a/src/sensfuse.cpp b/src/sensfuse.cpp new file mode 100644 index 0000000..e69de29