diff --git a/IMU-BLE-Communication/BLE_IMU_comms.ino/BLE_IMU_comms.ino.ino b/1IMU-BLE-Communication/BLE_IMU_comms.ino/BLE_IMU_comms.ino.ino similarity index 100% rename from IMU-BLE-Communication/BLE_IMU_comms.ino/BLE_IMU_comms.ino.ino rename to 1IMU-BLE-Communication/BLE_IMU_comms.ino/BLE_IMU_comms.ino.ino diff --git a/BLE-Communication/client.py b/2BLE-Serial-Communication/client.py similarity index 100% rename from BLE-Communication/client.py rename to 2BLE-Serial-Communication/client.py diff --git a/BLE-Communication/server/server.ino b/2BLE-Serial-Communication/server/main.cpp similarity index 52% rename from BLE-Communication/server/server.ino rename to 2BLE-Serial-Communication/server/main.cpp index 4945a10..f61414c 100644 --- a/BLE-Communication/server/server.ino +++ b/2BLE-Serial-Communication/server/main.cpp @@ -1,8 +1,24 @@ +#include +#include +#include +#include +#include // not used in this demo but required! #include #include #include #include + +// This part is taken from +Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(); + +#define LSM9DS1_SCK A5 +#define LSM9DS1_MISO 12 +#define LSM9DS1_MOSI A4 +#define LSM9DS1_XGCS 6 +#define LSM9DS1_MCS 5 + +// BLE part BLEServer* pServer = NULL; BLECharacteristic* pSensorCharacteristic = NULL; BLECharacteristic* pLedCharacteristic = NULL; @@ -10,15 +26,15 @@ bool deviceConnected = false; bool oldDeviceConnected = false; uint32_t value = 0; -const int ledPin = 2; // Use the appropriate GPIO pin for your setup - -// See the following for generating UUIDs: -// https://www.uuidgenerator.net/ +const int ledPin = 2; //This is the internal LED; used to demonstrate bidirectional communication #define SERVICE_UUID "19b10000-e8f2-537e-4f6c-d104768a1214" #define SENSOR_CHARACTERISTIC_UUID "19b10001-e8f2-537e-4f6c-d104768a1214" #define LED_CHARACTERISTIC_UUID "19b10002-e8f2-537e-4f6c-d104768a1214" +// Will need to provide a more detailed description in your bachelor thesis, but the UUIDs +// generally describe what you use a feature for - e.g. weather data collection, computer mouse etc. + class MyServerCallbacks: public BLEServerCallbacks { void onConnect(BLEServer* pServer) { deviceConnected = true; @@ -46,10 +62,57 @@ class MyCharacteristicCallbacks : public BLECharacteristicCallbacks { } }; -void setup() { + +void setupSensor() +{ + // 1.) Set the accelerometer range + lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ); + //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ); + + // 2.) Set the magnetometer sensitivity + lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS); + //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS); + + // 3.) Setup the gyroscope + lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS); + //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS); + //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS); +} + + +void setup() +{ + // General settings Serial.begin(115200); pinMode(ledPin, OUTPUT); + + // LSM9DS1 Setup + + while (!Serial) { + delay(1); // will pause Zero, Leonardo, etc until serial console opens + } + + Serial.println("LSM9DS1 data read demo"); + + // Try to initialise and warn if we couldn't detect the chip + if (!lsm.begin()) + { + Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!"); + while (1); + } + Serial.println("Found LSM9DS1 9DOF"); + + // helper to just set the default scaling we want, see above! + setupSensor(); + + + // Bluetooth part + // Create the BLE Device BLEDevice::init("ESP32"); @@ -95,12 +158,43 @@ void setup() { Serial.println("Waiting a client connection to notify..."); } -void loop() { +void loop() +{ + lsm.read(); /* ask it to read in the data */ + + /* Get a new sensor event */ + sensors_event_t a, m, g, temp; + + lsm.getEvent(&a, &m, &g, &temp); + + String test_str = "Accel X: " + String(a.acceleration.x) + " m/s^2" + + "\tY: " + String(a.acceleration.y) + " m/s^2 " + + "\tZ: " + String(a.acceleration.z) + " m/s^2\n" + + "Mag X: " + String(m.magnetic.x) + " uT" + + "\tY: " + String(m.magnetic.y) + " uT" + + "\tZ: " + String(m.magnetic.z) + " uT\n" + + "Gyro X: " + String(g.gyro.x) + " rad/s" + + "\tY: " + String(g.gyro.y) + " rad/s" + + "\tZ: " + String(g.gyro.z) + " rad/s\n"; + + Serial.print("Accel X: "); Serial.print(a.acceleration.x); Serial.print(" m/s^2"); + Serial.print("\tY: "); Serial.print(a.acceleration.y); Serial.print(" m/s^2 "); + Serial.print("\tZ: "); Serial.print(a.acceleration.z); Serial.println(" m/s^2 "); + + Serial.print("Mag X: "); Serial.print(m.magnetic.x); Serial.print(" uT"); + Serial.print("\tY: "); Serial.print(m.magnetic.y); Serial.print(" uT"); + Serial.print("\tZ: "); Serial.print(m.magnetic.z); Serial.println(" uT"); + + Serial.print("Gyro X: "); Serial.print(g.gyro.x); Serial.print(" rad/s"); + Serial.print("\tY: "); Serial.print(g.gyro.y); Serial.print(" rad/s"); + Serial.print("\tZ: "); Serial.print(g.gyro.z); Serial.println(" rad/s"); + + Serial.println(); // notify changed value if (deviceConnected) { - pSensorCharacteristic->setValue(String(value).c_str()); + pSensorCharacteristic->setValue(test_str.c_str()); pSensorCharacteristic->notify(); - value++; + //value++; Serial.print("New value notified: "); Serial.println(value); delay(100); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms @@ -119,4 +213,5 @@ void loop() { oldDeviceConnected = deviceConnected; Serial.println("Device Connected"); } -} + +} \ No newline at end of file diff --git a/3 Sensor fusion/main.cpp b/3 Sensor fusion/main.cpp new file mode 100644 index 0000000..ca1ab55 --- /dev/null +++ b/3 Sensor fusion/main.cpp @@ -0,0 +1,165 @@ +// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF +// sensor sets. +// You *must* perform a magnetic calibration before this code will work. +// +// To view this data, use the Arduino Serial Monitor to watch the +// scrolling angles, or run the OrientationVisualiser example in Processing. +// Based on https://github.com/PaulStoffregen/NXPMotionSense with adjustments +// to Adafruit Unified Sensor interface + +#include +#include + +Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer; + +#include +Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1(); + +bool init_sensors(void) { + if (!lsm9ds.begin()) { + return false; + } + accelerometer = &lsm9ds.getAccel(); + gyroscope = &lsm9ds.getGyro(); + magnetometer = &lsm9ds.getMag(); + + return true; +} + +void setup_sensors(void) { + // set lowest range +#ifdef __LSM9DS0_H__ + lsm9ds.setupAccel(lsm9ds.LSM9DS0_ACCELRANGE_2G); + lsm9ds.setupMag(lsm9ds.LSM9DS0_MAGGAIN_4GAUSS); + lsm9ds.setupGyro(lsm9ds.LSM9DS0_GYROSCALE_245DPS); +#else + lsm9ds.setupAccel(lsm9ds.LSM9DS1_ACCELRANGE_2G); + lsm9ds.setupMag(lsm9ds.LSM9DS1_MAGGAIN_4GAUSS); + lsm9ds.setupGyro(lsm9ds.LSM9DS1_GYROSCALE_245DPS); +#endif +} +// pick your filter! slower == better quality output +//Adafruit_NXPSensorFusion filter; // slowest +Adafruit_Madgwick filter; // faster than NXP +//Adafruit_Mahony filter; // fastest/smalleset + +#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM) + Adafruit_Sensor_Calibration_EEPROM cal; +#else + Adafruit_Sensor_Calibration_SDFat cal; +#endif + +#define FILTER_UPDATE_RATE_HZ 100 +#define PRINT_EVERY_N_UPDATES 10 +//#define AHRS_DEBUG_OUTPUT + +uint32_t timestamp; + +void setup() { + Serial.begin(115200); + while (!Serial) yield(); + + if (!cal.begin()) { + Serial.println("Failed to initialize calibration helper"); + } else if (! cal.loadCalibration()) { + Serial.println("No calibration loaded/found"); + } + + if (!init_sensors()) { + Serial.println("Failed to find sensors"); + while (1) delay(10); + } + + accelerometer->printSensorDetails(); + gyroscope->printSensorDetails(); + magnetometer->printSensorDetails(); + + setup_sensors(); + filter.begin(FILTER_UPDATE_RATE_HZ); + timestamp = millis(); + + Wire.setClock(400000); // 400KHz +} + + +void loop() { + float roll, pitch, heading; + float gx, gy, gz; + static uint8_t counter = 0; + + if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) { + return; + } + timestamp = millis(); + // Read the motion sensors + sensors_event_t accel, gyro, mag; + accelerometer->getEvent(&accel); + gyroscope->getEvent(&gyro); + magnetometer->getEvent(&mag); +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms"); +#endif + + cal.calibrate(mag); + cal.calibrate(accel); + cal.calibrate(gyro); + // Gyroscope needs to be converted from Rad/s to Degree/s + // the rest are not unit-important + gx = gyro.gyro.x * SENSORS_RADS_TO_DPS; + gy = gyro.gyro.y * SENSORS_RADS_TO_DPS; + gz = gyro.gyro.z * SENSORS_RADS_TO_DPS; + + // Update the SensorFusion filter + filter.update(gx, gy, gz, + accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, + mag.magnetic.x, mag.magnetic.y, mag.magnetic.z); +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms"); +#endif + + // only print the calculated output once in a while + if (counter++ <= PRINT_EVERY_N_UPDATES) { + return; + } + // reset the counter + counter = 0; + +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("Raw: "); + Serial.print(accel.acceleration.x, 4); Serial.print(", "); + Serial.print(accel.acceleration.y, 4); Serial.print(", "); + Serial.print(accel.acceleration.z, 4); Serial.print(", "); + Serial.print(gx, 4); Serial.print(", "); + Serial.print(gy, 4); Serial.print(", "); + Serial.print(gz, 4); Serial.print(", "); + Serial.print(mag.magnetic.x, 4); Serial.print(", "); + Serial.print(mag.magnetic.y, 4); Serial.print(", "); + Serial.print(mag.magnetic.z, 4); Serial.println(""); +#endif + + // print the heading, pitch and roll + roll = filter.getRoll(); + pitch = filter.getPitch(); + heading = filter.getYaw(); + Serial.print("Orientation: "); + Serial.print(heading); + Serial.print(", "); + Serial.print(pitch); + Serial.print(", "); + Serial.println(roll); + + float qw, qx, qy, qz; + filter.getQuaternion(&qw, &qx, &qy, &qz); + Serial.print("Quaternion: "); + Serial.print(qw, 4); + Serial.print(", "); + Serial.print(qx, 4); + Serial.print(", "); + Serial.print(qy, 4); + Serial.print(", "); + Serial.println(qz, 4); + +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms"); +#endif +} \ No newline at end of file diff --git a/Ringinator/platformio.ini b/Ringinator/platformio.ini index b8574fc..f1012b5 100644 --- a/Ringinator/platformio.ini +++ b/Ringinator/platformio.ini @@ -16,4 +16,6 @@ board_build.f_cpu = 240000000L upload_protocol = esptool framework = arduino monitor_speed = 115200 -lib_deps = adafruit/Adafruit LSM9DS1 Library@^2.2.1 +lib_deps = + adafruit/Adafruit LSM9DS1 Library@^2.2.1 + adafruit/Adafruit AHRS@^2.3.6 diff --git a/Ringinator/src/main.cpp b/Ringinator/src/main.cpp index f61414c..ca1ab55 100644 --- a/Ringinator/src/main.cpp +++ b/Ringinator/src/main.cpp @@ -1,217 +1,165 @@ -#include -#include -#include +// Full orientation sensing using NXP/Madgwick/Mahony and a range of 9-DoF +// sensor sets. +// You *must* perform a magnetic calibration before this code will work. +// +// To view this data, use the Arduino Serial Monitor to watch the +// scrolling angles, or run the OrientationVisualiser example in Processing. +// Based on https://github.com/PaulStoffregen/NXPMotionSense with adjustments +// to Adafruit Unified Sensor interface + +#include +#include + +Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer; + #include -#include // not used in this demo but required! -#include -#include -#include -#include +Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1(); +bool init_sensors(void) { + if (!lsm9ds.begin()) { + return false; + } + accelerometer = &lsm9ds.getAccel(); + gyroscope = &lsm9ds.getGyro(); + magnetometer = &lsm9ds.getMag(); -// This part is taken from -Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(); - -#define LSM9DS1_SCK A5 -#define LSM9DS1_MISO 12 -#define LSM9DS1_MOSI A4 -#define LSM9DS1_XGCS 6 -#define LSM9DS1_MCS 5 - -// BLE part -BLEServer* pServer = NULL; -BLECharacteristic* pSensorCharacteristic = NULL; -BLECharacteristic* pLedCharacteristic = NULL; -bool deviceConnected = false; -bool oldDeviceConnected = false; -uint32_t value = 0; - -const int ledPin = 2; //This is the internal LED; used to demonstrate bidirectional communication - -#define SERVICE_UUID "19b10000-e8f2-537e-4f6c-d104768a1214" -#define SENSOR_CHARACTERISTIC_UUID "19b10001-e8f2-537e-4f6c-d104768a1214" -#define LED_CHARACTERISTIC_UUID "19b10002-e8f2-537e-4f6c-d104768a1214" - -// Will need to provide a more detailed description in your bachelor thesis, but the UUIDs -// generally describe what you use a feature for - e.g. weather data collection, computer mouse etc. - -class MyServerCallbacks: public BLEServerCallbacks { - void onConnect(BLEServer* pServer) { - deviceConnected = true; - }; - - void onDisconnect(BLEServer* pServer) { - deviceConnected = false; - } -}; - -class MyCharacteristicCallbacks : public BLECharacteristicCallbacks { - void onWrite(BLECharacteristic* pLedCharacteristic) { - std::string value = pLedCharacteristic->getValue(); - if (value.length() > 0) { - Serial.print("Characteristic event, written: "); - Serial.println(static_cast(value[0])); // Print the integer value - - int receivedValue = static_cast(value[0]); - if (receivedValue == 1) { - digitalWrite(ledPin, HIGH); - } else { - digitalWrite(ledPin, LOW); - } - } - } -}; - - -void setupSensor() -{ - // 1.) Set the accelerometer range - lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ); - //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ); - //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ); - //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ); - - // 2.) Set the magnetometer sensitivity - lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS); - //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS); - //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS); - //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS); - - // 3.) Setup the gyroscope - lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS); - //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS); - //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS); + return true; } +void setup_sensors(void) { + // set lowest range +#ifdef __LSM9DS0_H__ + lsm9ds.setupAccel(lsm9ds.LSM9DS0_ACCELRANGE_2G); + lsm9ds.setupMag(lsm9ds.LSM9DS0_MAGGAIN_4GAUSS); + lsm9ds.setupGyro(lsm9ds.LSM9DS0_GYROSCALE_245DPS); +#else + lsm9ds.setupAccel(lsm9ds.LSM9DS1_ACCELRANGE_2G); + lsm9ds.setupMag(lsm9ds.LSM9DS1_MAGGAIN_4GAUSS); + lsm9ds.setupGyro(lsm9ds.LSM9DS1_GYROSCALE_245DPS); +#endif +} +// pick your filter! slower == better quality output +//Adafruit_NXPSensorFusion filter; // slowest +Adafruit_Madgwick filter; // faster than NXP +//Adafruit_Mahony filter; // fastest/smalleset -void setup() -{ - // General settings +#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM) + Adafruit_Sensor_Calibration_EEPROM cal; +#else + Adafruit_Sensor_Calibration_SDFat cal; +#endif + +#define FILTER_UPDATE_RATE_HZ 100 +#define PRINT_EVERY_N_UPDATES 10 +//#define AHRS_DEBUG_OUTPUT + +uint32_t timestamp; + +void setup() { Serial.begin(115200); - pinMode(ledPin, OUTPUT); + while (!Serial) yield(); + if (!cal.begin()) { + Serial.println("Failed to initialize calibration helper"); + } else if (! cal.loadCalibration()) { + Serial.println("No calibration loaded/found"); + } - // LSM9DS1 Setup - - while (!Serial) { - delay(1); // will pause Zero, Leonardo, etc until serial console opens + if (!init_sensors()) { + Serial.println("Failed to find sensors"); + while (1) delay(10); } - Serial.println("LSM9DS1 data read demo"); - - // Try to initialise and warn if we couldn't detect the chip - if (!lsm.begin()) - { - Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!"); - while (1); - } - Serial.println("Found LSM9DS1 9DOF"); + accelerometer->printSensorDetails(); + gyroscope->printSensorDetails(); + magnetometer->printSensorDetails(); - // helper to just set the default scaling we want, see above! - setupSensor(); + setup_sensors(); + filter.begin(FILTER_UPDATE_RATE_HZ); + timestamp = millis(); - - // Bluetooth part - - // Create the BLE Device - BLEDevice::init("ESP32"); - - // Create the BLE Server - pServer = BLEDevice::createServer(); - pServer->setCallbacks(new MyServerCallbacks()); - - // Create the BLE Service - BLEService *pService = pServer->createService(SERVICE_UUID); - - // Create a BLE Characteristic - pSensorCharacteristic = pService->createCharacteristic( - SENSOR_CHARACTERISTIC_UUID, - BLECharacteristic::PROPERTY_READ | - BLECharacteristic::PROPERTY_WRITE | - BLECharacteristic::PROPERTY_NOTIFY | - BLECharacteristic::PROPERTY_INDICATE - ); - - // Create the ON button Characteristic - pLedCharacteristic = pService->createCharacteristic( - LED_CHARACTERISTIC_UUID, - BLECharacteristic::PROPERTY_WRITE - ); - - // Register the callback for the ON button characteristic - pLedCharacteristic->setCallbacks(new MyCharacteristicCallbacks()); - - // https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml - // Create a BLE Descriptor - pSensorCharacteristic->addDescriptor(new BLE2902()); - pLedCharacteristic->addDescriptor(new BLE2902()); - - // Start the service - pService->start(); - - // Start advertising - BLEAdvertising *pAdvertising = BLEDevice::getAdvertising(); - pAdvertising->addServiceUUID(SERVICE_UUID); - pAdvertising->setScanResponse(false); - pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter - BLEDevice::startAdvertising(); - Serial.println("Waiting a client connection to notify..."); + Wire.setClock(400000); // 400KHz } -void loop() -{ - lsm.read(); /* ask it to read in the data */ - /* Get a new sensor event */ - sensors_event_t a, m, g, temp; +void loop() { + float roll, pitch, heading; + float gx, gy, gz; + static uint8_t counter = 0; - lsm.getEvent(&a, &m, &g, &temp); + if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) { + return; + } + timestamp = millis(); + // Read the motion sensors + sensors_event_t accel, gyro, mag; + accelerometer->getEvent(&accel); + gyroscope->getEvent(&gyro); + magnetometer->getEvent(&mag); +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms"); +#endif - String test_str = "Accel X: " + String(a.acceleration.x) + " m/s^2" + - "\tY: " + String(a.acceleration.y) + " m/s^2 " + - "\tZ: " + String(a.acceleration.z) + " m/s^2\n" + - "Mag X: " + String(m.magnetic.x) + " uT" + - "\tY: " + String(m.magnetic.y) + " uT" + - "\tZ: " + String(m.magnetic.z) + " uT\n" + - "Gyro X: " + String(g.gyro.x) + " rad/s" + - "\tY: " + String(g.gyro.y) + " rad/s" + - "\tZ: " + String(g.gyro.z) + " rad/s\n"; + cal.calibrate(mag); + cal.calibrate(accel); + cal.calibrate(gyro); + // Gyroscope needs to be converted from Rad/s to Degree/s + // the rest are not unit-important + gx = gyro.gyro.x * SENSORS_RADS_TO_DPS; + gy = gyro.gyro.y * SENSORS_RADS_TO_DPS; + gz = gyro.gyro.z * SENSORS_RADS_TO_DPS; - Serial.print("Accel X: "); Serial.print(a.acceleration.x); Serial.print(" m/s^2"); - Serial.print("\tY: "); Serial.print(a.acceleration.y); Serial.print(" m/s^2 "); - Serial.print("\tZ: "); Serial.print(a.acceleration.z); Serial.println(" m/s^2 "); + // Update the SensorFusion filter + filter.update(gx, gy, gz, + accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, + mag.magnetic.x, mag.magnetic.y, mag.magnetic.z); +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms"); +#endif - Serial.print("Mag X: "); Serial.print(m.magnetic.x); Serial.print(" uT"); - Serial.print("\tY: "); Serial.print(m.magnetic.y); Serial.print(" uT"); - Serial.print("\tZ: "); Serial.print(m.magnetic.z); Serial.println(" uT"); + // only print the calculated output once in a while + if (counter++ <= PRINT_EVERY_N_UPDATES) { + return; + } + // reset the counter + counter = 0; - Serial.print("Gyro X: "); Serial.print(g.gyro.x); Serial.print(" rad/s"); - Serial.print("\tY: "); Serial.print(g.gyro.y); Serial.print(" rad/s"); - Serial.print("\tZ: "); Serial.print(g.gyro.z); Serial.println(" rad/s"); +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("Raw: "); + Serial.print(accel.acceleration.x, 4); Serial.print(", "); + Serial.print(accel.acceleration.y, 4); Serial.print(", "); + Serial.print(accel.acceleration.z, 4); Serial.print(", "); + Serial.print(gx, 4); Serial.print(", "); + Serial.print(gy, 4); Serial.print(", "); + Serial.print(gz, 4); Serial.print(", "); + Serial.print(mag.magnetic.x, 4); Serial.print(", "); + Serial.print(mag.magnetic.y, 4); Serial.print(", "); + Serial.print(mag.magnetic.z, 4); Serial.println(""); +#endif - Serial.println(); - // notify changed value - if (deviceConnected) { - pSensorCharacteristic->setValue(test_str.c_str()); - pSensorCharacteristic->notify(); - //value++; - Serial.print("New value notified: "); - Serial.println(value); - delay(100); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms - } - // disconnecting - if (!deviceConnected && oldDeviceConnected) { - Serial.println("Device disconnected."); - delay(500); // give the bluetooth stack the chance to get things ready - pServer->startAdvertising(); // restart advertising - Serial.println("Start advertising"); - oldDeviceConnected = deviceConnected; - } - // connecting - if (deviceConnected && !oldDeviceConnected) { - // do stuff here on connecting - oldDeviceConnected = deviceConnected; - Serial.println("Device Connected"); - } + // print the heading, pitch and roll + roll = filter.getRoll(); + pitch = filter.getPitch(); + heading = filter.getYaw(); + Serial.print("Orientation: "); + Serial.print(heading); + Serial.print(", "); + Serial.print(pitch); + Serial.print(", "); + Serial.println(roll); + + float qw, qx, qy, qz; + filter.getQuaternion(&qw, &qx, &qy, &qz); + Serial.print("Quaternion: "); + Serial.print(qw, 4); + Serial.print(", "); + Serial.print(qx, 4); + Serial.print(", "); + Serial.print(qy, 4); + Serial.print(", "); + Serial.println(qz, 4); +#if defined(AHRS_DEBUG_OUTPUT) + Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms"); +#endif } \ No newline at end of file