192 lines
5.4 KiB
C++
192 lines
5.4 KiB
C++
// 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
|
||
|
||
|
||
// SERIAL
|
||
#ifdef BOARD_HAS_USB_SERIAL
|
||
#include <SLIPEncodedUSBSerial.h>
|
||
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
|
||
#else
|
||
#include <SLIPEncodedSerial.h>
|
||
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
|
||
#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
|
||
SLIPSerial.begin(115200);
|
||
//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();
|
||
|
||
// 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());
|
||
|
||
// 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
|
||
|
||
SLIPSerial.beginPacket();
|
||
bundle.send(SLIPSerial);
|
||
SLIPSerial.endPacket();
|
||
|
||
#ifdef BT_OSC
|
||
SLIPBTSerial.beginPacket();
|
||
bundle.send(SLIPBTSerial);
|
||
SLIPBTSerial.endPacket();
|
||
#endif
|
||
|
||
bundle.empty();
|
||
}
|
||
}
|
||
}
|