Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
402 lines (319 sloc) 13.8 KB
/*
____ _____ _ _
| __ )| ____| | / \
| _ \| _| | | / _ \
| |_) | |___| |___ / ___ \
|____/|_____|_____/_/ \_\
The platform for ultra-low latency audio and sensor processing
http://bela.io
A project of the Augmented Instruments Laboratory within the
Centre for Digital Music at Queen Mary University of London.
http://www.eecs.qmul.ac.uk/~andrewm
(c) 2016 Augmented Instruments Laboratory: Andrew McPherson,
Astrid Bin, Liam Donovan, Christian Heinrichs, Robert Jack,
Giulio Moro, Laurel Pardue, Victor Zappi. All rights reserved.
The Bela software is distributed under the GNU Lesser General Public License
(LGPL 3.0), available here: https://www.gnu.org/licenses/lgpl-3.0.txt
*/
#include <Bela.h>
#include <xenomai/init.h>
#include <signal.h>
#include <cmath>
#include <math.h>
#include "Adafruit_FXOS8700.h"
#include "Adafruit_FXAS21002C.h"
#include <OSCClient.h>
OSCClient oscClient;
//=====================================================================================================
// MadgwickAHRS.h
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
//
//=====================================================================================================
//---------------------------------------------------------------------------------------------------
// Definitions
#define sampleFreq 50.0f // sample frequency in Hz
#define betaDef 0.1f // 2 * proportional gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
float beta = betaDef; // 2 * proportional gain (Kp)
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
//---------------------------------------------------------------------------------------------------
// Function declarations
void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
//======================================================================================================
// How many pins there are
#define NUM_TOUCH_PINS 12
// Change this to change how often the imu is read (in Hz)
int readInterval = sampleFreq;
Adafruit_FXOS8700 accmag(0x8700A, 0x8700B); // Object to handle FXOS8700 sensing
Adafruit_FXAS21002C gyro(0x0021002C);
AuxiliaryTask i2cTask; // Auxiliary task to read I2C
void readIMU(void*);
// SuperCollider default ports: 57120 = sclang, 57110 = scsynth
// Send OSC messages to local sclang
/*int remotePort = 57120;
const char* remoteIp = "127.0.0.1";*/
// Send OSC messages to remote sclang
int remotePort = 57120;
const char* remoteIp = "192.168.7.1";
// Required to init auxiliary tasks
extern int gXenomaiInited;
// Handle Ctrl-C by requesting that the audio rendering stop
void interrupt_handler(int var)
{
gShouldStop = true;
}
int main(int main_argc, char *main_argv[])
{
// Setup threading
int argc = 0;
char *const *argv;
xenomai_init(&argc, &argv);
gXenomaiInited = 1;
// Set up interrupt handler to catch Control-C and SIGTERM
signal(SIGINT, interrupt_handler);
signal(SIGTERM, interrupt_handler);
// Run until told to stop
if(!accmag.begin(ACCEL_RANGE_2G)) {
printf("Error initialising FXOS8700\n");
return false;
}
if(!gyro.begin()) {
printf("Error initialising FXAS21002C\n");
return false;
}
// Function to be run, priority, name
i2cTask = Bela_createAuxiliaryTask(readIMU, 50, "bela-IMU");
// OSC
oscClient.setup(remotePort, remoteIp);
int sleepTime = 1000000/sampleFreq;
while(!gShouldStop)
{
Bela_scheduleAuxiliaryTask(i2cTask);
usleep(sleepTime);
}
return false;
Bela_deleteAllAuxiliaryTasks();
}
void cleanup(BelaContext *context, void *userData)
{ }
// Auxiliary task to read the I2C board
void readIMU(void*)
{
accmag.readSensor();
gyro.readSensor();
//printf("GYRO x=%f, y=%f, z=%f\n", gyro.data.x, gyro.data.y, gyro.data.z);
// Do some analysis
accmag.calculateVelocity();
//printf("VELOCITY x: %f, y: %f, z: %f\n", accmag.accel_vel.x, accmag.accel_vel.y, accmag.accel_vel.z);
float mag = sqrt(pow(accmag.accel_vel.x, 2) + pow(accmag.accel_vel.y, 2) + pow(accmag.accel_vel.z, 2));
mag = mag * 50;
//printf("MAGNITUDE: %f \n", mag);
oscClient.sendMessageNow(oscClient.newMessage.to("/imu").add(mag).end());
//float Racc = std::sqrt(pow(accmag.accel_ms2.x, 2) + pow(accmag.accel_ms2.y, 2) + pow(accmag.accel_ms2.z, 2));
//printf("Racc= %f", Racc);
//MadgwickAHRSupdate(gyro.data.x, gyro.data.y, gyro.data.z, accmag.accel_ms2.x, accmag.accel_ms2.y, accmag.accel_ms2.z, accmag.mag_uTesla.x, accmag.mag_uTesla.y, accmag.mag_uTesla.z);
//printf("Madgwick q0: %f, q1: %f, q2: %f, q3: %f\n", q0, q1, q2, q3);
// You can use this to read binary on/off touch state more easily
//printf("Touched: %x\n", mpr121.touched());
}
//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
//
//=====================================================================================================
//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrt(float x);
//====================================================================================================
// Functions
//---------------------------------------------------------------------------------------------------
// AHRS algorithm update
void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// IMU algorithm update
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
/**
\example capacitive-touch/render.cpp
Capacitive touch sensing with MPR121
---------------------------
This sketch allows you to hook up an MPR121 capactive touch sensing device
to Bela, for example the SparkFun Capacitive Touch Sensor Breakout - MPR121.
The breakout board gives you 12 electrode connections.
To get this working with Bela you need to connect the breakout board to the I2C
terminal on the Bela board. See the Pin guide for details of which pin is which.
The sensor data will then be available for you to use in the array
`sensorValue[NUM_TOUCH_PINS]`.
*/
You can’t perform that action at this time.