// Read every sensor and record a time stamp
// Init DCM with unfiltered orientation
// TODO re-init global vars?
void reset_sensor_fusion() {
float temp1[3];
float temp2[3];
float xAxis[] = { 1.0f, 0.0f, 0.0f };
read_sensors();
timestamp = millis();
// GET PITCH
// Using y-z-plane-component/x-component of gravity vector
pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));
// GET ROLL
// Compensate pitch of gravity vector
Vector_Cross_Product(temp1, accel, xAxis);
Vector_Cross_Product(temp2, xAxis, temp1);
// Normally using x-z-plane-component/y-component of compensated gravity vector
// roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
// Since we compensated for pitch, x-z-plane-component equals z-component:
roll = atan2(temp2[1], temp2[2]);
// GET YAW
Compass_Heading();
yaw = MAG_Heading;
// Init rotation matrix
init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}
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 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 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 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 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
}
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);
}
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;
}
}
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 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(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);
}
}
/**************************************************************************
* \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);
//}
}
