Feature/imu lsm9ds1

blaze-lite/core/lib/IMU/LSM9DS1IMU.cpp

@@ -0,0 +1,129 @@
+/**
+ * @file LSM9DS1IMU.cpp
+ * @brief Implementation of LSM9DS1IMU class
+ */
+
+#include <Arduino.h>
+
+#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;
+}

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 <SPI.h>
+#include <Adafruit_LSM9DS1.h>
+#include <Adafruit_Sensor.h>
+
+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

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]

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);
-}
+}

blaze-lite/core/test/LSM9DS1IMUTest.cpp

@@ -0,0 +1,85 @@
+#include <Arduino.h>
+#include <SPI.h>
+
+#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);
+}