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
}
