/*------------------------------------------------------------------------
*
* PROTOTYPE : void V3XMatrix_Rotate_Z(int32_t Theta, V3XSCALAR *Matrice)
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_Z(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin=(V3XSCALAR)sin16(Theta);
cos=(V3XSCALAR)cos16(Theta);
M[0]=MULF32(cos, Matrice[0])-MULF32(sin, Matrice[3]);
M[1]=MULF32(cos, Matrice[1])-MULF32(sin, Matrice[4]);
M[2]=MULF32(cos, Matrice[2])-MULF32(sin, Matrice[5]);
M[3]=MULF32(sin, Matrice[0])+MULF32(cos, Matrice[3]);
M[4]=MULF32(sin, Matrice[1])+MULF32(cos, Matrice[4]);
M[5]=MULF32(sin, Matrice[2])+MULF32(cos, Matrice[5]);
Matrice[0]=M[0]; Matrice[1]=M[1]; Matrice[2]=M[2];
Matrice[3]=M[3]; Matrice[4]=M[4]; Matrice[5]=M[5];
}
/*------------------------------------------------------------------------
*
* PROTOTYPE : void V3XMatrix_Rotate_X_Local(int32_t Theta, V3XSCALAR *Matrice)
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_X_Local(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin=(V3XSCALAR)sin16(Theta);
cos=(V3XSCALAR)cos16(Theta);
M[1]=MULF32(cos, Matrice[1])-MULF32(sin, Matrice[2]);
M[4]=MULF32(cos, Matrice[4])-MULF32(sin, Matrice[5]);
M[7]=MULF32(cos, Matrice[7])-MULF32(sin, Matrice[8]);
M[2]=MULF32(sin, Matrice[1])+MULF32(cos, Matrice[2]);
M[5]=MULF32(sin, Matrice[4])+MULF32(cos, Matrice[5]);
M[8]=MULF32(sin, Matrice[7])+MULF32(cos, Matrice[8]);
Matrice[1]=M[1]; Matrice[4]=M[4]; Matrice[7]=M[7];
Matrice[2]=M[2]; Matrice[5]=M[5]; Matrice[8]=M[8];
}
/*------------------------------------------------------------------------
*
* PROTOTYPE : void V3XMatrix_Rotate_Y(int32_t Theta, V3XSCALAR *Matrice)
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_Y(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin = (V3XSCALAR)sin16(Theta);
cos = (V3XSCALAR)cos16(Theta);
M[0]=MULF32(cos, Matrice[0])-MULF32(sin, Matrice[6]);
M[1]=MULF32(cos, Matrice[1])-MULF32(sin, Matrice[7]);
M[2]=MULF32(cos, Matrice[2])-MULF32(sin, Matrice[8]);
M[6]=MULF32(sin, Matrice[0])+MULF32(cos, Matrice[6]);
M[7]=MULF32(sin, Matrice[1])+MULF32(cos, Matrice[7]);
M[8]=MULF32(sin, Matrice[2])+MULF32(cos, Matrice[8]);
Matrice[0]=M[0]; Matrice[1]=M[1]; Matrice[2]=M[2];
Matrice[6]=M[6]; Matrice[7]=M[7]; Matrice[8]=M[8];
}
/*------------------------------------------------------------------------
*
* PROTOTYPE :
*
* DESCRIPTION :
*
*/
void V3XMatrix_Rotate_X(int32_t Theta, V3XSCALAR *Matrice)
{
V3XSCALAR sin, cos;
V3XSCALAR M[9];
sin = sin16(Theta);
cos = cos16(Theta);
M[3]=MULF32(cos, Matrice[3])-MULF32(sin, Matrice[6]);
M[4]=MULF32(cos, Matrice[4])-MULF32(sin, Matrice[7]);
M[5]=MULF32(cos, Matrice[5])-MULF32(sin, Matrice[8]);
M[6]=MULF32(sin, Matrice[3])+MULF32(cos, Matrice[6]);
M[7]=MULF32(sin, Matrice[4])+MULF32(cos, Matrice[7]);
M[8]=MULF32(sin, Matrice[5])+MULF32(cos, Matrice[8]);
Matrice[3]=M[3];
Matrice[4]=M[4];
Matrice[5]=M[5];
Matrice[6]=M[6];
Matrice[7]=M[7];
Matrice[8]=M[8];
}
bool ahrsUpdate(int16_t *yaw, int16_t *pitch, int16_t *roll){
int16_t axt, ayt, azt, mxt, myt, mzt;
int32_t ax, ay, az, mx, my, mz;
int32_t rollSin, rollCos, pitchSin, pitchCos;
int32_t tmpA, tmpB, tmpC, tmpD, tmpE;
// Read data from IMU and convert to plane based coordinate system.
// X forward, Y starbord, and Z down, but invert gravity vector as
// well so gravity is positive.
ahrsReadAcc(&axt, &ayt, &azt);
ahrsReadMag(&mxt, &myt, &mzt);
ax = -axt; ay = ayt; az = azt;
mx = mxt; my = -myt; mz = -mzt;
// Calculate the roll angle.
*roll = atan216(ay, az);
// Limit roll angle to -180 to 180.
if (*roll > TRIG16_CYCLE/2){
*roll -= TRIG16_CYCLE;
}
// Compute trig functions of roll.
rollSin = sin16(*roll);
rollCos = cos16(*roll);
// Calculate the pitch angle.
tmpA = (ay*rollSin)/TRIG16_ONE;
tmpB = (az*rollCos)/TRIG16_ONE;
*pitch = atan216(-ax, tmpA + tmpB);
// Limit pitch to -90 to +90
if (*pitch > TRIG16_CYCLE/4){
*pitch = TRIG16_CYCLE/2 - *pitch;
}
if (*pitch < -TRIG16_CYCLE/4){
*pitch = -TRIG16_CYCLE/2 - *pitch;
}
// Compute trig functions of pitch.
pitchSin = sin16(*pitch);
pitchCos = cos16(*pitch);
// Calculate the yaw angle.
tmpA = (mx*pitchCos)/TRIG16_ONE;
tmpB = (mz*pitchSin)/TRIG16_ONE;
tmpC = (((mz*rollSin)/TRIG16_ONE)*pitchCos)/TRIG16_ONE;
tmpD = (((mx*rollSin)/TRIG16_ONE)*pitchSin)/TRIG16_ONE;
tmpE = (my*rollCos)/TRIG16_ONE;
*yaw = atan216(tmpC - tmpD - tmpE, tmpA + tmpB);
// Check for validity of solution.
tmpA = (ax*ax)/IMU_ONE + (ay*ay)/IMU_ONE + (az*az)/IMU_ONE;
if (tmpA < IMU_ACC_MAG_MIN || IMU_ACC_MAG_MAX < tmpA){
return false; // possible errors
}
// Result is valid.
return true;
}
