From dbe5bac427642b3cd3cec031e2c52d9b975f216f Mon Sep 17 00:00:00 2001 From: Porter Fencl Date: Mon, 20 Oct 2025 19:28:52 -0500 Subject: [PATCH 1/5] 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/5] 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/5] 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 From 29450b04d4e0564bd03c64092e52299543610803 Mon Sep 17 00:00:00 2001 From: ThomasDerl <145520479+ThomasDerl@users.noreply.github.com> Date: Tue, 18 Nov 2025 13:49:16 -0600 Subject: [PATCH 4/5] Second version of sensorfusion. Cleaned up files based on proper Ccoding practices --- include/gyro.h | 20 -------- include/sensfuse.h | 51 +++++++++++++++++++ src/gyro.cpp | 41 --------------- src/main.cpp | 121 +++++++-------------------------------------- src/sensfuse.cpp | 99 +++++++++++++++++++++++++++++++++++++ 5 files changed, 168 insertions(+), 164 deletions(-) delete mode 100644 include/gyro.h delete mode 100644 src/gyro.cpp diff --git a/include/gyro.h b/include/gyro.h deleted file mode 100644 index 5601433..0000000 --- a/include/gyro.h +++ /dev/null @@ -1,20 +0,0 @@ -#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/include/sensfuse.h b/include/sensfuse.h index e69de29..27feb0b 100644 --- a/include/sensfuse.h +++ b/include/sensfuse.h @@ -0,0 +1,51 @@ +#ifndef __sensfuse__h__ +#define __sensfuse__h__ +#include +#include +#include +#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 + +#define FUSION_STEP 0.05f // Fusion correct constant + +// begin() function declaration +class SensorFusion { + public: + + SensorFusion(); + + bool begin(); + void update(); + + float getPitch() { + return ptch_accum; + } + + float getRoll() { + return roll_accum; + } + + private: + + Adafruit_ISM330DHCX ism330dhcx; // Create an instance of the gyro/accelerometer class + + unsigned long last_micros; + + float ptch_accum; + float roll_accum; + + float fusion_correct_pitch; + float fusion_correct_roll; + + float gyro_bias_x; + float gyro_bias_y; + +}; + +#endif \ No newline at end of file diff --git a/src/gyro.cpp b/src/gyro.cpp deleted file mode 100644 index 642b3b0..0000000 --- a/src/gyro.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#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 e605c31..552e2b2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,115 +1,30 @@ -#include "gyro.h" +#include "sensfuse.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; +SensorFusion fusion; // Create sensor fusion object void setup() { - Serial.begin(115200); - 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); + Serial.begin(115200); + while (!Serial) + delay(10); + + Serial.println("Adafruit ISM330DHCX test!"); + if (!fusion.begin()) { + Serial.println("IMU initialization failed"); + 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; - - // 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() { - // 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; + fusion.update(); - // 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; + Serial.print("Pitch: "); + Serial.print(fusion.getPitch()); + Serial.print("Roll: "); + Serial.println(fusion.getRoll()); - if (roll_acc_only > roll_accum) - fusion_correct_roll += FUSION_STEP; - else if (roll_acc_only < roll_accum) - fusion_correct_roll -= FUSION_STEP; + delay(100); } diff --git a/src/sensfuse.cpp b/src/sensfuse.cpp index e69de29..f96d54d 100644 --- a/src/sensfuse.cpp +++ b/src/sensfuse.cpp @@ -0,0 +1,99 @@ +#include "sensfuse.h" + +SensorFusion::SensorFusion() + : last_micros(0), + ptch_accum(0), + roll_accum(0), + fusion_correct_pitch(0), + fusion_correct_roll(0), + gyro_bias_x(0), + gyro_bias_y(0) +{ + +} + +// begin function for SPI initialization +bool SensorFusion::begin() { + 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; + + // 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 + + return true; +} + +// Make functions for gyro, accelerometer calculations +// Perform function calls in main +// Main should be clean, only using function calls and tests. +void SensorFusion::update() { + // 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; + } +} \ No newline at end of file From a27179b4bce16d8e31faca57afa9d87b9c20db9b Mon Sep 17 00:00:00 2001 From: ThomasDerl <145520479+ThomasDerl@users.noreply.github.com> Date: Mon, 1 Dec 2025 19:58:04 -0600 Subject: [PATCH 5/5] Moved getPitch() and getRoll() definitions to .cpp --- include/sensfuse.h | 41 ++++++++++++------------------- src/sensfuse.cpp | 61 +++++++++++++++------------------------------- 2 files changed, 36 insertions(+), 66 deletions(-) diff --git a/include/sensfuse.h b/include/sensfuse.h index 27feb0b..3eeb6df 100644 --- a/include/sensfuse.h +++ b/include/sensfuse.h @@ -14,38 +14,29 @@ #define FUSION_STEP 0.05f // Fusion correct constant -// begin() function declaration class SensorFusion { - public: +public: + SensorFusion(); - SensorFusion(); + bool begin(); + void update(); - bool begin(); - void update(); + float getPitch(); + float getRoll(); - float getPitch() { - return ptch_accum; - } +private: + Adafruit_ISM330DHCX ism330dhcx; - float getRoll() { - return roll_accum; - } + unsigned long last_micros; - private: + float ptch_accum; + float roll_accum; - Adafruit_ISM330DHCX ism330dhcx; // Create an instance of the gyro/accelerometer class - - unsigned long last_micros; - - float ptch_accum; - float roll_accum; - - float fusion_correct_pitch; - float fusion_correct_roll; - - float gyro_bias_x; - float gyro_bias_y; + float fusion_correct_pitch; + float fusion_correct_roll; + float gyro_bias_x; + float gyro_bias_y; }; -#endif \ No newline at end of file +#endif \ No newline at end of file diff --git a/src/sensfuse.cpp b/src/sensfuse.cpp index f96d54d..b18d5fe 100644 --- a/src/sensfuse.cpp +++ b/src/sensfuse.cpp @@ -9,20 +9,14 @@ SensorFusion::SensorFusion() gyro_bias_x(0), gyro_bias_y(0) { - } -// begin function for SPI initialization bool SensorFusion::begin() { if (!ism330dhcx.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { Serial.println("Failed to find ISM330DHCX chip1"); - while (1) { - delay(10); - } + 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; @@ -32,68 +26,53 @@ bool SensorFusion::begin() { delay(2); } - // Calculate average bias gyro_bias_x = sumX / 500.0f; gyro_bias_y = sumY / 500.0f; - // 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 + ism330dhcx.configInt1(false, false, true); + ism330dhcx.configInt2(false, true, false); return true; } -// Make functions for gyro, accelerometer calculations -// Perform function calls in main -// Main should be clean, only using function calls and tests. void SensorFusion::update() { - // 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 + unsigned long now = micros(); + float dt = (now - last_micros) * 1e-6f; + last_micros = now; - // 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 + ism330dhcx.readGyroscope(gx, gy, gz); + ism330dhcx.readAcceleration(ax, ay, az); - // 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 (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; - } + if (roll_acc_only > roll_accum) fusion_correct_roll += FUSION_STEP; + else if (roll_acc_only < roll_accum) fusion_correct_roll -= FUSION_STEP; +} + +float SensorFusion::getPitch() { + return ptch_accum; +} + +float SensorFusion::getRoll() { + return roll_accum; } \ No newline at end of file