Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

remove trailing whitespaces... again...

Browse Source
This commit is contained in:
Felix Ruess
2010-12-09 12:51:38 +01:00
parent bbba46bce7
commit 2fa31c507f
2 changed files with 62 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/dcm/analogimu.c
+10 -10
View File
@@ -1,6 +1,6 @@
/*
* $Id: analogimu.c $
*
*
* Copyright (C) 2010 Oliver Riesener, Christoph Niemann
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*
*/
@@ -65,7 +65,7 @@ float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
/**
* accel2ms2():
*
* \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
* \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
*/
static void accel2ms2( void ) {
accel[ACC_X] = (float)(adc_average[3]) * IMU_ACCEL_X_SENS;
@@ -75,7 +75,7 @@ static void accel2ms2( void ) {
/**
* gyro2rads():
*
* \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
* \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
*/
static void gyro2rads( void ) {
/** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
@@ -84,7 +84,7 @@ static void gyro2rads( void ) {
gyro[G_YAW] = (float)(adc_average[2]) * IMU_GYRO_R_SENS;
}
void analog_imu_init( void ) {
void analog_imu_init( void ) {
imu_impl_init();
}
@@ -99,13 +99,13 @@ void analog_imu_offset_set( void ) {
}
// Z channel should read
analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS);
analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS);
}
/**
* analog_imu_update():
*/
void analog_imu_update( void ) {
void analog_imu_update( void ) {
uint8_t i;
// read IMU
@@ -121,7 +121,7 @@ void analog_imu_update( void ) {
// functions
void analog_imu_downlink( void ) {
void analog_imu_downlink( void ) {
//uint8_t id = 0;
//float time = GET_CUR_TIME_FLOAT();
//time *= 1000;//secs to msecs
@@ -140,12 +140,12 @@ void estimator_update_state_analog_imu( void ) {
Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
Gyro_Vector[1]= -gyro_to_zero[G_PITCH] + gyro[G_PITCH];
Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];
Accel_Vector[0] = accel[ACC_X];
Accel_Vector[1] = accel[ACC_Y];
Accel_Vector[2] = accel[ACC_Z];
Matrix_update();
Normalize();
sw/airborne/subsystems/ahrs/dcm/dcm.c
+52 -52
View File
@@ -99,23 +99,23 @@ void Normalize(void)
float temporary[3][3];
float renorm=0;
boolean problem=FALSE;
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]);
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);
#if PERFORMANCE_REPORTING == 1
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
#if PRINT_DEBUG != 0
@@ -124,8 +124,8 @@ void Normalize(void)
Serial.print (",ERR:");
Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
} else {
problem = TRUE;
@@ -138,18 +138,18 @@ void Normalize(void)
Serial.print (",ERR:");
Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
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= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
#if PRINT_DEBUG != 0
@@ -158,8 +158,8 @@ void Normalize(void)
Serial.print (",ERR:");
Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
} else {
problem = TRUE;
@@ -172,18 +172,18 @@ void Normalize(void)
Serial.print (",ERR:");
Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
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= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
#if PRINT_DEBUG != 0
@@ -192,11 +192,11 @@ void Normalize(void)
Serial.print (",ERR:");
Serial.print (error);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
} else {
problem = TRUE;
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
@@ -204,12 +204,12 @@ void Normalize(void)
Serial.print("???PRB:3,RNM:");
Serial.print (renorm);
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
}
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;
@@ -220,7 +220,7 @@ void Normalize(void)
DCM_Matrix[2][0]= 0.0f;
DCM_Matrix[2][1]= 0.0f;
DCM_Matrix[2][2]= 1.0f;
problem = FALSE;
problem = FALSE;
}
}
@@ -240,7 +240,7 @@ float speed_3d = 0;
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
@@ -268,34 +268,34 @@ void Drift_correction(void)
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); //
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#if PERFORMANCE_REPORTING == 1
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_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);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
#if USE_MAGNETOMETER==1
#if USE_MAGNETOMETER==1
// 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);
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
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_mode==3 && ground_speed>= 0.5) //hwarm
{
@@ -304,12 +304,12 @@ void Drift_correction(void)
COGY = sin(RadOfDeg(ground_course));
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
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
@@ -321,12 +321,12 @@ void Drift_correction(void)
Serial.print (ToDeg(Integrator_magnitude));
Serial.print (",TOW:");
Serial.print (iTOW);
Serial.println("***");
Serial.print (iTOW);
Serial.println("***");
#endif
}
}
/**************************************************/
@@ -338,11 +338,11 @@ void Matrix_update(void)
if (gps_mode==3) //Remove centrifugal acceleration.
{
Accel_Vector[1] += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
}
#if OUTPUTMODE==1 // With corrected data (drift correction)
#if OUTPUTMODE==1 // With corrected data (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
@@ -371,7 +371,7 @@ void Matrix_update(void)
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
}