Converting vector from relative coordinate system to an absolute one - c++

Question

I'm trying to make a quadcopter drone. I'm using an mpu6050 to get acceleration & angular speed and then convert them to Roll / pitch / yaw
With the acceleration, i'm also trying to get the speed & position by integration. However , i need them to be in absolute coordinate system, because the mpu6050 gives you the values in its relative coordinates system.
the origin of the new coordinate system is the starting position of the drone, and the direction is "where the drone is looking" , we assume Yaw = 0 at the beginning, and we get yaw using the gyrometer's data.
I tried to rotate the vectors using the Roll / pitch values, but that doesn't seem to work very well.
I tried with this being the gravity vector for example : (-2, -2, -1)
If i convert it to the absolute coordinate system, i should get : (0,0, 3)
#include <iostream>
using namespace std;
#include <math.h>
// Vector class, to handle all the vector operations for us
// Thanks to : https://stackoverflow.com/questions/14607640/rotating-a-vector-in-3d-space
class cVector
{
public:
float x;
float y;
float z;
// Constructor
cVector();
cVector(float x1, float y1, float z1);
// returns the vector's magnitude
float Magnitude();
// Normalize ( change length to 1, while keeping the same direction)
void Normalize();
// Rotate around the Axis
void RotateX(float angle);
void RotateY(float angle);
void RotateZ(float angle);
// TODO : Add operators for Addition & Substraction
// Addition
cVector operator+(cVector const& v1) const
{
return cVector(x + v1.x,
y + v1.y,
z + v1.z);
}
void operator+=(cVector const& v1)
{
x += v1.x;
y += v1.y;
z += v1.z;
}
// Substraction
cVector operator-(cVector const& v1) const
{
return cVector(x - v1.x,
y - v1.y,
z - v1.z);
}
void operator-=(cVector const& v1)
{
x -= v1.x;
y -= v1.y;
z -= v1.z;
}
// Multiplication
void operator*=(const float scalar)
{
x *= scalar;
y *= scalar;
z *= scalar;
}
cVector operator*(const float scalar) const
{
return cVector(x * scalar,
y * scalar,
z * scalar);
}
// Division
void operator/=(const float scalar)
{
x /= scalar;
y /= scalar;
z /= scalar;
}
cVector operator/(const float scalar) const
{
return cVector(x / scalar,
y / scalar,
z / scalar);
}
};
// Constructor
cVector::cVector()
{
}
cVector::cVector(float x1, float y1, float z1)
{
x = x1;
y = y1;
z = z1;
}
// returns the vector's magnitude
float cVector::Magnitude()
{
return sqrt((x * x) + (y * y) + (z * z));
}
// Normalize ( change length to 1, while keeping the same direction)
void cVector::Normalize()
{
float flMagnitude = Magnitude();
// We devide the coordinates by the magnitude
x /= flMagnitude;
y /= flMagnitude;
z /= flMagnitude;
}
// Rotate around the Axis
void cVector::RotateX(float angle)
{
// Calculate the sinus and cosinus
float flCos = static_cast<float>(cos(angle));
float flSin = static_cast<float>(sin(angle));
// We save the current values temporarily
float _y = y;
float _z = z;
y = _y * flCos - _z * flSin;
z = _y * flSin + _z * flCos;
}
void cVector::RotateY(float angle)
{
// Calculate the sinus and cosinus
float flCos = static_cast<float>(cos(angle));
float flSin = static_cast<float>(sin(angle));
// We save the current values temporarily
float _x = x;
float _z = z;
x = _x * flCos + _z * flSin;
z = - _x * flSin + _z * flCos;
}
void cVector::RotateZ(float angle)
{
// Calculate the sinus and cosinus
float flCos = static_cast<float>(cos(angle));
float flSin = static_cast<float>(sin(angle));
// We save the current values temporarily
float _x = x;
float _y = y;
x = _x * flCos - _y * flSin;
y = _x * flSin + _y * flCos;
}
void PrintVector(cVector vec)
{
cout << "X : " << vec.x << " Y : " << vec.y << " Z : " << vec.z << endl;
}
// TODO : Add operators for Addition & Substraction
int main()
{
cVector vec(-2, -2, -1);
// Calculate pitch / roll
float pitch = static_cast<float>(atan2( vec.y , sqrt( pow(vec.x,2) + pow(vec.z,2) ) ));
float roll = static_cast<float>(atan2(-1 * vec.x , sqrt( pow(vec.y,2) + pow(vec.z,2) ) ));
// vec.RotateY(1.570796f);
vec.RotateX(roll);
vec.RotateY(pitch);
PrintVector(vec);
cin.get();
return 0;
}
expected result ( 0, 0, 3 )
Actual results : (-0.104919 , -0.824045, -2.8827 )