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/sensfuse.h b/include/sensfuse.h new file mode 100644 index 0000000..27feb0b --- /dev/null +++ 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/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 cb9fbba..552e2b2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,18 +1,30 @@ -#include +#include "sensfuse.h" +#include -// put function declarations here: -int myFunction(int, int); +SensorFusion fusion; // Create sensor fusion object void setup() { - // put your setup code here, to run once: - int result = myFunction(2, 3); + Serial.begin(115200); + while (!Serial) + delay(10); + + Serial.println("Adafruit ISM330DHCX test!"); + if (!fusion.begin()) { + Serial.println("IMU initialization failed"); + while (1) { + delay(10); + } + } } void loop() { - // put your main code here, to run repeatedly: + fusion.update(); + + Serial.print("Pitch: "); + Serial.print(fusion.getPitch()); + Serial.print("Roll: "); + Serial.println(fusion.getRoll()); + + delay(100); } -// put function definitions here: -int myFunction(int x, int y) { - return x + y; -} \ No newline at end of file diff --git a/src/sensfuse.cpp b/src/sensfuse.cpp new file mode 100644 index 0000000..f96d54d --- /dev/null +++ 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