void Normalize(void) { fix16_t error = 0; fix16_t temporary[3][3]; fix16_t renorm = 0; error = -fix16_mul(Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0]), const_fix16_from_dbl(.5)); Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3), -Vector_Dot_Product(&temporary[0][0],&temporary[0][0]))); Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3), -Vector_Dot_Product(&temporary[1][0],&temporary[1][0]))); Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3), - Vector_Dot_Product(&temporary[2][0],&temporary[2][0]))); Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); }
void Matrix_Multiply(fix16_t a[3][3], fix16_t b[3][3],fix16_t mat[3][3]) { fix16_t op[3]; int x,y,w; for( x=0; x<3; x++) { for( y=0; y<3; y++) { for( w=0; w<3; w++) { op[w] = fix16_mul(a[x][w],b[w][y]); } mat[x][y]=0; mat[x][y]=fix16_sadd(op[0],fix16_sadd(op[1],op[2])); } } }
void Vector_Add(fix16_t vectorOut[3],fix16_t vectorIn1[3], fix16_t vectorIn2[3]) { int c; for(c=0; c<3; c++) { vectorOut[c]=fix16_sadd(vectorIn1[c],vectorIn2[c]); } }
fix16_t Vector_Dot_Product(fix16_t vector1[3],fix16_t vector2[3]) { fix16_t op=0; int c; for( c=0; c<3; c++) { op = fix16_sadd(op,fix16_mul(vector1[c],vector2[c])); } return op; }
void calculate_heading_hmc5843( fix16_t roll, fix16_t pitch) { // float Head_X; // float Head_Y; // float cos_roll; // float sin_roll; // float cos_pitch; // float sin_pitch; // // // cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM? // sin_roll = sin(roll); // cos_pitch = cos(pitch); // sin_pitch = sin(pitch); // // Tilt compensated Magnetic field X component: // Head_X = hmc5843_mag_x*cos_pitch+ // hmc5843_mag_y*sin_roll*sin_pitch+ // hmc5843_mag_z*cos_roll*sin_pitch; // // Tilt compensated Magnetic field Y component: // Head_Y = hmc5843_mag_y*cos_roll- // hmc5843_mag_z*sin_roll; // // Magnetic Heading // //hmc_5843_heading = atan2(-Head_Y,Head_X); fix16_t cos_roll = fix16_cos(roll); fix16_t sin_roll = fix16_sin(roll); fix16_t cos_pitch = fix16_cos(pitch); fix16_t sin_pitch = fix16_sin(pitch); hmc5843_head_x = fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_x),cos_pitch), fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_y),fix16_mul(sin_roll,sin_pitch)), fix16_mul(fix16_from_int(hmc5843_mag_z),fix16_mul(cos_roll,sin_pitch)))); hmc5843_head_y = fix16_sadd(fix16_mul(fix16_from_int(hmc5843_mag_y),cos_roll), -fix16_mul(fix16_from_int(hmc5843_mag_z),sin_roll)); hmc5843_heading = fix16_atan2(-hmc5843_head_y,hmc5843_head_x); }
void Matrix_Addto(fix16_t matrixOut[3][3],fix16_t a[3][3]) { int x; int y; for( x=0; x<3; x++) //Matrix Addition (update) { for(y=0; y<3; y++) { matrixOut[x][y] = fix16_sadd(a[x][y], matrixOut[x][y]); //printf("%f ",fix16_to_dbl(DCM_Matrix[x][y])); } //printf("\n"); } }
void Drift_correction(fix16_t head_x, fix16_t head_y) { fix16_t errorCourse; static fix16_t Scaled_Omega_P[3]; static fix16_t Scaled_Omega_I[3]; Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); errorRollPitch[0] = constrain(errorRollPitch[0],-fix16_from_int(1000),fix16_from_int(1000)); errorRollPitch[1] = constrain(errorRollPitch[1],-fix16_from_int(1000),fix16_from_int(1000)); errorRollPitch[2] = constrain(errorRollPitch[2],-fix16_from_int(1000),fix16_from_int(1000)); Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH); Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); #ifdef IsMAG errorCourse= fix16_sadd(fix16_mul(DCM_Matrix[0][0], head_x), fix16_mul(DCM_Matrix[1][0],head_y)); Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. errorYaw[0] = constrain(errorYaw[0],-fix16_from_int(50),fix16_from_int(50)); errorYaw[1] = constrain(errorYaw[1],-fix16_from_int(50),fix16_from_int(50)); errorYaw[2] = constrain(errorYaw[2],-fix16_from_int(50),fix16_from_int(50)); Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I #endif }
const Fix16 ssub(const int16_t other) const { Fix16 ret = fix16_sadd(value, -fix16_from_int(other)); return ret; }
const Fix16 ssub(const float other) const { Fix16 ret = fix16_sadd(value, -fix16_from_float(other)); return ret; }
const Fix16 ssub(const double other) const { Fix16 ret = fix16_sadd(value, -fix16_from_dbl(other)); return ret; }
const Fix16 ssub(const fix16_t other) const { Fix16 ret = fix16_sadd(value, -other); return ret; }
const Fix16 ssub(const Fix16 &other) const { Fix16 ret = fix16_sadd(value, -other.value); return ret; }
void Vector_Cross_Product(fix16_t vectorOut[3], fix16_t v1[3],fix16_t v2[3]) { vectorOut[0]= fix16_sadd(fix16_mul(v1[1],v2[2]), -fix16_mul(v1[2],v2[1])); vectorOut[1]= fix16_sadd(fix16_mul(v1[2],v2[0]), -fix16_mul(v1[0],v2[2])); vectorOut[2]= fix16_sadd(fix16_mul(v1[0],v2[1]), -fix16_mul(v1[1],v2[0])); }