Connecting 2x BNO055 to Bela

Hi,
I'm trying to read data from 2 BNO055 (from Adafruit) and send them via OSC.
FIRST: Thanks to @giuliomoro and @theleadingzero for sharing your library and examples on Github.

Now to my problem:
I can't initialize 2 Instances of class I2C_BNO055. One Sensor at a time works perfectly.
The wiring seems to be ok, as I can find both sensors when scanning i2c with their respective addresses.

Here's the error I got:

Building project ...
Building render.cpp...
...done
Using library OscSender
Using library UdpClient
Linking...
...done
Build finished
Running project ...
id: 160
switching to operation mode
Unable to send data from readRegister: -1
in operation mode
Initialised BNO055 2
id: 160
switching to operation mode
Unable to send data from readRegister: -1
Segmentation fault
Makefile:621: recipe for target 'runide' failed
make: *** [runide] Error 139
Bela stopped

And the Code of my reder.ccp

#include <Bela.h>
#include <cmath>
//#include <rtdk.h>
#include "Bela_BNO055.h"
#include <fcntl.h>
#include <linux/input.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <libraries/OscSender/OscSender.h>

OscSender oscSender;

std::string gOscAddress = "/imu"; // OSC address. Message format: <address> <encoderId> <encoderValue>
std::string gDestinationIp = "192.168.7.1"; // 192.168.7.1 is the host computer (if it's a Mac or Linux, it would be 192.168.6.1 if it's Windows).
//std::string gDestinationIp = "127.0.0.1"; // local IP Adress of the Bela
int gDestinationPort = 5555;
//UdpClient sock;

/*BNO055 INIT*/
// Change this to change how often the BNO055 IMU is read (in Hz)
int readInterval = 100;

//I2C_BNO055 bno; // IMU sensor object
I2C_BNO055 bno1; // = I2C_BNO055(1, BNO055_ADDRESS_A);
I2C_BNO055 bno2; // = I2C_BNO055(1, BNO055_ADDRESS_B);
int buttonPin = 1; // calibration button pin
int lastButtonValue = 0; // using a pulldown resistor

// Quaternions and Vectors
imu::Quaternion gCal1, gCalLeft1, gCalRight1, gIdleConj1, gCal2, gCalLeft2, gCalRight2, gIdleConj2 = {1, 0, 0, 0};
imu::Quaternion qGravIdle1, qGravCal1, quat1, steering1, qRaw1, qGravIdle2, qGravCal2, quat2, steering2, qRaw2;

imu::Vector<3> gRaw1;         

imu::Vector<3> gGravIdle1, gGravCal1;
imu::Vector<3> ypr1; //yaw pitch and roll angles
imu::Vector<3> gRaw2;         

imu::Vector<3> gGravIdle2, gGravCal2;
imu::Vector<3> ypr2; //yaw pitch and roll angles

int calibrationState = 0; // state machine variable for calibration
int setForward = 0; // flag for setting forward orientation

// variables handling threading
AuxiliaryTask i2cTask;		// Auxiliary task to read I2C
AuxiliaryTask gravityNeutralTask;		// Auxiliary task to read gravity from I2C
AuxiliaryTask gravityDownTask;		// Auxiliary task to read gravity from I2C

int readCount = 0;			// How long until we read again...
int readIntervalSamples = 0; // How many samples between reads

int printThrottle = 0; // used to limit printing frequency

// function declarations
void readIMU(void*);
void getNeutralGravity(void*);
void getDownGravity(void*);
void calibrate();
void resetOrientation();

/*BNO055 INIT END*/

// setup() is called once before the audio rendering starts.
// Use it to perform any initialisation and allocation which is dependent
// on the period size or sample rate.
//
// userData holds an opaque pointer to a data structure that was passed
// in from the call to initAudio().
//
// Return true on success; returning false halts the program.

bool setup(BelaContext *context, void *userData)
{
	bool init = false;
	if(!bno2.begin( 1, BNO055_ADDRESS_B)) {
		rt_printf("Error initialising BNO055 2\n");
		init =  false;
	}else{
		rt_printf("Initialised BNO055 2\n");
		init = true;
	}
	if(!bno1.begin( 1, BNO055_ADDRESS_A)) {
		rt_printf("Error initialising BNO055 1\n");
	}else{
		rt_printf("Initialised BNO055 1\n");
		init = true;
	}
	
return init;

rt_printf("Initialised BNO055\n");

// use external crystal for better accuracy
bno1.setExtCrystalUse(true);
bno2.setExtCrystalUse(true);

// get the system status of the sensor to make sure everything is ok
uint8_t sysStatus, selfTest, sysError;
bno1.getSystemStatus(&sysStatus, &selfTest, &sysError);
rt_printf("System Status 1: %d (0 is Idle)   Self Test: %d (15 is all good)   System Error: %d (0 is no error)\n", sysStatus, selfTest, sysError);
bno2.getSystemStatus(&sysStatus, &selfTest, &sysError);
rt_printf("System Status 2: %d (0 is Idle)   Self Test: %d (15 is all good)   System Error: %d (0 is no error)\n", sysStatus, selfTest, sysError);


// set sensor reading in a separate thread
// so it doesn't interfere with the audio processing
i2cTask = Bela_createAuxiliaryTask(&readIMU, 5, "bela-bno");
readIntervalSamples = context->audioSampleRate / readInterval;

gravityNeutralTask = Bela_createAuxiliaryTask(&getNeutralGravity, 5, "bela-neu-gravity");
gravityDownTask = Bela_createAuxiliaryTask(&getDownGravity, 5, "bela-down-gravity");

// set up button pin
pinMode(context, 0, buttonPin, INPUT); 

oscSender.setup(gDestinationPort, gDestinationIp);
return true;
}

static void sendIMUXYZOsc(const std::string& subPath, float x1, float y1, float z1, float x2, float y2, float z2)
{
	std::string address = gOscAddress + subPath;
    oscSender.newMessage(address).add(x1).add(y1).add(z1).add(x2).add(y2).add(z2).send();

/*

oscpkt::PacketWriter pw;
pw.init();
std::string address = gOscAddress + subPath;
oscpkt::Message msg(address);
pw.addMessage(msg.pushFloat(value));
//printf("%s %f\n", address.c_str(), value);
if(pw.isOk())
	sock.send((void*)pw.packetData(), pw.packetSize());
	*/
}
/**
int main(){
	if(!sock.setup(gDestinationPort, gDestinationIp.c_str()))
	{
		fprintf(stderr, "Unable to send to %s:%d\n", gDestinationIp.c_str(), gDestinationPort);
		return 1;
	}

// Variables keyboard control
struct input_event ev;
const char *dev = "/dev/input/event1";

// qwerty keyboard capture
int fd = open(dev, O_RDONLY);
if (fd == -1) {
	fprintf(stderr, "Cannot open %s:.\n", dev);
	return -1;
}
while (1)
{
	ssize_t n = read(fd, &ev, sizeof ev);
	if (n < 0) {
		fprintf(stderr, "Error while reading: %d %s\n", errno, strerror(errno));
		return -1;
	} else if (n != sizeof(ev)) {
		fprintf(stderr, "Read unexpected length\n");
		return -1;
	} else {
		if (ev.type == EV_KEY && ev.value == 0 && ev.value <= 2)
		{
			// key released
			sendOsc("/released", ev.code);
		}
		if (ev.type == EV_KEY && ev.value == 1 && ev.value <= 2)
		{
			// key pressed
			sendOsc("", ev.code);
		}
	}
}
close(fd); // won't actually get here unless an error occurs
return 0;
}**/

// render() is called regularly at the highest priority by the audio engine.
// Input and output are given from the audio hardware and the other
// ADCs and DACs (if available). If only audio is available, numAnalogFrames
// will be 0.

void render(BelaContext *context, void *userData)
{
	// iteratre through all audio samples in the frame
	for(int n = 0; n < context->audioFrames; n++) {
		// this schedules the imu sensor readings
		if(++readCount >= readIntervalSamples) {
			readCount = 0;
			Bela_scheduleAuxiliaryTask(i2cTask);
			//sendOsc("/released", ev.code);
		}
		
	// print IMU values, but not every sample
	printThrottle++;
	if(printThrottle >= 500){
		sendIMUXYZOsc("", ypr1[0], ypr1[1], ypr1[2], ypr2[0], ypr2[1], ypr2[2]);
		//sendOsc("/x", ypr[0]);
		//sendOsc("/y", ypr[1]);
		//sendOsc("/z", ypr[2]);
		//rt_printf("%f %f %f\n", ypr[0], ypr[1], ypr[2]);
		imu::Vector<3> qForward1 = gIdleConj1.toEuler();
		imu::Vector<3> qForward2 = gIdleConj2.toEuler();
		printThrottle = 0;
	}

	//read the value of the button
	int buttonValue = digitalRead(context, 0, buttonPin); 

	// if button wasn't pressed before and is pressed now
	if( buttonValue != lastButtonValue && buttonValue == 1 ){
		// then run calibration to set looking forward (gGravIdle) 
		// and looking down (gGravCal)
		switch(calibrationState) {
		case 0: // first time button was pressed
			setForward = 1;
			// run task to get gravity values when sensor in neutral position
			Bela_scheduleAuxiliaryTask(gravityNeutralTask);
			calibrationState = 1;	// progress calibration state
			break;
		case 1: // second time button was pressed
			// run task to get gravity values when sensor 'looking down' (for head-tracking) 
		 	Bela_scheduleAuxiliaryTask(gravityDownTask);
			calibrationState = 0; // reset calibration state for next time
			break;
		} 
	}
	lastButtonValue = buttonValue;
	
	// map yaw, pitch, or roll to frequency for synth
	// change ypr[0] to ypr[1] or ypr[2] to access the other axes
/**		gFrequency = map(ypr[0], M_PI*-0.5, M_PI*0.5, 100, 800);
		
	// calculate audio sample for synth
	float out = 0.3 * sinf(gPhase);
	gPhase += 2.0 * M_PI * gFrequency * gInverseSampleRate;
	if(gPhase > 2.0 * M_PI)
		gPhase -= 2.0 * M_PI;

	// write sample to output
	for(unsigned int channel = 0; channel < context->audioOutChannels; channel++) {
		audioWrite(context, n, channel, out);
	}**/
}
}




// Auxiliary task to read from the I2C board
void readIMU(void*)
{
	// get calibration status
	uint8_t sys, gyro, accel, mag;
	bno1.getCalibration(&sys, &gyro, &accel, &mag);
	bno2.getCalibration(&sys, &gyro, &accel, &mag);
	// status of 3 means fully calibrated
	//rt_printf("CALIBRATION STATUSES\n");
	//rt_printf("System: %d   Gyro: %d Accel: %d  Mag: %d\n", sys, gyro, accel, mag);
	
// quaternion data routine from MrHeadTracker
imu::Quaternion qRaw1 = bno1.getQuat(); //get sensor raw quaternion data
imu::Quaternion qRaw2 = bno2.getQuat(); //get sensor raw quaternion data

if( setForward ) {
	gIdleConj1 = qRaw1.conjugate(); // sets what is looking forward
	gIdleConj2 = qRaw2.conjugate(); // sets what is looking forward
	setForward = 0; // reset flag so only happens once
}
	
steering1 = gIdleConj1 * qRaw1; // calculate relative rotation data
steering2 = gIdleConj2 * qRaw2; // calculate relative rotation data
quat1 = gCalLeft1 * steering1; // transform it to calibrated coordinate system
quat2 = gCalLeft2 * steering2; // transform it to calibrated coordinate system
quat1 = quat1 * gCalRight1;
quat2 = quat1 * gCalRight2;

ypr1 = quat1.toEuler(); // transform from quaternion to Euler
ypr2 = quat2.toEuler(); // transform from quaternion to Euler
}

// Auxiliary task to read from the I2C board
void getNeutralGravity(void*) {
	// read in gravity value
  	imu::Vector<3> gravity1 = bno1.getVector(I2C_BNO055::VECTOR_GRAVITY);
  	imu::Vector<3> gravity2 = bno2.getVector(I2C_BNO055::VECTOR_GRAVITY);
  	gravity1 = gravity1.scale(-1);
  	gravity1.normalize();
  	gravity2 = gravity2.scale(-1);
  	gravity2.normalize();
  	gGravIdle1 = gravity1;
  	gGravIdle2 = gravity2;

}

// Auxiliary task to read from the I2C board
void getDownGravity(void*) {
	// read in gravity value
  	imu::Vector<3> gravity1 = bno1.getVector(I2C_BNO055::VECTOR_GRAVITY);
  	imu::Vector<3> gravity2 = bno2.getVector(I2C_BNO055::VECTOR_GRAVITY);
  	gravity1 = gravity1.scale(-1);
  	gravity2 = gravity2.scale(-1);
  	gravity1.normalize();
  	gravity2.normalize();
  	gGravCal1 = gravity1;
  	gGravCal2 = gravity2;
  	// run calibration routine as we should have both gravity values
  	calibrate(); 
}

// calibration of coordinate system from MrHeadTracker
// see http://www.aes.org/e-lib/browse.cfm?elib=18567 for full paper
// describing algorithm
void calibrate() {
  	imu::Vector<3> g1, gravCalTemp1, x1, y1, z1;
  	imu::Vector<3> g2, gravCalTemp2, x2, y2, z2;
  	g1 = gGravIdle1; // looking forward in neutral position
  	g2 = gGravIdle2; // looking forward in neutral position
  	
  

z1 = g1.scale(-1); 
z2 = g1.scale(-1); 
z1.normalize();
z2.normalize();

gravCalTemp1 = gGravCal1; // looking down
gravCalTemp2 = gGravCal2; // looking down
y1 = gravCalTemp1.cross(g1);
y2 = gravCalTemp1.cross(g2);
y1.normalize();
y2.normalize();

x1 = y1.cross(z1);
x2 = y2.cross(z2);
x1.normalize();
x2.normalize();

imu::Matrix<3> rot1;
rot1.cell(0, 0) = x1.x();
rot1.cell(1, 0) = x1.y();
rot1.cell(2, 0) = x1.z();
rot1.cell(0, 1) = y1.x();
rot1.cell(1, 1) = y1.y();
rot1.cell(2, 1) = y1.z();
rot1.cell(0, 2) = z1.x();
rot1.cell(1, 2) = z1.y();
rot1.cell(2, 2) = z1.z();

imu::Matrix<3> rot2;
rot2.cell(0, 0) = x2.x();
rot2.cell(1, 0) = x2.y();
rot2.cell(2, 0) = x2.z();
rot2.cell(0, 1) = y2.x();
rot2.cell(1, 1) = y2.y();
rot2.cell(2, 1) = y2.z();
rot2.cell(0, 2) = z2.x();
rot2.cell(1, 2) = z2.y();
rot2.cell(2, 2) = z2.z();

gCal1.fromMatrix(rot1);
gCal2.fromMatrix(rot2);

resetOrientation();
}

// from MrHeadTracker
// resets values used for looking forward
void resetOrientation() {
  	gCalLeft1 = gCal1.conjugate();
  	gCalRight1 = gCal1;
  	gCalLeft2 = gCal2.conjugate();
  	gCalRight2 = gCal2;
}


// cleanup() is called once at the end, after the audio has stopped.
// Release any resources that were allocated in setup().

void cleanup(BelaContext *context, void *userData)
{
	// Nothing to do here
}


/**
\imu-sine-sythn/render.cpp

Inertial Measurement Unit (IMU) Sensing with the BNO055
----------------------------------------------------------

This sketch allows you to hook up BNO055 IMU movement sensing device
to Bela, for example the Adafruit BNO055 breakout board.

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.

Connect a push button with a pull-down resistor to pin P8_08.

When running sketch, hold IMU in a neutral position and press the push button 
once. Tilt IMU down (if wearing as for head-tracking, look down) and press the 
push button a second time. The system is now calibrated. Calibration can be
run again at any time.

*/

Sorry, I just found one of the errors.
I feel like an idiot...

bool init = false;
	if(!bno2.begin( 1, BNO055_ADDRESS_B)) {
		rt_printf("Error initialising BNO055 2\n");
		init =  false;
	}else{
		rt_printf("Initialised BNO055 2\n");
		init = true;
	}
	if(!bno1.begin( 1, BNO055_ADDRESS_A)) {
		rt_printf("Error initialising BNO055 1\n");
	}else{
		rt_printf("Initialised BNO055 1\n");
		init = true;
	}
	
return init; // idiot me!

I'll come back, when I fixed all the other errors.