void Normalize(void)
{
float error = 0;
float temporary[3][3];
float renorm = 0;
error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
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 = .5 * (3 - Vector_Dot_Product(&temporary[0][0], &temporary[0][0])); //eq.21
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm = .5 * (3 - Vector_Dot_Product(&temporary[1][0], &temporary[1][0])); //eq.21
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm = .5 * (3 - Vector_Dot_Product(&temporary[2][0], &temporary[2][0])); //eq.21
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}
void Matrix_update(float dt)
{
Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if OUTPUTMODE==1 // With corrected data (drift correction)
Update_Matrix[0][0] = 0;
Update_Matrix[0][1] = -dt * Omega_Vector[2]; //-z
Update_Matrix[0][2] = dt * Omega_Vector[1]; //y
Update_Matrix[1][0] = dt * Omega_Vector[2]; //z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -dt * Omega_Vector[0]; //-x
Update_Matrix[2][0] = -dt * Omega_Vector[1]; //-y
Update_Matrix[2][1] = 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] = -dt * ahrs_dcm.imu_rate.r; //-z
Update_Matrix[0][2] = dt * ahrs_dcm.imu_rate.q; //y
Update_Matrix[1][0] = dt * ahrs_dcm.imu_rate.r; //z
Update_Matrix[1][1] = 0;
Update_Matrix[1][2] = -dt * ahrs_dcm.imu_rate.p;
Update_Matrix[2][0] = -dt * ahrs_dcm.imu_rate.q;
Update_Matrix[2][1] = dt * ahrs_dcm.imu_rate.p;
Update_Matrix[2][2] = 0;
#endif
Matrix_Multiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); //a*b=c
for (int x = 0; x < 3; x++) { //Matrix Addition (update)
for (int y = 0; y < 3; y++) {
DCM_Matrix[x][y] += Temporary_Matrix[x][y];
}
}
}
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_update(void)
{
int diff=0;
for(int axis=0; axis <3; axis++){
if(SENSOR_SIGN[axis]<0){
diff = AN_OFFSET[axis]-AN[axis];
Gyro_Vector[axis]=Gyro_Scaled(diff); //gyro x roll
//Gyro_Vector[axis]=(AN_OFFSET[axis]-AN[axis]); //gyro x roll
}
else{
diff = AN[axis] - AN_OFFSET[axis];
Gyro_Vector[axis]=Gyro_Scaled(diff); //gyro x roll
//Gyro_Vector[axis]=Gyro_Scaled_X(AN[axis]-AN_OFFSET[axis]); //gyro x roll
}
}
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
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0] * Accel_Vector[0] + Accel_Vector[1]
* Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY_DIV; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
// Accel_weight = constrain(1 - 2 * abs(1 - Accel_magnitude), 0, 1); //
Accel_weight = 1 - 2 * fabs(1 - Accel_magnitude);
// constrain
if (Accel_weight > 1)
Accel_weight = 1;
else if (Accel_weight < 0)
Accel_weight = 0;
Vector_Cross_Product(&errorRollPitch[0], &Accel_Vector[0],
&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCHa * Accel_weight);
Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCHa
* Accel_weight);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
#ifdef NO_MAGNETOMETER
float mag_heading_x;
float mag_heading_y;
float errorCourse;
static float Scaled_Omega_P[3];
//*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading
mag_heading_x = cos(MAG_Heading);
mag_heading_y = sin(MAG_Heading);
errorCourse = (DCM_Matrix[0][0] * mag_heading_y) - (DCM_Matrix[1][0]
* mag_heading_x); //Calculating YAW error
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);//.01proportional of YAW.
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);//.00001Integrator
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);//adding integrator to the Omega_I
#endif
}
void Matrix_update(void)
{
// maybe make the values EXTERN to reduce function calls
Gyro_Vector[0] = ToRad(((float)imu_read_sensor(GYRO_X)/ GYRO_DIV)); //gyro x roll // 14.375 = Gyro RAW to Grad/s
Gyro_Vector[1] = ToRad(((float)imu_read_sensor(GYRO_Y)/ GYRO_DIV)); //gyro y pitch
Gyro_Vector[2] = ToRad(((float)imu_read_sensor(GYRO_Z)/ GYRO_DIV)); //gyro Z yaw
Accel_Vector[0] = imu_read_sensor(ACC_X);
Accel_Vector[1] = imu_read_sensor(ACC_Y);
Accel_Vector[2] = imu_read_sensor(ACC_Z); // Expect 0,0,4096 (ideal)
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[0][0], &Update_Matrix[0][0],
&Temporary_Matrix[0][0]); //a*b=c
int x, y;
for (x = 0; x < 3; x++) //Matrix Addition (update)
{
for (y = 0; y < 3; y++)
{
DCM_Matrix[x][y] += Temporary_Matrix[x][y];
}
}
}
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];
}
}
}
///
//Determines if and how an AABB is colliding with an oct tree node.
//
//Parameters:
// node: The node to check if the game object is colliding with
// AABB: The AABB to test for
// frame: The frame of reference with which to orient the AABB
//
//Returns:
// 0 if the AABB does not collide with the octent
// 1 if the AABB intersects the octent but is not contained within the octent
// 2 if the AABB is completely contained within the octent
static unsigned char OctTree_Node_DoesAABBCollide(struct OctTree_Node* node, struct ColliderData_AABB* AABB, FrameOfReference* frame)
{
unsigned char collisionStatus = 0;
//Get centroid in world space to have the real center of the AABB
Vector pos;
Vector_INIT_ON_STACK(pos, 3);
Vector_Add(&pos, AABB->centroid, frame->position);
//Get the scaled dimensions of AABB
struct ColliderData_AABB scaled;
AABBCollider_GetScaledDimensions(&scaled, AABB, frame);
float bounds[6] =
{
pos.components[0] - scaled.width / 2.0f,
pos.components[0] + scaled.width / 2.0f,
pos.components[1] - scaled.height / 2.0f,
pos.components[1] + scaled.height / 2.0f,
pos.components[2] - scaled.depth / 2.0f,
pos.components[2] + scaled.depth / 2.0f
};
unsigned char overlap = 0;
if(node->left <= bounds[1] && node->right >= bounds[0])
{
if(node->bottom <= bounds[3] && node->top >= bounds[2])
{
if(node->back <= bounds[5] && node->front >= bounds[4])
{
overlap = 1;
}
}
}
//Set the collision status
collisionStatus = overlap;
//If we found that the bounds do overlap, we must check if the node contains the sphere
if(collisionStatus == 1)
{
overlap = 0;
if(node->left <= bounds[0] && node->right >= bounds[1])
{
if(node->bottom <= bounds[2] && node->top >= bounds[3])
{
if(node->back <= bounds[4] && node->front >= bounds[5])
{
overlap = 1;
}
}
}
//Update collision status
collisionStatus += overlap;
}
return collisionStatus;
}
void controller(float32_t *input, ControllerParams params, float32_t *thetadot, float32_t *error, float32_t dt)
{
float32_t ctheta, cphi;
float32_t total;
float32_t tmp_1[3], tmp_2[3], tmp_3[3];
float32_t err[3];
//Compute total thrust
cphi = FastCos(params.integral[0]);
ctheta = FastCos(params.integral[1]);
total = params.m * params.g / params.k / (cphi * ctheta);
//Compute error and inputs.
Vector_Multiply_By_Scale(tmp_1, thetadot, params.Kd);
Vector_Multiply_By_Scale(tmp_2, params.integral, params.Kp);
Vector_Multiply_By_Scale(tmp_3, params.integral2rd, params.Ki);
Vector_Add(err, tmp_1, tmp_2);
Vector_Subtract(err, err, tmp_3);
err2inputs(input, params, err, total);
// Update controller state.
Vector_Integral(params.integral, thetadot, dt);
Vector_Integral(params.integral2rd, params.integral, dt);
if ((thetadot[0] < 0.2f) && (thetadot[1] < 0.2f) && (thetadot[2] < 0.2f)){
//recalculate
cphi = FastCos(params.integral[0]);
ctheta = FastCos(params.integral[1]);
total = params.m * params.g / params.k / (cphi * ctheta);
//to do
}
}
void Matrix_update(void)
{
Gyro_Vector[0] = gyro_scale(g[X]); //gyro x roll
Gyro_Vector[1] = gyro_scale(g[Y]); //gyro y pitch
Gyro_Vector[2] = gyro_scale(g[Z]); //gyro Z yaw
Accel_Vector[0] = -a[X];
Accel_Vector[1] = -a[Y];
Accel_Vector[2] = -a[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
#ifdef CORRECT_DRIFT
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
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void DCM_driftCorrection(float* accelVector, float scaledAccelMag, float magneticHeading)
{
//Compensation the Roll, Pitch and Yaw drift.
float magneticHeading_X;
float magneticHeading_Y;
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_weight;
float Integrator_magnitude;
float tempfloat;
//*****Roll and Pitch***************
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - scaledAccelMag),0,1);
Vector_Cross_Product(&errorRollPitch[0],&accelVector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
//Calculate Heading_X and Heading_Y
magneticHeading_X = cos(magneticHeading);
magneticHeading_Y = sin(magneticHeading);
// We make the gyro YAW drift correction based on compass magnetic heading
errorCourse=(DCM_Matrix[0][0]*magneticHeading_Y) - (DCM_Matrix[1][0]*magneticHeading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
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
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > ToRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
}
}
void DCM_matrixUpdate(float timeDiff, float* gyroRadSec)
{
int x = 0, y = 0;
Vector_Add(&Omega[0], &gyroRadSec[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
// Update_Matrix[0][0]=0;
// Update_Matrix[0][1]=-timeDiff*gyroRadSec[2];//-z
// Update_Matrix[0][2]=timeDiff*gyroRadSec[1];//y
// Update_Matrix[1][0]=timeDiff*gyroRadSec[2];//z
// Update_Matrix[1][1]=0;
// Update_Matrix[1][2]=-timeDiff*gyroRadSec[0];//-x
// Update_Matrix[2][0]=-timeDiff*gyroRadSec[1];//-y
// Update_Matrix[2][1]=timeDiff*gyroRadSec[0];//x
// Update_Matrix[2][2]=0;
//
//drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-timeDiff*Omega_Vector[2];//-z
Update_Matrix[0][2]=timeDiff*Omega_Vector[1];//y
Update_Matrix[1][0]=timeDiff*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-timeDiff*Omega_Vector[0];//-x
Update_Matrix[2][0]=-timeDiff*Omega_Vector[1];//-y
Update_Matrix[2][1]=timeDiff*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
Matrix_Multiply(Temporary_Matrix, DCM_Matrix,Update_Matrix); //a*b=c
while (x < 3)
{
y= 0;
while (y < 3)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
y++;
}
x++;
}
}
void Matrix_update(void)
{
Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
Accel_Vector[0]=accel[0];
Accel_Vector[1]=accel[1];
Accel_Vector[2]=accel[2];
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
#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use 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;
#else // Use drift correction
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;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
int x,y;
for(x=0; x<3; x++) //Matrix Addition (update)
{
for(y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
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 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
}
/**************************************************************************
* \brief Filter DCM Matrix Update
*
* \param ---
*
* \return ---
***************************************************************************/
static void filter_dcm_matrix_update(void)
{
Gyro_Vector[0] = -ToRad(filterdata->xGyr); //+ //+
Gyro_Vector[1] = -ToRad(filterdata->yGyr); //- //+
Gyro_Vector[2] = -ToRad(filterdata->zGyr); //- //+
Accel_Vector[0] = filterdata->xAcc; //-
Accel_Vector[1] = filterdata->yAcc; //-
Accel_Vector[2] = -filterdata->zAcc; //-
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
/* Remove centrifugal acceleration. */
//filter_dcm_accel_adjust(); //Not yet used!
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=dt*Omega_Vector[1];//y
Update_Matrix[1][0]=dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
filter_dcm_matrix_multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
/* Matrix Addition (update) */
for(int x=0; x<3; x++)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
//
// A spring dampening function
// for the camera
//
void SpringDamp(
Vector currPos,
Vector trgPos, // Target Position
Vector prevTrgPos, // Previous Target Position
Vector result,
float springConst, // Hooke's Constant
float dampConst, // Damp Constant
float springLen)
{
Vector disp; // Displacement
Vector velocity; // Velocity
Vector mx;
Vector z;
float len;
float dot;
float forceMag; // Force Magnitude
// Calculate Spring Force
Vector_Minus(currPos, trgPos, disp);
Vector_Minus(prevTrgPos, trgPos, velocity);
len = Vector_Length(disp);
// get dot product
dot = DotProduct(disp, velocity);
forceMag = springConst * (springLen - len) +
dampConst * (dot / len);
Vector_Normalize(disp, z);
//disp *= forceMag;
Vector_Multiply(z, mx, forceMag);
printf("%0.2f %0.2f\n", mx[0], currPos[0]);
Vector_Add(currPos, mx, result);
//result[0] = currPos[0];
//result[1] = currPos[1];
//result[2] = currPos[2];
} // end of the function
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
}
struct trace *Trace_ClipMoveToEdict(struct server *server, struct edict *edict, vec3_t mins, vec3_t maxs, vec3_t start, vec3_t stop)
{
struct trace *trace;
struct hull *hull;
vec3_t offset, start_l, stop_l;
if (!server || !edict)
return NULL;
hull = Server_HullForEdict(server, edict, mins, maxs, offset);
Vector_Subtract(start_l, start, offset);
Vector_Subtract(stop_l, stop, offset);
trace = Trace_HullTrace(NULL, hull, start_l, stop_l);
Vector_Add(trace->endpos, offset, trace->endpos);
if (trace->fraction < 1 || trace->allsolid)
trace->e.ent = edict;
return trace;
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Integrator_magnitude;
// Local Working Variables
float errorRollPitch[3];
float errorYaw[3];
float errorCourse;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#if PERFORMANCE_REPORTING == 1
{
float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
imu_health += tempfloat;
Bound(imu_health,129,65405);
}
#endif
Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
#ifdef USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
// float mag_heading_x = cos(MAG_Heading);
// float mag_heading_y = sin(MAG_Heading);
// 2D dot product
errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
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
#elif defined USE_GPS // Use GPS Ground course to correct yaw gyro drift
if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s
float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(ground_course); //Course overground X axis
float COGY = sinf(ground_course); //Course overground Y axis
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
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
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > DegOfRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude);
}
}
void Drift_correction()
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Integrator_magnitude;
// Local Working Variables
float errorRollPitch[3];
float errorYaw[3];
float errorCourse;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2 * fabs(1 - Accel_magnitude), 0, 1); //
#if PERFORMANCE_REPORTING == 1
{
//amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
float tempfloat = ((Accel_weight - 0.5) * 256.0f);
imu_health += tempfloat;
Bound(imu_health, 129, 65405);
}
#endif
Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight);
Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
//*****YAW***************
#if USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
// float mag_heading_x = cos(MAG_Heading);
// float mag_heading_y = sin(MAG_Heading);
// 2D dot product
//Calculating YAW error
errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
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.
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
#else // Use GPS Ground course to correct yaw gyro drift
if (ahrs_dcm.gps_course_valid) {
float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(course); //Course overground X axis
float COGY = sinf(course); //Course overground Y axis
errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
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.
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
}
#if USE_MAGNETOMETER_ONGROUND == 1
PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
else if (launch == FALSE) {
float COGX = mag_float.x; // Non-Tilt-Compensated (for filter stability reasons)
float COGY = mag_float.y; // Non-Tilt-Compensated (for filter stability reasons)
errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
// P only
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
}
#endif // USE_MAGNETOMETER_ONGROUND
#endif
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I, Omega_I));
if (Integrator_magnitude > RadOfDeg(300)) {
Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude);
}
}
void Normalize(void)
{
float error = 0;
float temporary[3][3];
float renorm = 0;
uint8_t problem = FALSE;
// Find the non-orthogonality of X wrt Y
error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
// Add half the XY error to X, and half to Y
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
// The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
// Normalize lenght of X
renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
// a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
// b) if the norm is further from 1, use a real sqrt
// c) norm is huge: disaster! reset! mayday!
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
// Normalize lenght of Y
renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
// Normalize lenght of Z
renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
// Reset on trouble
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
problem = FALSE;
}
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Integrator_magnitude;
// Local Working Variables
float errorRollPitch[3];
float errorYaw[3];
float errorCourse;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#if USE_HIGH_ACCEL_FLAG
// Test for high acceleration:
// - low speed
// - high thrust
if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
if (high_accel_flag) {
Accel_weight = 0.;
}
#endif
#if PERFORMANCE_REPORTING == 1
{
float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
imu_health += tempfloat;
Bound(imu_health,129,65405);
}
#endif
Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
#if USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
// float mag_heading_x = cos(MAG_Heading);
// float mag_heading_y = sin(MAG_Heading);
// 2D dot product
errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
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
#else // Use GPS Ground course to correct yaw gyro drift
if (ahrs_impl.gps_course_valid) {
float course = ahrs_impl.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(course); //Course overground X axis
float COGY = sinf(course); //Course overground Y axis
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
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
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > RadOfDeg(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*RadOfDeg(300)/Integrator_magnitude);
}
}
void DCM_normalize(void)
{
float error=0;
float temporary[3][3];
float renorm=0;
boolean problem=0;
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
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= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
if (renorm < 1.5625f && renorm > 0.64f)
{
renorm= .5 * (3-renorm); //eq.21
}
else if (renorm < 100.0f && renorm > 0.01f)
{
renorm= 1. / sqrt(renorm);
}
else
{
problem = 1;
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
if (renorm < 1.5625f && renorm > 0.64f)
{
renorm= .5 * (3-renorm); //eq.21
}
else if (renorm < 100.0f && renorm > 0.01f)
{
renorm= 1. / sqrt(renorm);
}
else
{
problem = 1;
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
if (renorm < 1.5625f && renorm > 0.64f)
{
renorm= .5 * (3-renorm); //eq.21
}
else if (renorm < 100.0f && renorm > 0.01f)
{
renorm= 1. / sqrt(renorm);
}
else
{
problem = 1;
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
DCM_Matrix[0][0]= 1.0f;
DCM_Matrix[0][1]= 0.0f;
DCM_Matrix[0][2]= 0.0f;
DCM_Matrix[1][0]= 0.0f;
DCM_Matrix[1][1]= 1.0f;
DCM_Matrix[1][2]= 0.0f;
DCM_Matrix[2][0]= 0.0f;
DCM_Matrix[2][1]= 0.0f;
DCM_Matrix[2][2]= 1.0f;
problem = 0;
}
}
/**************************************************************************
* \brief Filter DCM Drift Correction
*
* \param ---
*
* \return ---
***************************************************************************/
static void filter_dcm_drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
//float mag_heading_x;
//float mag_heading_y;
//static float Scaled_Omega_P[3];
//float Integrator_magnitude;
//float tempfloat;
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//***** Roll and Pitch ***************
/* Calculate the magnitude of the accelerometer vector */
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
//Accel_magnitude = Accel_magnitude/4096; // Scale to gravity.
/* Dynamic weighting of accelerometer info (reliability filter)
* Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) */
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],filterdata->Kp_rollPitch*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],filterdata->Ki_rollPitch*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
/*********** YAW ***************/
//#if FILTER_USE_MAGNETOMETER==1
//// We make the gyro YAW drift correction based on compass magnetic heading
//#if BOARD_VERSION < 3
//errorCourse=(DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error
//#endif
//#if BOARD_VERSION == 3
//errorCourse=(DCM_Matrix[0][0]*Heading_Y) - (DCM_Matrix[1][0]*Heading_X); //Calculating YAW error
//#endif
//Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
//
//Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
//Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
//
//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
//#else // Use GPS Ground course to correct yaw gyro drift
//if(GPS.ground_speed>=SPEEDFILT*100) // Ground speed from GPS is in m/s
//{
//COGX = cos(ToRad(GPS.ground_course/100.0));
//COGY = sin(ToRad(GPS.ground_course/100.0));
//errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
//Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
//
//Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
//Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
//
//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
//// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
//Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
//if (Integrator_magnitude > ToRad(300)) {
//Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
//}
}
