/*------------------------------------------------------------------------ * * 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; }