I've done a little code for headtracking over BT for this board, still awaiting it's arrival so I can't test it right now, but would like to hear some suggestions, criticism etc from you guys if I need to fix something for this project to work.
I am using Arduino Nano 33 BLE and it's LSM9DS1 sensors for Head Tracking of x, y, z, yaw, pitch and roll, sending data over BT to opentrack program on PC via BT for sims (flying, racing etc).
Thanks
// Include libraries
#include <ArduinoBLE.h>
#include <Arduino_LSM9DS1.h>
#include <MadgwickAHRS.h>
// Declare the BLE object
BLEService movementService("19B10000-E8F2-537E-4F6C-D104768A1214");
BLEFloatCharacteristic xChar("19B10001-E8F2-537E-4F6C-D104768A1214", BLERead | BLENotify);
BLEFloatCharacteristic yChar("19B10002-E8F2-537E-4F6C-D104768A1214", BLERead | BLENotify);
BLEFloatCharacteristic zChar("19B10003-E8F2-537E-4F6C-D104768A1214", BLERead | BLENotify);
BLEFloatCharacteristic yawChar("19B10004-E8F2-537E-4F6C-D104768A1214", BLERead | BLENotify);
BLEFloatCharacteristic pitchChar("19B10005-E8F2-537E-4F6C-D104768A1214", BLERead | BLENotify);
BLEFloatCharacteristic rollChar("19B10006-E8F2-537E-4F6C-D104768A1214", BLERead | BLENotify);
// Filter setup
Madgwick filter; // Madgwick filter
// Sensor measurements
float ax, ay, az; // Accelerometer data in m/s^2
float gx, gy, gz; // Gyroscope data in rad/s
float mx, my, mz; // Magnetometer data in uT
// additional variables
float distance = 0; // distance travelled
float vx = 0, vy = 0, vz = 0; // velocity readings
float prev_ax = 0, prev_ay = 0, prev_az = 0; // previous acceleration readings
float prev_vx = 0, prev_vy = 0, prev_vz = 0; // previous velocity readings
float x_accel = 0, y_accel = 0, z_accel = 0; // accel temporary coordinates
float x_yawpitch = 0, y_yawpitch = 0, z_yawpitch = 0; // yaw and pitch temporary coordinates
// Output variables
float x = 0, y = 0, z = 0;
float roll = 0, pitch = 0, yaw = 0;
// Time variables
unsigned long prevTime = 0;
unsigned long currentTime = 0;
float dt = 0;
// Define complementary filter constants
const float alpha = 0.9; // Weight for accelerometer data
const float beta = 0.1; // Weight for yaw and pitch data
// Setup
void setup() {
// Initialize the BLE module
if (!BLE.begin()) {
while (1);
}
// Set BLE local name
BLE.setLocalName("Head Tracker");
// Add BLE service and characteristic
BLE.addService(movementService);
BLE.setAdvertisedService(movementService);
movementService.addCharacteristic(xChar);
movementService.addCharacteristic(yChar);
movementService.addCharacteristic(zChar);
movementService.addCharacteristic(yawChar);
movementService.addCharacteristic(pitchChar);
movementService.addCharacteristic(rollChar);
// Device name over BlueTooth
BLE.setDeviceName("Head Tracker");
BLE.setAppearance(0x00);
// Start advertising the BLE service
BLE.advertise();
// Initialize the LSM9DS1 sensor
if (!IMU.begin()) {
while (1);
}
// Zeroing
xChar.setValue(0);
yChar.setValue(0);
zChar.setValue(0);
yawChar.setValue(0);
pitchChar.setValue(0);
rollChar.setValue(0);
// Initialize the Madgwick filter with a sampling frequency of 60 Hz
filter.begin(60);
}
// Main
void loop() {
// Compute time interval since last loop iteration
currentTime = micros();
dt = (currentTime - prevTime) / 1000000.0;
if (dt >= 1/60) {
// Read sensor data
IMU.readAcceleration(ax, ay, az);
IMU.readGyroscope(gx, gy, gz);
IMU.readMagneticField(mx, my, mz);
// Perform sensor fusion with the Madgwick filter
filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
// Get orientation estimates in terms of pitch, yaw, and roll
roll = filter.getRoll();
pitch = filter.getPitch();
yaw = filter.getYaw();
// calculate position over acceleration
vx = vx + (ax + prev_ax) * dt / 2;
vy = vy + (ay + prev_ay) * dt / 2;
vz = vz + (az + prev_az) * dt / 2;
x_accel = x + (vx + prev_vx) * dt / 2;
y_accel = y + (vy + prev_vy) * dt / 2;
z_accel = z + (vz + prev_vz) * dt / 2;
// calculate distance
distance = sqrt(x*x + y*y + z*z);
// calculate position over yaw and pitch
x_yawpitch = cos(yaw) * cos(pitch) * distance;
y_yawpitch = sin(yaw) * cos(pitch) * distance;
z_yawpitch = sin(pitch) * distance;
// Combine data using complementary filter
x = alpha * x_accel + beta * x_yawpitch;
y = alpha * y_accel + beta * y_yawpitch;
z = alpha * z_accel + beta * z_yawpitch;
// Send the sensor data over BLE
rollChar.writeValue(roll);
pitchChar.writeValue(pitch);
yawChar.writeValue(yaw);
xChar.writeValue(x);
yChar.writeValue(y);
zChar.writeValue(z);
// Save current values
prevTime = currentTime;
prev_vx = vx;
prev_ax = ax;
prev_vy = vy;
prev_ay = ay;
prev_vz = vz;
prev_az = az;
}
BLE.poll();
}
