diff --git a/blaze-lite/core/lib/IMU/LSM9DS1IMU.cpp b/blaze-lite/core/lib/IMU/LSM9DS1IMU.cpp new file mode 100644 index 0000000..d48545b --- /dev/null +++ b/blaze-lite/core/lib/IMU/LSM9DS1IMU.cpp @@ -0,0 +1,129 @@ +/** + * @file LSM9DS1IMU.cpp + * @brief Implementation of LSM9DS1IMU class + */ + +#include + +#include "LSM9DS1IMU.h" + +LSM9DS1IMU::LSM9DS1IMU(uint8_t accelGyroCsPin, uint8_t magCsPin) + : _lsm9ds1(accelGyroCsPin, magCsPin), + _hasSample(false), + _isReady(false) {} + +LSM9DS1IMU::~LSM9DS1IMU() {} + +bool LSM9DS1IMU::setUp() { + Serial.println("Initializing LSM9DS1 IMU..."); + if (!_lsm9ds1.begin()) { + _isReady = false; + _hasSample = false; + Serial.println("Failed to initialize LSM9DS1 IMU!"); + return false; + } + + _isReady = true; + Serial.println("Setting sensor range"); + _lsm9ds1.setupAccel(_lsm9ds1.LSM9DS1_ACCELRANGE_16G, _lsm9ds1.LSM9DS1_ACCELDATARATE_952HZ); + _lsm9ds1.setupMag(_lsm9ds1.LSM9DS1_MAGGAIN_16GAUSS); + _lsm9ds1.setupGyro(_lsm9ds1.LSM9DS1_GYROSCALE_245DPS); + return true; +} + +void LSM9DS1IMU::pollSensors() { + if (!_isReady) { + _hasSample = false; + return; + } + + _lsm9ds1.read(); + _lsm9ds1.getEvent(&_accel, &_mag, &_gyro, &_temp); + _hasSample = true; +} + +bool LSM9DS1IMU::readAccel(float &x, float &y, float &z) { + if (!_isReady) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + return false; + } + if (!_hasSample) { + pollSensors(); + } + if (!_hasSample) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + return false; + } + + x = _accel.acceleration.x; + y = _accel.acceleration.y; + z = _accel.acceleration.z; + return true; +} + +bool LSM9DS1IMU::readGyro(float &x, float &y, float &z) { + if (!_isReady) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + return false; + } + if (!_hasSample) { + pollSensors(); + } + if (!_hasSample) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + return false; + } + + x = _gyro.gyro.x; + y = _gyro.gyro.y; + z = _gyro.gyro.z; + return true; +} + +bool LSM9DS1IMU::readMag(float &x, float &y, float &z) { + if (!_isReady) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + return false; + } + if (!_hasSample) { + pollSensors(); + } + if (!_hasSample) { + x = 0.0f; + y = 0.0f; + z = 0.0f; + return false; + } + + x = _mag.magnetic.x; + y = _mag.magnetic.y; + z = _mag.magnetic.z; + return true; +} + +bool LSM9DS1IMU::readTemp(float &celsius) { + if (!_isReady) { + celsius = 0.0f; + return false; + } + if (!_hasSample) { + pollSensors(); + } + if (!_hasSample) { + celsius = 0.0f; + return false; + } + + celsius = _temp.temperature; + return true; +} \ No newline at end of file diff --git a/blaze-lite/core/lib/IMU/LSM9DS1IMU.h b/blaze-lite/core/lib/IMU/LSM9DS1IMU.h new file mode 100644 index 0000000..15394cc --- /dev/null +++ b/blaze-lite/core/lib/IMU/LSM9DS1IMU.h @@ -0,0 +1,39 @@ +/** + * @file LSM9DS1IMU.h + * @brief Wrapper class for Adafruit LSM9DS1 IMU using SPI communication + * + * This class provides a simplified interface for interacting with the LSM9DS1 + * IMU using SPI communication. It wraps the Adafruit_LSM9DS1 class + * from the Adafruit LSM9DS1 Library. + */ + +#ifndef LSM9DS1IMU_H +#define LSM9DS1IMU_H + +#include +#include +#include + +class LSM9DS1IMU { +public: + LSM9DS1IMU(uint8_t accelGyroCsPin, uint8_t magCsPin); + ~LSM9DS1IMU(); + + bool setUp(); + void pollSensors(); + bool readAccel(float &x, float &y, float &z); + bool readGyro(float &x, float &y, float &z); + bool readMag(float &x, float &y, float &z); + bool readTemp(float &celsius); + +private: + Adafruit_LSM9DS1 _lsm9ds1; + sensors_event_t _accel; + sensors_event_t _gyro; + sensors_event_t _mag; + sensors_event_t _temp; + bool _hasSample; + bool _isReady; +}; + +#endif \ No newline at end of file diff --git a/blaze-lite/core/platformio.ini b/blaze-lite/core/platformio.ini index bff1ec4..0827907 100644 --- a/blaze-lite/core/platformio.ini +++ b/blaze-lite/core/platformio.ini @@ -22,10 +22,11 @@ build_flags = lib_deps = https://github.com/sparkfun/SparkFun_KX13X_Arduino_Library.git adafruit/Adafruit BMP280 Library - adafruit/Adafruit Unified Sensor adafruit/SdFat - Adafruit Fork@^2.3.102 arduino-libraries/SD@^1.3.0 - mikem/RadioHead@^1.120 + mikem/RadioHead@^1.120 + adafruit/Adafruit LSM9DS1 Library@^2.2.1 + adafruit/Adafruit Unified Sensor@^1.1.15 adafruit/Adafruit SPIFlash@^5.1.1 ; [env:genericSTM32F411CE] diff --git a/blaze-lite/core/src/main.cpp b/blaze-lite/core/src/main.cpp index 046268e..31297b9 100644 --- a/blaze-lite/core/src/main.cpp +++ b/blaze-lite/core/src/main.cpp @@ -23,6 +23,7 @@ #include "sdCard.h" #include "spiFlash.h" #include "dataPacket.h" +#include "LSM9DS1IMU.h" // System libraries #include "SensorData.h" @@ -32,7 +33,6 @@ // Pin Definitions // ============================================================================ - // Radio (RF69 - SPI) #define RADIO_CS_PIN PA4 #define RADIO_INT_PIN PA3 @@ -44,6 +44,13 @@ // SPI Flash (W25Q128 - placeholder, adjust pin as needed) #define SPI_FLASH_CS_PIN PB8 +// Accelerometer (KX134 - SPI) +#define LSM9DS1_XGCS_PIN PA1 //accel/gyro +#define LSM9DS1_MCS_PIN PA2 //magnetometer + +//Accelerometer CS pin +#define KX134_CS_PIN PB2 + // ============================================================================ // SPI Settings // ============================================================================ @@ -59,6 +66,7 @@ // Sensors KX134Accelerometer accelerometer; +LSM9DS1IMU imu(LSM9DS1_XGCS_PIN, LSM9DS1_MCS_PIN); // Communication Radio radio(RADIO_CS_PIN, RADIO_INT_PIN, RADIO_RST_PIN); @@ -126,14 +134,14 @@ void setup() { digitalWrite(SD_CS_PIN, HIGH); pinMode(SPI_FLASH_CS_PIN, OUTPUT); digitalWrite(SPI_FLASH_CS_PIN, HIGH); - pinMode(PB2, OUTPUT); //Accelerometer CS pin - digitalWrite(PB2, HIGH); + pinMode(KX134_CS_PIN, OUTPUT); + digitalWrite(KX134_CS_PIN, HIGH); + pinMode(LSM9DS1_XGCS_PIN, OUTPUT); + digitalWrite(LSM9DS1_XGCS_PIN, HIGH); + pinMode(LSM9DS1_MCS_PIN, OUTPUT); + digitalWrite(LSM9DS1_MCS_PIN, HIGH); pinMode(PA0, OUTPUT); digitalWrite(PA0, HIGH); - pinMode(PA1, OUTPUT); - digitalWrite(PA1, HIGH); - pinMode(PA2, OUTPUT); - digitalWrite(PA2, HIGH); pinMode(PB9, OUTPUT); digitalWrite(PB9, HIGH); @@ -195,6 +203,12 @@ void setup() { spiFlashMem.unmountfs(); } + //initialize IMU + if (!imu.setUp()) { + writeSystemLog("[%lu] ERROR: LSM9DS1 initialization failed!\r\n", millis()); + stateMachine.setError("IMU init failed"); + } + // Initialize State Machine stateMachine.init(); Serial.println("State machine initialized - Starting in UNARMED state"); @@ -711,4 +725,4 @@ void parseRadioCommand(const DecodedPacket& decoded) { Serial.print(decoded.sequenceID); Serial.print(", Timestamp: "); Serial.println(decoded.timestamp); -} +} \ No newline at end of file diff --git a/blaze-lite/core/test/LSM9DS1IMUTest.cpp b/blaze-lite/core/test/LSM9DS1IMUTest.cpp new file mode 100644 index 0000000..ddabeb9 --- /dev/null +++ b/blaze-lite/core/test/LSM9DS1IMUTest.cpp @@ -0,0 +1,85 @@ +#include +#include + +#include "LSM9DS1IMU.h" + +#define LSM9DS1_XGCS_PIN PA1 //accel/gyro +#define LSM9DS1_MCS_PIN PA2 //magnetometer + +#define SPI_SCK_PIN PA5 +#define SPI_MISO_PIN PA6 +#define SPI_MOSI_PIN PA7 + +LSM9DS1IMU imu(LSM9DS1_XGCS_PIN, LSM9DS1_MCS_PIN); + +void setup() { + Serial.begin(9600); + while (!Serial) { + delay(100); + } + delay(1000); // Allow time for serial monitor to connect + + Serial.println("Starting SPI"); + SPI.setSCLK(SPI_SCK_PIN); + SPI.setMISO(SPI_MISO_PIN); + SPI.setMOSI(SPI_MOSI_PIN); + + SPI.begin(); + delay(100); // Allow time for SPI to initialize + Serial.println("Setting up IMU"); + if (!imu.setUp()) { + Serial.println("IMU setup failed"); + } +} + +void loop() { + float ax, ay, az; + float gx, gy, gz; + float mx, my, mz; + float tempC; + + imu.pollSensors(); + + if (imu.readAccel(ax, ay, az)) { + Serial.print("ACC (m/s^2): "); + Serial.print(ax, 4); + Serial.print(", "); + Serial.print(ay, 4); + Serial.print(", "); + Serial.println(az, 4); + } else { + Serial.println("ACC: read failed"); + } + + if (imu.readGyro(gx, gy, gz)) { + Serial.print("GYRO (dps): "); + Serial.print(gx, 4); + Serial.print(", "); + Serial.print(gy, 4); + Serial.print(", "); + Serial.println(gz, 4); + } else { + Serial.println("GYRO: read failed"); + } + + if (imu.readMag(mx, my, mz)) { + Serial.print("MAG (gauss): "); + Serial.print(mx, 4); + Serial.print(", "); + Serial.print(my, 4); + Serial.print(", "); + Serial.println(mz, 4); + } else { + Serial.println("MAG: read failed"); + } + + if (imu.readTemp(tempC)) { + Serial.print("TEMP (C): "); + Serial.println(tempC, 2); + } else { + Serial.println("TEMP: read failed"); + } + + Serial.println("---"); + delay(200); +} diff --git a/blaze-lite/core/test/README.md b/blaze-lite/core/test/README.md deleted file mode 100644 index 12a719a..0000000 --- a/blaze-lite/core/test/README.md +++ /dev/null @@ -1 +0,0 @@ -Example