From bbba46bce78376fc8530c478854c167a104cb08b Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 9 Dec 2010 11:35:33 +0100 Subject: [PATCH] Inline math, reused paparazzi math and defines, organize, remove unused --- .../fixedwing/attitude_analogimu.makefile | 6 +- sw/airborne/subsystems/ahrs/dcm/analogimu.c | 89 ++---------- sw/airborne/subsystems/ahrs/dcm/analogimu.h | 5 - sw/airborne/subsystems/ahrs/dcm/dcm.c | 134 +++++++++++------- sw/airborne/subsystems/ahrs/dcm/dcm.h | 30 ++-- sw/airborne/subsystems/ahrs/dcm/matrix.c | 24 ---- sw/airborne/subsystems/ahrs/dcm/matrix.h | 2 - sw/airborne/subsystems/ahrs/dcm/vector.c | 42 ------ sw/airborne/subsystems/ahrs/dcm/vector.h | 5 - 9 files changed, 122 insertions(+), 215 deletions(-) delete mode 100644 sw/airborne/subsystems/ahrs/dcm/matrix.c delete mode 100644 sw/airborne/subsystems/ahrs/dcm/matrix.h delete mode 100644 sw/airborne/subsystems/ahrs/dcm/vector.c delete mode 100644 sw/airborne/subsystems/ahrs/dcm/vector.h diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile index 05d8420ead..1371900722 100644 --- a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile +++ b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile @@ -4,9 +4,11 @@ ifeq ($(ARCH), lpc21) ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7 -ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/dcm.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/matrix.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//vector.c +ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/dcm.c +ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu.c +ap.srcs += $(SRC_FIXEDWING)/subsystems/imu/imu_analog.c +ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu_util.c -ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu.c $(SRC_FIXEDWING)/subsystems/imu/imu_analog.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu_util.c endif # since there is currently no SITL sim for the Analog IMU, we use the infrared sim diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.c b/sw/airborne/subsystems/ahrs/dcm/analogimu.c index 6f17c28197..68971aec06 100644 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c +++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.c @@ -26,6 +26,9 @@ * \brief Analog IMU Routines * */ + +#include "generated/airframe.h" + #if ! (defined SITL || defined HITL) @@ -33,22 +36,24 @@ #include "subsystems/imu/imu_analog.h" // AHRS attitude computations +#include "dcm.h" +#include "estimator.h" +#include "analogimu_util.h" +#include "analogimu.h" + +// Debugging and output #include "led.h" #include "mcu_periph/uart.h" //#include "downlink.h" //#include "ap_downlink.h" -#include "estimator.h" #include "sys_time.h" -#include "dcm.h" -#include "analogimu_util.h" -#include "analogimu.h" #endif // variables -uint16_t analog_imu_offset[NB_ANALOG_IMU_ADC] = {0,}; -int adc_average[16] = { 0 }; +uint16_t analog_imu_offset[NB_ANALOG_IMU_ADC]; +int adc_average[NB_ANALOG_IMU_ADC]; // remotely settable float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT); @@ -62,7 +67,7 @@ float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT); * * \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z] */ -void accel2ms2( void ) { +static void accel2ms2( void ) { accel[ACC_X] = (float)(adc_average[3]) * IMU_ACCEL_X_SENS; accel[ACC_Y] = (float)(adc_average[4]) * IMU_ACCEL_Y_SENS; accel[ACC_Z] = (float)(adc_average[5]) * IMU_ACCEL_Z_SENS; @@ -72,7 +77,7 @@ void accel2ms2( void ) { * * \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW] */ -void gyro2rads( void ) { +static void gyro2rads( void ) { /** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */ gyro[G_ROLL] = (float)(adc_average[0]) * IMU_GYRO_P_SENS; gyro[G_PITCH] = (float)(adc_average[1]) * IMU_GYRO_Q_SENS; @@ -114,9 +119,6 @@ void analog_imu_update( void ) { gyro2rads(); } -/** earth accelecation */ -volatile float g = 0.; - // functions void analog_imu_downlink( void ) { @@ -130,69 +132,7 @@ void analog_imu_downlink( void ) { } - -/** - * Minimalistic version to get angles from acceleration - * - * \todo why has this function 3 callers ? - * \return g, angle[ANG_ROLL], angle[ANG_PITCH] - */ -void accel2euler( void ) { - // Calculate g ( ||g_vec|| ) - g = sqrt(accel[ACC_X] * accel[ACC_X] + - accel[ACC_Y] * accel[ACC_Y] + - accel[ACC_Z] * accel[ACC_Z]); - if( g < 0.01 && g > -0.01 ) - { - g=0.01; - }else{ - //nothing - } - //values in radians -#define NEW -#ifdef OLD - angle[ANG_PITCH] = -asinf( accel[ACC_X] / g ); - angle[ANG_ROLL] = asinf( accel[ACC_Y] / g ); - angle[ANG_YAW] = 0.0; -#endif - -#ifdef NEW - - float a1 = accel[ACC_X] / -g; - - if(a1 > 1.0 && a1 >= 0.0){ - a1 = 1.0; - } else if(a1 < -1.0 && a1 < 0.0){ - a1 = -1.0; - } - - angle[ANG_PITCH] = asinf( a1 ); - - if(accel[ACC_Z] < 0 && angle[ANG_PITCH] > 0) angle[ANG_PITCH] = + 3.145/2 + (3.145/2 - angle[ANG_PITCH]); - else if (accel[ACC_Z] < 0 && angle[ANG_PITCH] < 0) angle[ANG_PITCH] = -3.145/2 - (3.145/2 + angle[ANG_PITCH]); - - - if( accel[ACC_Z] < 0.01 && accel[ACC_Z] > -0.01 ) - { - accel[ACC_Z]=0.01; - }else{ - //nothing - } - - - angle[ANG_ROLL] = atan2f( accel[ACC_Y], accel[ACC_Z] ); - //angle[ANG_PITCH] = -atan2f( accel[ACC_X] , accel[ACC_Z] ); - angle[ANG_YAW] = 0.0; -#endif -} - - void estimator_update_state_analog_imu( void ) { -#undef ANGLE_FROM_ACCEL -#ifdef ANGLE_FROM_ACCEL - estimator_phi = (float)(atan2f((float)((analog_raw[6]-510)),(float)(-(analog_raw[7]-510)))); - estimator_theta = (float)(atan2f((float)(-(analog_raw[5]-510)),(float)(-(analog_raw[7]-510)))); -#else analog_imu_update(); @@ -208,6 +148,8 @@ void estimator_update_state_analog_imu( void ) { Matrix_update(); Normalize(); + + Drift_correction(); Euler_angles(); @@ -218,6 +160,5 @@ void estimator_update_state_analog_imu( void ) { //estimator_theta = angle[ANG_PITCH]; estimator_psi = euler[EULER_YAW]; -#endif } #endif diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.h b/sw/airborne/subsystems/ahrs/dcm/analogimu.h index 6148e83eb0..e3b0b13cea 100644 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu.h +++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.h @@ -32,8 +32,6 @@ #include -#include "generated/airframe.h" - extern float imu_roll_neutral; extern float imu_pitch_neutral; @@ -42,10 +40,7 @@ void analog_imu_init( void ); void analog_imu_update( void ); void analog_imu_downlink( void ); void analogconversion( void ); -void accel2euler( void ); -void accel2ms2( void ); void estimator_update_state_analog_imu( void ); -void gyro2rads( void ); void analog_imu_offset_set( void ); #endif // _ANALOGIMU_H_ diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.c b/sw/airborne/subsystems/ahrs/dcm/dcm.c index eed0e00b60..46449771ca 100644 --- a/sw/airborne/subsystems/ahrs/dcm/dcm.c +++ b/sw/airborne/subsystems/ahrs/dcm/dcm.c @@ -1,48 +1,98 @@ -#include -#include "vector.h" -#include "matrix.h" - -#ifdef ANALOG_IMU -#include "analogimu.h" -#include "analogimu_util.h" -#endif // ANALOG_IMU +#include "std.h" #include "dcm.h" -// Own Math -#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) -#define abs(x) ((x)>0?(x):-(x)) - -// DCM Working variables -float Gyro_Vector[3] = {0,0,0}; -float Accel_Vector[3] = {0,0,0}; - // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. // Positive pitch : nose up // Positive roll : right wing down // Positive yaw : clockwise + +// DCM Working variables float G_Dt=0.05; -float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data -float Omega_P[3]= {0,0,0};//Omega Proportional correction -float Omega_I[3]= {0,0,0};//Omega Integrator -float Omega[3]= {0,0,0}; +float Gyro_Vector[3] = {0,0,0}; +float Accel_Vector[3] = {0,0,0}; +float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data +float Omega_P[3]= {0,0,0}; //Omega Proportional correction +float Omega_I[3]= {0,0,0}; //Omega Integrator +float Omega[3]= {0,0,0}; float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - - -/** -* Estimated angles as Euler -*/ float euler[EULER_LAST] = {0.}; /**************************************************/ + +// Algebra + +//Computes the dot product of two vectors +static inline float Vector_Dot_Product(float vector1[3],float vector2[3]) +{ + return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2]; +} + +//Computes the cross product of two vectors +static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) +{ + vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); + vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); + vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); +} + +//Multiply the vector by a scalar. +static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) +{ + vectorOut[0]=vectorIn[0]*scale2; + vectorOut[1]=vectorIn[1]*scale2; + vectorOut[2]=vectorIn[2]*scale2; +} + +static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) +{ + vectorOut[0]=vectorIn1[0]+vectorIn2[0]; + vectorOut[1]=vectorIn1[1]+vectorIn2[1]; + vectorOut[2]=vectorIn1[2]+vectorIn2[2]; +} + +/* +#define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \ + _m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \ + _m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \ + _m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \ + _m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \ + _m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \ + _m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \ + _m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \ + _m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \ + _m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \ + } +*/ + +static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) +{ + float op[3]; + for(int x=0; x<3; x++) + { + for(int y=0; y<3; y++) + { + for(int w=0; w<3; w++) + { + op[w]=a[x][w]*b[w][y]; + } + mat[x][y]=op[0]+op[1]+op[2]; + } + } +} + + + + + void Normalize(void) { float error=0; @@ -218,12 +268,12 @@ 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 = constrain(1 - 2*abs(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; - imu_health = constrain(imu_health,129,65405); + Bound(imu_health,129,65405); #endif Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference @@ -280,21 +330,18 @@ void Drift_correction(void) } /**************************************************/ -void Accel_adjust(void) -{ - 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 -} -/**************************************************/ - void Matrix_update(void) { 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 (gps_mode==3) Accel_adjust(); //Remove centrifugal acceleration. + 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 + } + -#define OUTPUTMODE 1 #if OUTPUTMODE==1 // With corrected data (drift correction) Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z @@ -330,25 +377,16 @@ void Matrix_update(void) void Euler_angles(void) { - //#define OUTPUTMODE 2 #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) euler[EULER_ROLL] = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) - //euler[EULER_PITCH] = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x) - //todo: chni:ordentlich lösen! - euler[EULER_PITCH] = -asin((Accel_Vector[0])/9.81); // asin(acc_x) + euler[EULER_PITCH] = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x) euler[EULER_YAW] = 0; #else //pitch = -asin(DCM_Matrix[2][0]); //roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); //yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - #ifndef ANALOGIMU_ROTATED - euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]); - euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - #else - euler[EULER_ROLL] = -asin(DCM_Matrix[2][0]); - euler[EULER_PITCH] = -atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - #endif - + euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]); + euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); euler[EULER_YAW] = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); euler[EULER_YAW] += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.h b/sw/airborne/subsystems/ahrs/dcm/dcm.h index 4ed6686b9f..19468c6d12 100644 --- a/sw/airborne/subsystems/ahrs/dcm/dcm.h +++ b/sw/airborne/subsystems/ahrs/dcm/dcm.h @@ -1,8 +1,3 @@ -void Euler_angles(void); -void Accel_adjust(void); -void Drift_correction(void); -void Normalize(void); - // Inputs for DCM extern float Gyro_Vector[3]; @@ -10,7 +5,17 @@ extern float Accel_Vector[3]; // Integrate Inertial void Matrix_update(void); +void Normalize(void); +// Input GPS/Pressure/Magnetometer data + +// Correct +void Drift_correction(void); + +// Get outputs +void Euler_angles(void); +enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST }; +extern float euler[3]; // DCM Parameters @@ -20,14 +25,13 @@ void Matrix_update(void); #define Kp_YAW 1.2 //High yaw drift correction gain - use with caution! #define Ki_YAW 0.00005 - - -/** defines for euler vector */ -enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST }; - -#define M_PI 3.14159265358979323846 #define GRAVITY 9.81 -/** output vector with angles in rad */ -extern float euler[EULER_LAST]; +#define OUTPUTMODE 1 +// Mode 0 = DCM integration without Ki gyro bias +// Mode 1 = DCM integration with Kp and Ki +// Mode 2 = direct accelerometer -> euler + + + diff --git a/sw/airborne/subsystems/ahrs/dcm/matrix.c b/sw/airborne/subsystems/ahrs/dcm/matrix.c deleted file mode 100644 index a959e40297..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/matrix.c +++ /dev/null @@ -1,24 +0,0 @@ -/**************************************************/ -//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). - -#include "matrix.h" - -void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) -{ - float op[3]; - for(int x=0; x<3; x++) - { - for(int y=0; y<3; y++) - { - for(int w=0; w<3; w++) - { - op[w]=a[x][w]*b[w][y]; - } - mat[x][y]=0; - mat[x][y]=op[0]+op[1]+op[2]; - - } - } -} - - diff --git a/sw/airborne/subsystems/ahrs/dcm/matrix.h b/sw/airborne/subsystems/ahrs/dcm/matrix.h deleted file mode 100644 index 0b6f1639dd..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/matrix.h +++ /dev/null @@ -1,2 +0,0 @@ - -void Matrix_Multiply(float a[3][3],float b[3][3],float mat[3][3]); diff --git a/sw/airborne/subsystems/ahrs/dcm/vector.c b/sw/airborne/subsystems/ahrs/dcm/vector.c deleted file mode 100644 index 9848e21ea3..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/vector.c +++ /dev/null @@ -1,42 +0,0 @@ -#include "vector.h" - -//Computes the dot product of two vectors -float Vector_Dot_Product(float vector1[3],float vector2[3]) -{ - float op=0; - - for(int c=0; c<3; c++) - { - op+=vector1[c]*vector2[c]; - } - - return op; -} - -//Computes the cross product of two vectors -void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) -{ - vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); - vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); - vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); -} - -//Multiply the vector by a scalar. -void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn[c]*scale2; - } -} - -void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) -{ - for(int c=0; c<3; c++) - { - vectorOut[c]=vectorIn1[c]+vectorIn2[c]; - } -} - - - diff --git a/sw/airborne/subsystems/ahrs/dcm/vector.h b/sw/airborne/subsystems/ahrs/dcm/vector.h deleted file mode 100644 index b9686543a1..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/vector.h +++ /dev/null @@ -1,5 +0,0 @@ - -void Vector_Add(float vectorOut[3],float vectorIn1[3],float vectorIn2[3]); -void Vector_Scale(float vectorOut[3],float vectorIn[3],float scale2); -void Vector_Cross_Product(float vectorOut[3],float v1[3],float v2[3]); -float Vector_Dot_Product(float vector1[3],float vector2[3]);