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 }