I'd recommend you have everything on a single thread, e.g. (copying some stuff I don't understand from https://github.com/giuliomoro/belaonurhead/blob/master/lib/libpd-render-with-IMU.cpp, and also assuming that you are using the other files from that folder):
#include <Bela_BNO055.h>
#include <libraries/UdpClient/UdpClient.h>
#include <oscpkt.hh>
std::vector<I2C_BNO055> bnos(2);
UdpClient udpClient;
// Quaternions and Vectors
imu::Quaternion gCal, gCalLeft, gCalRight, gIdleConj = {1, 0, 0, 0};
imu::Quaternion qGravIdle, qGravCal, quat, steering, qRaw;
imu::Vector<3> gRaw;
imu::Vector<3> gGravIdle, gGravCal;
imu::Vector<3> ypr; //yaw pitch and roll angles
int calibrationState = 0; // state machine variable for calibration
int setForward = 0; // flag for setting forward orientation
int main()
{
udpClient.setup(1234, "192.168.7.1"); // use 192.168.7.1 to send to host or 192.168.7.2 to send to other program on Bela
// TODO: bno initialisation here
while(1)
{
oscpkt::Message msg("/address/path");
for(auto& bno : bnos)
{
// just copying code I don't really understand from readIMU() in
// https://github.com/giuliomoro/belaonurhead/blob/master/lib/libpd-render-with-IMU.cpp
// (includes undeclared variables)
// get calibration status
uint8_t sys, gyro, accel, mag;
bno.getCalibration(&sys, &gyro, &accel, &mag);
// quaternion data routine from MrHeadTracker
imu::Quaternion qRaw = bno.getQuat(); //get sensor raw quaternion data
if( setForward ) {
gIdleConj = qRaw.conjugate(); // sets what is looking forward
setForward = 0; // reset flag so only happens once
}
steering = gIdleConj * qRaw; // calculate relative rotation data
quat = gCalLeft * steering; // transform it to calibrated coordinate system
quat = quat * gCalRight;
ypr = quat.toEuler(); // transform from quaternion to Euler
msg.pushFloat(ypr[0]).pushFloat(ypr[1]).pushFloat(ypr[2]);
}
oscpkt::PacketWriter pw;
pw.init().addMessage(msg);
udpClient.send( pw.packetData(), pw.packetSize());
// the instructions above will take some time (especially reading I2C),
// so to achieve 10ms period you will have to tweak the below
usleep(90000);
}
return 0;
}
Try out the above. If the timing is not accurate enough for you, then you should turn this into a RT thread (i.e.: Linux thread with SCHED_FIFO and priority > 0). A Xenomai thread won't give any meaningful advantage over a SCHED_FIFO thread, because you are using I2C and networking which have to go through Linux.