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]));
}
