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 Vector_Normalize(Vector3f *a) { float mag = sqrtf(Vector_Dot_Product(a,a)); a->x /= mag; a->y /= mag; a->z /= mag; }
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_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); } }