void Matrix_update( int gx , int gy , int gz , int ax , int ay , int az )
{
Gyro_Vector[0] = Gyro_Scaled_X(fix16_from_int(-gx));
Gyro_Vector[1] = Gyro_Scaled_X(fix16_from_int(gy));
Gyro_Vector[2] = Gyro_Scaled_X(fix16_from_int(-gz));
/*
Accel_Vector[0]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)),
fix16_mul(fix16_from_int(ax),const_fix16_from_dbl(0.4)));
Accel_Vector[1]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)),
fix16_mul(fix16_from_int(ay),const_fix16_from_dbl(0.4)));
Accel_Vector[2]=fix16_sadd(fix16_mul(Accel_Vector[0],const_fix16_from_dbl(0.6)),
fix16_mul(fix16_from_int(az),const_fix16_from_dbl(0.4)));
*/
/* NO LOW PASS FILTER */
Accel_Vector[0] = fix16_from_int(ax);
Accel_Vector[1] = fix16_from_int(-ay);
Accel_Vector[2] = fix16_from_int(az);
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional
#if OUTPUTMODE==1 // corrected mode
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -fix16_mul(G_Dt, Omega_Vector[2]);//-z
Update_Matrix[0][2] = fix16_mul(G_Dt, Omega_Vector[1]);//y
Update_Matrix[1][0] = fix16_mul(G_Dt, Omega_Vector[2]);//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -fix16_mul(G_Dt, Omega_Vector[0]);//-x
Update_Matrix[2][0] = -fix16_mul(G_Dt, Omega_Vector[1]);//-y
Update_Matrix[2][1] = fix16_mul(G_Dt, Omega_Vector[0]);//x
Update_Matrix[2][2] = 0;
#endif
Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix);
Matrix_Addto(DCM_Matrix, Temporary_Matrix);
}
void Matrix_update(void)
{
Gyro_Vector[0] = Gyro_Scaled_X(gyro_x); //gyro x roll
Gyro_Vector[1] = Gyro_Scaled_Y(gyro_y); //gyro y pitch
Gyro_Vector[2] = Gyro_Scaled_Z(gyro_z); //gyro Z yaw
Accel_Vector[0] = accel_x;
Accel_Vector[1] = accel_y;
Accel_Vector[2] = accel_z;
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#if OUTPUTMODE==1
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -G_Dt * Omega_Vector[2];//-z
Update_Matrix[0][2] = G_Dt * Omega_Vector[1];//y
Update_Matrix[1][0] = G_Dt * Omega_Vector[2];//z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -G_Dt * Omega_Vector[0];//-x
Update_Matrix[2][0] = -G_Dt * Omega_Vector[1];//-y
Update_Matrix[2][1] = G_Dt * Omega_Vector[0];//x
Update_Matrix[2][2] = 0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); //a*b=c
int x;
for (x = 0; x < 3; x++) //Matrix Addition (update)
{
int y;
for (y = 0; y < 3; y++)
{
DCM_Matrix[x][y] += Temporary_Matrix[x][y];
}
}
}
void Orientation_scaleGyro(float* rawGyro, float* scaledGyro)
{
scaledGyro[0]=-1.0*Gyro_Scaled_X(rawGyro[0]); //gyro x roll
scaledGyro[1]=-1.0*Gyro_Scaled_Y(rawGyro[1]); //gyro y pitch
scaledGyro[2]=-1.0*Gyro_Scaled_Z(rawGyro[2]); //gyro Z yaw
}
