176 lines
5.0 KiB
Arduino
176 lines
5.0 KiB
Arduino
|
// ESP32 Dev Module
|
||
|
|
||
|
#include <Wire.h>
|
||
|
|
||
|
// Parameters for device
|
||
|
#define BTNAME "kegel 2"
|
||
|
|
||
|
// IMU libraries
|
||
|
#include "I2Cdev.h"
|
||
|
#include "RTIMUSettings.h"
|
||
|
#include "RTIMU.h"
|
||
|
#include "RTFusionRTQF.h"
|
||
|
#include "CalLib.h"
|
||
|
#include <EEPROM.h>
|
||
|
|
||
|
#include <OSCBundle.h>
|
||
|
#include <OSCBoards.h>
|
||
|
|
||
|
#define BT_OSC
|
||
|
|
||
|
#define DISPLAY_INTERVAL 5 // interval between pose displays
|
||
|
|
||
|
|
||
|
// Bluetooth
|
||
|
#ifdef BT_OSC
|
||
|
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
|
||
|
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
|
||
|
#endif
|
||
|
|
||
|
#include <SLIPEncodedSerial.h>
|
||
|
#include "BluetoothSerial.h"
|
||
|
#include "SLIPEncodedBluetoothSerial.h"
|
||
|
|
||
|
BluetoothSerial SerialBT;
|
||
|
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
|
||
|
#endif
|
||
|
|
||
|
// Motion sensor objects
|
||
|
RTIMU *imu; // the IMU object
|
||
|
RTFusionRTQF fusion; // the fusion object
|
||
|
RTIMUSettings settings; // the settings object
|
||
|
|
||
|
unsigned long lastDisplay;
|
||
|
unsigned long lastRate;
|
||
|
int sampleCount;
|
||
|
RTQuaternion gravity;
|
||
|
|
||
|
bool reset; // For quaternion calibration
|
||
|
|
||
|
/* OSC MSG channels */
|
||
|
OSCBundle bundle;
|
||
|
|
||
|
void setup() {
|
||
|
int errcode;
|
||
|
// Basic(debug) serial init
|
||
|
Serial.begin(115200); // set this as high as you can reliably run on your platform
|
||
|
Serial.println("Starting up...");
|
||
|
|
||
|
// Init EEPROM based on magnet calibration size requirement
|
||
|
EEPROM.begin(512);
|
||
|
|
||
|
// I2C init
|
||
|
Wire.begin();
|
||
|
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||
|
|
||
|
// create the imu object
|
||
|
imu = RTIMU::createIMU(&settings);
|
||
|
|
||
|
Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
|
||
|
if ((errcode = imu->IMUInit()) < 0) {
|
||
|
Serial.print("Failed to init IMU: "); Serial.println(errcode);
|
||
|
}
|
||
|
|
||
|
|
||
|
if (imu->getCalibrationValid())
|
||
|
Serial.println("Using compass calibration");
|
||
|
else
|
||
|
Serial.println("No valid compass calibration data");
|
||
|
|
||
|
// Gravity obj
|
||
|
gravity.setScalar(0);
|
||
|
gravity.setX(0);
|
||
|
gravity.setY(0);
|
||
|
gravity.setZ(1);
|
||
|
/*
|
||
|
fusion.setSlerpPower(0.02);
|
||
|
fusion.setGyroEnable(true);
|
||
|
fusion.setAccelEnable(true);
|
||
|
fusion.setCompassEnable(true);
|
||
|
*/
|
||
|
|
||
|
lastDisplay = lastRate = millis();
|
||
|
sampleCount = 0;
|
||
|
|
||
|
#ifdef BT_OSC
|
||
|
SerialBT.begin(BTNAME);
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
void loop() {
|
||
|
unsigned long now = millis();
|
||
|
unsigned long delta;
|
||
|
RTVector3 realAccel;
|
||
|
RTQuaternion rotatedGravity;
|
||
|
RTQuaternion fusedConjugate;
|
||
|
RTQuaternion qTemp;
|
||
|
int loopCount = 0;
|
||
|
|
||
|
// get the latest data if ready yet
|
||
|
while (imu->IMURead()) {
|
||
|
// this flushes remaining data in case we are falling behind
|
||
|
if (++loopCount >= 10)
|
||
|
continue;
|
||
|
|
||
|
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
|
||
|
|
||
|
// do gravity rotation and subtraction
|
||
|
|
||
|
// create the conjugate of the pose
|
||
|
fusedConjugate = fusion.getFusionQPose().conjugate();
|
||
|
|
||
|
// now do the rotation - takes two steps with qTemp as the intermediate variable
|
||
|
qTemp = gravity * fusion.getFusionQPose();
|
||
|
rotatedGravity = fusedConjugate * qTemp;
|
||
|
|
||
|
// now adjust the measured accel and change the signs to make sense
|
||
|
realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
|
||
|
realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
|
||
|
realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
|
||
|
|
||
|
sampleCount++;
|
||
|
if ((delta = now - lastRate) >= 1000) {
|
||
|
Serial.print("Sample rate: "); Serial.print(sampleCount);
|
||
|
if (!imu->IMUGyroBiasValid())
|
||
|
Serial.println(", calculating gyro bias");
|
||
|
else
|
||
|
Serial.println();
|
||
|
|
||
|
sampleCount = 0;
|
||
|
lastRate = now;
|
||
|
}
|
||
|
|
||
|
//RTMath::display("Accel:", realAccel);
|
||
|
//Serial.println();
|
||
|
|
||
|
// Quaternion - rotacija
|
||
|
//bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z
|
||
|
|
||
|
// Euler - rotacija
|
||
|
//eulerVector = eulerFromQuaternion(q);
|
||
|
//bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
|
||
|
//bundle.add("/euler").add(fusion.getFusionPose().x()).add(fusion.getFusionPose().y()).add(fusion.getFusionPose().z());
|
||
|
// Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
|
||
|
//bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
|
||
|
|
||
|
// Rotation diff value in euler angle
|
||
|
//eulerDiffVector = eulerFromQuaternion(diff);
|
||
|
//bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
|
||
|
|
||
|
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
|
||
|
lastDisplay = now;
|
||
|
// Accelerometer
|
||
|
bundle.add("/accel").add(realAccel.x()).add(realAccel.y()).add(realAccel.z());
|
||
|
|
||
|
// Some bug below, it seems
|
||
|
#ifdef BT_OSC
|
||
|
SLIPBTSerial.beginPacket();
|
||
|
bundle.send(SLIPBTSerial);
|
||
|
SLIPBTSerial.endPacket();
|
||
|
#endif
|
||
|
|
||
|
bundle.empty();
|
||
|
}
|
||
|
}
|
||
|
}
|