Sensor fusion implemented
This commit is contained in:
parent
0e76cfc25a
commit
61a5580269
6 changed files with 413 additions and 203 deletions
|
|
@ -1,8 +1,24 @@
|
|||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
#include <SPI.h>
|
||||
#include <Adafruit_LSM9DS1.h>
|
||||
#include <Adafruit_Sensor.h> // not used in this demo but required!
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
|
||||
|
||||
// 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");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
165
3 Sensor fusion/main.cpp
Normal file
165
3 Sensor fusion/main.cpp
Normal file
|
|
@ -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 <Adafruit_Sensor_Calibration.h>
|
||||
#include <Adafruit_AHRS.h>
|
||||
|
||||
Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
|
||||
|
||||
#include <Adafruit_LSM9DS1.h>
|
||||
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
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,217 +1,165 @@
|
|||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
#include <SPI.h>
|
||||
// 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 <Adafruit_Sensor_Calibration.h>
|
||||
#include <Adafruit_AHRS.h>
|
||||
|
||||
Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
|
||||
|
||||
#include <Adafruit_LSM9DS1.h>
|
||||
#include <Adafruit_Sensor.h> // not used in this demo but required!
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
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<int>(value[0])); // Print the integer value
|
||||
|
||||
int receivedValue = static_cast<int>(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
|
||||
}
|
||||
Loading…
Reference in a new issue