Getting Quaternions From Gyro Data. How do I get body coordinates? - c++

Question

I've got a gyro hooked up to an arduino and I'm getting angular rate out in rad/sec in all three axes.
I want to be able to get out yaw, pitch, roll in body coordinates so the three axes of rotation are fixed to the body. The problem I'm having now is that when I roll the sensor, the yaw and pitch I get out become swapped. As I roll the sensor 90 degrees, the yaw and pitch change places. Anywhere in between, the yaw and pitch are a mixture between the two.
Instead, I want to keep the pitch and yaw relative to the new body rotation rather than the initial position.
Here is my code:
void loop() {
currentTime = millis();
dt = ((currentTime - prevTime) / 1000.0 );
// Puts gyro data into data[2], data[4], data[5]
readBMI();
if(firstPass == false) {
omega[0] = (data[3]);
omega[1] = (data[4]);
omega[2] = (data[5]);
wLength = sqrt(sq(omega[0]) + sq(omega[1]) + sq(omega[2]));
theta = wLength * dt;
q_new[0] = cos(theta/2);
q_new[1] = (omega[0] / wLength * sin(theta / 2));
q_new[2] = (omega[1] / wLength * sin(theta / 2));
q_new[3] = (omega[2] / wLength * sin(theta / 2));
q[0] = q[0] * q_new[0] - q[1] * q_new[1] - q[2] * q_new[2] - q[3] * q_new[3];
q[1] = q[0] * q_new[1] + q[1] * q_new[0] + q[2] * q_new[3] - q[3] * q_new[2];
q[2] = q[0] * q_new[2] - q[1] * q_new[3] + q[2] * q_new[0] + q[3] * q_new[1];
q[3] = q[0] * q_new[3] + q[1] * q_new[2] - q[2] * q_new[1] + q[3] * q_new[0];
float sinr_cosp = 2 * (q[0] * q[1] + q[2] * q[3]);
float cosr_cosp = 1 - 2 * (sq(q[1]) + sq(q[2]));
roll = atan2(sinr_cosp, cosr_cosp) * 180 / PI;
pitch = asin(2 * (q[0] * q[2] - q[3] * q[1])) * 180 / PI;
double siny_cosp = 2 * (q[0] * q[3] + q[1] * q[2]);
double cosy_cosp = 1 - 2 * (sq(q[2]) + sq(q[3]));
yaw = atan2(siny_cosp, cosy_cosp) * 180 / PI;
}
Serial.print(roll);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.print(yaw);
Serial.print(" ");
Serial.println();
delay(20);
prevTime = currentTime;
}
I'm getting the angles out correctly but my only problem is the yaw and pitch swap when it rolls. So I'm guessing I need a way to convert from world to body coodrinates?