2022-05-27 14:57:30 +02:00
// 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
2022-11-27 09:47:28 +01:00
// 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
2022-05-27 14:57:30 +02:00
// 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
2022-11-27 09:47:28 +01:00
SLIPSerial . begin ( 115200 ) ;
//Serial.begin(115200); // set this as high as you can reliably run on your platform
2022-05-27 14:57:30 +02:00
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 ) {
2022-11-27 09:47:28 +01:00
//Serial.print("Sample rate: "); Serial.print(sampleCount);
if ( ! imu - > IMUGyroBiasValid ( ) ) {
// Serial.println(", calculating gyro bias");
} else {
// Serial.println();
}
2022-05-27 14:57:30 +02:00
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 ( ) ) ;
2022-11-27 09:47:28 +01:00
// 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 ( ) ;
2022-05-27 14:57:30 +02:00
# ifdef BT_OSC
SLIPBTSerial . beginPacket ( ) ;
bundle . send ( SLIPBTSerial ) ;
SLIPBTSerial . endPacket ( ) ;
# endif
bundle . empty ( ) ;
}
}
}