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 26 Packages Projects Releases Wiki Activity

Merge remote branch 'paparazzi/imu' into analogimu

Browse Source
This commit is contained in:
Felix Ruess
2010-12-09 22:11:19 +01:00
parent 4016064cdf 1096d4e1fb
commit 86b18fd20a
3 changed files with 17 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/dcm/analogimu.c
+2 -18
View File
@@ -90,39 +90,23 @@ void analog_imu_downlink( void ) {
void estimator_update_state_analog_imu( void ) {
/* Offset is set dynamic on Ground*/
/*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];*/
//FIXME run aligner to set gyro neutrals on ground
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
Gyro_Vector[0]= gyro_float.p;
Gyro_Vector[1]= gyro_float.q;
Gyro_Vector[2]= gyro_float.r;
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
Accel_Vector[0] = accel_float.x;
Accel_Vector[1] = accel_float.y;
Accel_Vector[2] = accel_float.z;
Matrix_update();
Normalize();
// FIXME convert gps data here
Drift_correction();
Euler_angles();
// return euler angles to phi and theta
// export results to estimator
estimator_phi = euler.phi-imu_roll_neutral;
//estimator_phi = angle[ANG_ROLL];
estimator_theta = euler.theta-imu_pitch_neutral;
//estimator_theta = angle[ANG_PITCH];
estimator_psi = euler.psi;
}
sw/airborne/subsystems/ahrs/dcm/dcm.c
+13 -13
View File
@@ -11,8 +11,8 @@
// DCM Working variables
float G_Dt=0.05;
float Gyro_Vector[3] = {0,0,0};
float Accel_Vector[3] = {0,0,0};
struct FloatRates gyro_float = {0,0,0};
struct FloatVect3 accel_float = {0,0,0};
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0}; //Omega Proportional correction
@@ -207,7 +207,7 @@ void Drift_correction(void)
//*****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 = 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)
@@ -222,7 +222,7 @@ void Drift_correction(void)
}
#endif
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
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);
@@ -270,13 +270,13 @@ void Drift_correction(void)
void Matrix_update(void)
{
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega[0], &gyro_float.p, &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
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_float.y += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
accel_float.z -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
}
@@ -292,13 +292,13 @@ void Matrix_update(void)
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[0][1]=-G_Dt*gyro_float.r;//-z
Update_Matrix[0][2]=G_Dt*gyro_float.q;//y
Update_Matrix[1][0]=G_Dt*gyro_float.r;//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[1][2]=-G_Dt*gyro_float.p;
Update_Matrix[2][0]=-G_Dt*gyro_float.q;
Update_Matrix[2][1]=G_Dt*gyro_float.p;
Update_Matrix[2][2]=0;
#endif
sw/airborne/subsystems/ahrs/dcm/dcm.h
+2 -2
View File
@@ -1,8 +1,8 @@
#include "math/pprz_algebra_float.h"
// Inputs for DCM
extern float Gyro_Vector[3];
extern float Accel_Vector[3];
extern struct FloatRates gyro_float;
extern struct FloatVect3 accel_float;
// Integrate Inertial
void Matrix_update(void);