g1smo
/
pifcamp-2021
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added sensor fusion to midible tilt

eulerConversion
fileneed 2022-03-04 02:39:17 +01:00
parent c57f219c5d
commit bd77bfab1e
1 changed files with 74 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
ArduinoNano33BLE/MidiBle-tilt/MidiBle-tilt.ino
View File

@ -1,5 +1,6 @@
#include <Arduino_LSM9DS1.h>
#include <BLEMIDI_Transport.h>
#include "SensorFusion.h" //SF
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
//#include <hardware/BLEMIDI_ESP32.h>
@ -30,9 +31,25 @@ float mgyrox, mgyroy, mgyroz;
// magnetometer
float magx, magy, magz;
float mmagx, mmagy, mmagz;
// sensor fusion
float mroll, mpitch, myaw;
float maxx, maxy, maxz;
//SF -
SF fusion;
float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw;
float deltat;
//Gyro Offset....: -605622 -24028 -338386
float goffx = -605622.0 / 1000000.0;
float goffy = -24028.0 / 1000000.0;
float goffz = -338386.0 / 1000000.0;
float fmap(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@ -109,27 +126,62 @@ void setup()
// -----------------------------------------------------------------------------
void loop()
{
if (IMU.accelerationAvailable()) {
IMU.readAcceleration(accx, accy, accz);
IMU.readGyroscope(gyrox, gyroy, gyroz);
IMU.readMagneticField(magx, magy, magz);
// now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
// NOTE: the gyroscope data have to be in radians
// if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD
accmag = sqrt(accx*accx+accy*accy+accz*accz);
if (IMU.magneticFieldAvailable()) {
IMU.readMagneticField(mx, my, mz);
}
if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) {
IMU.readAcceleration(ax, ay, az);
/*
ax *= 9.81;
ay *= 9.81;
az *= 9.81;
*/
IMU.readGyroscope(gx, gy, gz);
gx -= goffx;
gy -= goffy;
gz -= goffz;
gx *= DEG_TO_RAD;
gy *= DEG_TO_RAD;
gz *= DEG_TO_RAD;
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
//choose only one of these two:
//fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat); //else use the magwick, it is slower but more accurate
pitch = fusion.getPitch();
roll = fusion.getRoll(); //you could also use getRollRadians() ecc
yaw = fusion.getYaw();
accmag = sqrt(ax*ax+ay*ay+az*az);
// accmag = map(accmag, 0, 1000, 0,127);
maccx = fmap(accx,-4.0,4.0, 0.0,127.0);
maccy = fmap(accy,-4.0,4.0, 0.0,127.0);
maccz = fmap(accz,-4.0,4.0, 0.0,127.0);
mroll = fmap(roll,-180,180,0,127);
mpitch = fmap(pitch,-90,90,0,127);
myaw = fmap(yaw,0,360,0,127);
maccx = fmap(ax,-4.0,4.0, 0.0,127.0);
maccy = fmap(ay,-4.0,4.0, 0.0,127.0);
maccz = fmap(az,-4.0,4.0, 0.0,127.0);
//mgyrox = gyrox/2000 * 64 + 64;
//mgyroy = gyroy/2000 * 64 + 64;
//mgyroz = gyroz/2000 * 64 + 64;
mgyrox = map(gyrox, -2000,2000, 0.0, 127.0);
mgyroy = map(gyroy, -2000,2000, 0.0, 127.0);
mgyroz = map(gyroz, -2000,2000, 0.0, 127.0);
mgyrox = map(gx, -2000,2000, 0.0, 127.0);
mgyroy = map(gy, -2000,2000, 0.0, 127.0);
mgyroz = map(gz, -2000,2000, 0.0, 127.0);
// mz = max(10, abs(z) * 1000);
mmagx = fmap(magx, 0,60, 0,127);
mmagy = fmap(magy, 0,60, 0,127);
mmagz = fmap(magz, 0,60, 0,127);
mmagx = fmap(mx, 0,60, 0,127);
mmagy = fmap(my, 0,60, 0,127);
mmagz = fmap(mz, 0,60, 0,127);
maxx = max(magx, maxx);
maxy = max(magy, maxy);
@ -163,11 +215,14 @@ void loop()
MIDI.sendControlChange(7 + hand, mmagx, midichannel);
MIDI.sendControlChange(8 + hand, mmagy, midichannel);
MIDI.sendControlChange(9 + hand, mmagz, midichannel);
Serial.print(mmagx);
Serial.print('\t');
Serial.print(mmagy);
Serial.print('\t');
Serial.println(mmagz);
MIDI.sendControlChange(10 + hand, mroll, midichannel);
MIDI.sendControlChange(11 + hand, mpitch, midichannel);
MIDI.sendControlChange(12 + hand, myaw, midichannel);
// Serial.print(mmagx);
// Serial.print('\t');
// Serial.print(mmagy);
// Serial.print('\t');
// Serial.println(mmagz);
// Serial.print('\t');
// Serial.println(accmag * 100);
// Serial.println("ping");