diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml index e780a3ad43..8b9264005a 100644 --- a/conf/airframes/example_twog_analogimu.xml +++ b/conf/airframes/example_twog_analogimu.xml @@ -29,7 +29,7 @@ - + @@ -45,22 +45,39 @@ - - - + + + + + + + + - - - + + + + + + + + + + + + + + +
@@ -151,7 +168,7 @@ - +
@@ -171,29 +188,29 @@
- - - - - + + + + +
- - + + - + - - + + - + @@ -201,6 +218,8 @@ + + @@ -208,16 +227,6 @@ ap.CFLAGS += -DANALOGIMU_ROTATED ap.CFLAGS += -DANALOGIMU_ZERO_AVERAGE - - ap.CFLAGS += -DGPS_USE_LATLONG - - //ap.CFLAGS += -DWIND_INFO -DSTRONG_WIND - - //sim.CFLAGS += -DWIND_INFO -DSTRONG_WIND - - - - diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile deleted file mode 100644 index 09a0e4f3ac..0000000000 --- a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile +++ /dev/null @@ -1,35 +0,0 @@ -# attitude via analog imu - - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -DANALOG_IMU -ap.CFLAGS += -DADC -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_ADC_$(GYRO_R) -ap.CFLAGS += -DUSE_ADC_3 -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z) - -ap.CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R) -ap.CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z) - -ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c - -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/dcm.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu_util.c - -endif - -# since there is currently no SITL sim for the Analog IMU, we use the infrared sim - -ifeq ($(TARGET), sim) - -sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0 -sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0 - -$(TARGET).CFLAGS += -DUSE_INFRARED -$(TARGET).srcs += subsystems/sensors/infrared.c - -sim.srcs += $(SRC_ARCH)/sim_ir.c -sim.srcs += $(SRC_ARCH)/sim_analogimu.c - -endif - -jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile new file mode 100644 index 0000000000..cdc8ee2bb7 --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile @@ -0,0 +1,28 @@ +# attitude estimation for fixedwings via dcm algorithm + + +ifeq ($(ARCH), lpc21) +ap.CFLAGS += -DUSE_ANALOG_IMU + +ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c +ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c +ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c + +endif + +# since there is currently no SITL sim for the Analog IMU, we use the infrared sim + +ifeq ($(TARGET), sim) + +sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0 +sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0 + +sim.CFLAGS += -DUSE_INFRARED +sim.srcs += subsystems/sensors/infrared.c + +sim.srcs += $(SRC_ARCH)/sim_ir.c +sim.srcs += $(SRC_ARCH)/sim_analogimu.c + +endif + +jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile new file mode 100644 index 0000000000..b319b6d507 --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile @@ -0,0 +1,62 @@ +# +# Analog IMU connected to MCU ADC ports +# +# +# +# +# +# +# +# +# +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# + + +ifeq ($(ARCH), lpc21) +imu_CFLAGS += -DADC +imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_ADC_$(GYRO_R) +imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z) + +imu_CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R) +imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z) + +imu_srcs += $(SRC_SUBSYSTEMS)/imu.c +imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c +imu_srcs += math/pprz_trig_int.c + +endif + +# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets +# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example +ap.CFLAGS += $(imu_CFLAGS) +ap.srcs += $(imu_srcs) diff --git a/conf/settings/tuning_analog_imu.xml b/conf/settings/tuning_analog_imu.xml index 86a4e8ed1b..df7bfafe91 100644 --- a/conf/settings/tuning_analog_imu.xml +++ b/conf/settings/tuning_analog_imu.xml @@ -46,9 +46,9 @@ --> - - - + + + diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 16ee8ead57..faf12e3fe6 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -65,8 +65,13 @@ #endif -#ifdef ANALOG_IMU -#include "subsystems/ahrs/dcm/analogimu.h" +#ifdef USE_ANALOG_IMU +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" +#include "subsystems/ahrs/ahrs_float_dcm.h" +#include "subsystems/imu/imu_analog.h" +static inline void on_gyro_accel_event( void ); +static inline void on_mag_event( void ); #endif #if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY @@ -435,12 +440,11 @@ void periodic_task_ap( void ) { #error "Only 20 and 60 allowed for CONTROL_RATE" #endif -#ifdef ANALOG_IMU +#ifdef USE_ANALOG_IMU if (!_20Hz) { - estimator_update_state_analog_imu(); - analog_imu_downlink(); - } -#endif // ANALOG_IMU + imu_periodic(); + } +#endif // USE_ANALOG_IMU #if CONTROL_RATE == 20 if (!_20Hz) @@ -495,9 +499,10 @@ void init_ap( void ) { GpioInit(); #endif - -#ifdef ANALOG_IMU - analog_imu_init(); +#ifdef USE_ANALOG_IMU + imu_init(); + ahrs_aligner_init(); + ahrs_init(); #endif /************* Links initialization ***************/ @@ -548,18 +553,16 @@ void init_ap( void ) { #ifdef TRAFFIC_INFO traffic_info_init(); #endif - -#ifdef ANALOG_IMU - //wait 10secs for init - sys_time_usleep(10000000); - analog_imu_offset_set(); -#endif } /*********** EVENT ***********************************************************/ void event_task_ap( void ) { +#ifdef USE_ANALOG_IMU + ImuEvent(on_gyro_accel_event, on_mag_event); +#endif // USE_ANALOG_IMU + #ifdef USE_GPS #if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */ if (GpsBuffer()) { @@ -630,3 +633,30 @@ void event_task_ap( void ) { modules_event_task(); } /* event_task_ap() */ + +#ifdef USE_ANALOG_IMU +static inline void on_gyro_accel_event( void ) { + ImuScaleGyro(imu); + ImuScaleAccel(imu); + if (ahrs.status == AHRS_UNINIT) { + ahrs_aligner_run(); + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + ahrs_align(); + } + else { + ahrs_propagate(); + ahrs_update_accel(); + ahrs_update_fw_estimator(); + } +} + +static inline void on_mag_event(void) { + /* + ImuScaleMag(imu); + if (ahrs.status == AHRS_RUNNING) { + ahrs_update_mag(); + ahrs_update_fw_estimator(); + } + */ +} +#endif // USE_ANALOG_IMU diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index 1c689de290..7b3630e4cb 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -91,7 +91,7 @@ void ahrs_aligner_run(void) { ahrs_aligner.low_noise_cnt++; else if ( ahrs_aligner.low_noise_cnt > 0) - ahrs_aligner.low_noise_cnt--; + ahrs_aligner.low_noise_cnt--; if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) { ahrs_aligner.status = AHRS_ALIGNER_LOCKED; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c new file mode 100644 index 0000000000..0af2996e16 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -0,0 +1,403 @@ +/* + * Released under Creative Commons License + * + * 2010 The Paparazzi Team + * + * + * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun). + * Version 1.0 for flat board updated by Doug Weibel and Jose Julio + * + * Modified at Hochschule Bremen, Germany + * 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener + * + */ + +/** \file ahrs_float_dcm.c + * \brief Attitude estimation for fixedwings based on the DCM + * + */ + +#include "std.h" + +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_float_dcm.h" +#include "subsystems/ahrs/ahrs_float_utils.h" +#include "subsystems/ahrs/ahrs_aligner.h" +#include "subsystems/imu.h" + +#include "subsystems/ahrs/ahrs_float_dcm_algebra.h" + +#ifdef USE_GPS +#include "gps.h" +#endif + +#include + +//FIXME this is still needed for fixedwing integration +#include "estimator.h" + +struct AhrsFloatDCM ahrs_impl; + +// remotely settable +float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT); +float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT); + +// 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; + +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 +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}}; + +#ifdef USE_MAGNETOMETER +float MAG_Heading; +#endif + +static inline void compute_body_orientation_and_rates(void); +void Normalize(void); +void Drift_correction(void); +void Euler_angles(void); +void Matrix_update(void); + +/**************************************************/ + +void ahrs_update_fw_estimator( void ) +{ + Euler_angles(); + + //warning, only eulers written to ahrs struct so far + //compute_body_orientation_and_rates(); + + // export results to estimator + estimator_phi = ahrs_float.ltp_to_imu_euler.phi - imu_roll_neutral; + estimator_theta = ahrs_float.ltp_to_imu_euler.theta - imu_pitch_neutral; + estimator_psi = ahrs_float.ltp_to_imu_euler.psi; +} + + +void ahrs_init(void) { + ahrs_float.status = AHRS_UNINIT; + + /* + * Initialises our IMU alignement variables + * This should probably done in the IMU code instead + */ + struct FloatEulers body_to_imu_euler = + {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; + FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); + FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); + + /* set ltp_to_body to zero */ + FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); + FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); + FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); + FLOAT_RATES_ZERO(ahrs_float.body_rate); + + /* set ltp_to_imu so that body is zero */ + QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); + EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); + FLOAT_RATES_ZERO(ahrs_float.imu_rate); +} + +void ahrs_align(void) +{ + /* Compute an initial orientation using euler angles */ + ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + + /* Convert initial orientation in quaternion and rotation matrice representations. */ + FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); + FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); + + /* Compute initial body orientation */ + compute_body_orientation_and_rates(); + + /* use averaged gyro as initial value for bias */ + struct Int32Rates bias0; + RATES_COPY(bias0, ahrs_aligner.lp_gyro); + RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); + + ahrs.status = AHRS_RUNNING; +} + + +void ahrs_propagate(void) +{ + /* convert imu data to floating point */ + struct FloatRates gyro_float; + RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); + + /* unbias rate measurement */ + RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias); + + Matrix_update(); + Normalize(); + //INFO, ahrs struct only updated in ahrs_update_fw_estimator +} + +void ahrs_update_accel(void) +{ + ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + +#ifdef USE_GPS + if (gps_mode==3) { //Remove centrifugal acceleration. + accel_float.y += gps_speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ + accel_float.z -= gps_speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY + } +#endif + + Drift_correction(); +} + +void ahrs_update_mag(void) +{ + //TODO +} + +void Normalize(void) +{ + float error=0; + float temporary[3][3]; + float renorm=0; + boolean 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! + 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 = 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); + 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_mode==3 && gps_gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s + float ground_course = gps_course/10. - 180.; //This is the runaway direction of you "plane" in degrees + float COGX = cos(RadOfDeg(ground_course)); //Course overground X axis + float COGY = sin(RadOfDeg(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 Matrix_update(void) +{ + Vector_Add(&Omega[0], &ahrs_float.imu_rate.p, &Omega_I[0]); //adding proportional term + Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term + + #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 + Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z + Update_Matrix[1][1]=0; + Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x + Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y + Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x + Update_Matrix[2][2]=0; + #else // Uncorrected data (no drift correction) + Update_Matrix[0][0]=0; + Update_Matrix[0][1]=-G_Dt*ahrs_float.imu_rate.r;//-z + Update_Matrix[0][2]=G_Dt*ahrs_float.imu_rate.q;//y + Update_Matrix[1][0]=G_Dt*ahrs_float.imu_rate.r;//z + Update_Matrix[1][1]=0; + Update_Matrix[1][2]=-G_Dt*ahrs_float.imu_rate.p; + Update_Matrix[2][0]=-G_Dt*ahrs_float.imu_rate.q; + Update_Matrix[2][1]=G_Dt*ahrs_float.imu_rate.p; + Update_Matrix[2][2]=0; + #endif + + Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c + + for(int x=0; x<3; x++) //Matrix Addition (update) + { + for(int y=0; y<3; y++) + { + DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; + } + } +} + +void Euler_angles(void) +{ +#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) + ahrs_float.ltp_to_imu_euler.phi = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) + ahrs_float.ltp_to_imu_euler.theta = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x) + ahrs_float.ltp_to_imu_euler.psi = 0; +#else + ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); + ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); + ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); + ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ +#endif +} + +/* + * Compute body orientation and rates from imu orientation and rates + */ +static inline void compute_body_orientation_and_rates(void) { + + FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat, + ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); + FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat, + ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); + FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat); + FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate); + +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h new file mode 100644 index 0000000000..d088612961 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -0,0 +1,68 @@ +/* + * Released under Creative Commons License + * + * 2010 The Paparazzi Team + * + * + * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun). + * Version 1.0 for flat board updated by Doug Weibel and Jose Julio + * + * Modified at Hochschule Bremen, Germany + * 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener + * + */ + +/** \file ahrs_float_dcm.h + * \brief Attitude estimation for fixedwings based on the DCM + * + */ + +#ifndef AHRS_FLOAT_DCM_H +#define AHRS_FLOAT_DCM_H + +#include +#include "math/pprz_algebra_float.h" + +struct AhrsFloatDCM { + struct FloatRates gyro_bias; + struct FloatRates rate_correction; + /* + Holds float version of IMU alignement + in order to be able to run against the fixed point + version of the IMU + */ + struct FloatQuat body_to_imu_quat; + struct FloatRMat body_to_imu_rmat; +}; +extern struct AhrsFloatDCM ahrs_impl; + +extern float imu_roll_neutral; +extern float imu_pitch_neutral; + +void ahrs_update_fw_estimator(void); + +// DCM Parameters + +//#define Kp_ROLLPITCH 0.2 +#define Kp_ROLLPITCH 0.015 +#define Ki_ROLLPITCH 0.000010 +#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution! +#define Ki_YAW 0.00005 + +#define GRAVITY 9.81 + + +#define OUTPUTMODE 1 +// Mode 0 = DCM integration without Ki gyro bias +// Mode 1 = DCM integration with Kp and Ki +// Mode 2 = direct accelerometer -> euler + + +#define PERFORMANCE_REPORTING 0 +#if PERFORMANCE_REPORTING == 1 +extern int renorm_sqrt_count; +extern int renorm_blowup_count; +extern float imu_health; +#endif + +#endif // AHRS_FLOAT_DCM_H diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h new file mode 100644 index 0000000000..c639ad12b1 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h @@ -0,0 +1,57 @@ +//Algebra helper functions for DCM + +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]; +} + +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]); +} + +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]; + } + } +} diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.c b/sw/airborne/subsystems/ahrs/dcm/analogimu.c deleted file mode 100644 index 56779d822f..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c +++ /dev/null @@ -1,164 +0,0 @@ -/* - * $Id: analogimu.c $ - * - * Copyright (C) 2010 Oliver Riesener, Christoph Niemann - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * 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. - * - */ - -/** \file analogimu.c - * \brief Analog IMU Routines - * - */ - -#include "generated/airframe.h" - -#if ! (defined SITL || defined HITL) - - -// Actual Inertial Measurements -#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 "sys_time.h" - - -#endif - -// variables -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); -float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT); - -#if ! (defined SITL || defined HITL) - -// functions -/** - * accel2ms2(): - * - * \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; - accel[ACC_Y] = (float)(adc_average[4]) * IMU_ACCEL_Y_SENS; - accel[ACC_Z] = (float)(adc_average[5]) * IMU_ACCEL_Z_SENS; -} -/** - * gyro2rads(): - * - * \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 */ - gyro[G_ROLL] = (float)(adc_average[0]) * IMU_GYRO_P_SENS; - gyro[G_PITCH] = (float)(adc_average[1]) * IMU_GYRO_Q_SENS; - gyro[G_YAW] = (float)(adc_average[2]) * IMU_GYRO_R_SENS; -} - -void analog_imu_init( void ) { - imu_impl_init(); -} - -void analog_imu_offset_set( void ) { - uint8_t i; - - // read IMU - imu_periodic(); - - for(i = 0; i < NB_ANALOG_IMU_ADC; i++) { - analog_imu_offset[i] = analog_imu_values[i]; - } - - // Z channel should read - analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS); -} -/** - * analog_imu_update(): - */ - -void analog_imu_update( void ) { - uint8_t i; - - // read IMU - imu_periodic(); - - for(i = 0; i < NB_ANALOG_IMU_ADC; i++) { - adc_average[i] -= analog_imu_offset[i]; - } - - accel2ms2(); - gyro2rads(); -} - -// functions - -void analog_imu_downlink( void ) { - //uint8_t id = 0; - //float time = GET_CUR_TIME_FLOAT(); - //time *= 1000;//secs to msecs - //int mx = 0; - //int my = 0; - //int mz = 0; - //DOWNLINK_SEND_HB_FILTER( DefaultChannel,&time, &accel[ACC_X],&accel[ACC_Y],&accel[ACC_Z],&gyro[G_ROLL],&gyro[G_PITCH],&gyro[G_YAW],&heading,&mx,&my,&mz,&euler[EULER_ROLL],&euler[EULER_PITCH],&euler[EULER_YAW], &imu_roll_neutral, &imu_pitch_neutral ); -} - - -void estimator_update_state_analog_imu( void ) { - - analog_imu_update(); - - /* 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]; - - Accel_Vector[0] = accel[ACC_X]; - Accel_Vector[1] = accel[ACC_Y]; - Accel_Vector[2] = accel[ACC_Z]; - - - Matrix_update(); - Normalize(); - - - Drift_correction(); - Euler_angles(); - - // return euler angles to phi and theta - 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; - -} -#endif diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.h b/sw/airborne/subsystems/ahrs/dcm/analogimu.h deleted file mode 100644 index e3b0b13cea..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * $Id: analogimu.h $ - * - * Copyright (C) 2010 Oliver Riesener, Christoph Niemann - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * 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. - * - */ - -/** \file analogimu.h - * \brief Analog IMU Interface - * - */ - -#ifndef _ANALOGIMU_H_ -#define _ANALOGIMU_H_ - -#include - -extern float imu_roll_neutral; -extern float imu_pitch_neutral; - -//functions -void analog_imu_init( void ); -void analog_imu_update( void ); -void analog_imu_downlink( void ); -void analogconversion( void ); -void estimator_update_state_analog_imu( void ); -void analog_imu_offset_set( void ); - -#endif // _ANALOGIMU_H_ diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu_util.c b/sw/airborne/subsystems/ahrs/dcm/analogimu_util.c deleted file mode 100644 index dd69f09177..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu_util.c +++ /dev/null @@ -1,100 +0,0 @@ -/* - * $Id: analogimu_util.c $ - * - * Copyright (C) 2010 Christoph Niemann - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * 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. - * - */ - -/** \file analogimu_util.c - * \brief Analog IMU Utilities - * - */ - -#include "mcu_periph/adc.h" -#include "mcu_periph/uart.h" - -#include "std.h" -#include "subsystems/nav.h" -#include "estimator.h" -#include "autopilot.h" -#include "generated/flight_plan.h" -#include "generated/airframe.h" -#include "inter_mcu.h" - -#include "analogimu_util.h" - - -uint16_t analog_raw[NB_ADC]; - -// variables -/** gyro rate in rad */ -volatile float gyro[G_LAST] = {0.}; -/** gyro rate offset in rad */ -volatile float gyro_to_zero[G_LAST] = {0.}; -/** acceleration in ms2 */ -volatile float accel[ACC_LAST] = {0.}; -/** angle in rad */ -volatile float angle[ANG_LAST] = {0.}; -/** magnet heading \todo heading ? */ -volatile float heading; - -void analogimu_delay( void ) { - volatile int i,j; - for (i=0;i<1000;i++) - for (j=0;j<1000;j++); -} - -bool_t analog_imu_reset( void ) { - #ifndef ANALOGIMU_ZERO_AVERAGE - gyro_to_zero[GO_ROLL] = gyro[G_ROLL]; - gyro_to_zero[GO_PITCH] = gyro[G_PITCH]; - gyro_to_zero[GO_YAW] = gyro[G_YAW]; - #else - /** temp_value for calculating the average */ - volatile float gyro_to_zero_temp[G_LAST] = {0.}; - - gyro_to_zero_temp[GO_ROLL] = gyro[G_ROLL]; - gyro_to_zero_temp[GO_PITCH] = gyro[G_PITCH]; - gyro_to_zero_temp[GO_YAW] = gyro[G_YAW]; - - analogimu_delay(); - - gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL]; - gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH]; - gyro_to_zero_temp[GO_YAW] += gyro[G_YAW]; - - analogimu_delay(); - - gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL]; - gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH]; - gyro_to_zero_temp[GO_YAW] += gyro[G_YAW]; - - gyro_to_zero_temp[GO_ROLL] /= 3; - gyro_to_zero_temp[GO_PITCH] /= 3; - gyro_to_zero_temp[GO_YAW] /= 3; - - gyro_to_zero[GO_ROLL] = gyro_to_zero_temp[G_ROLL]; - gyro_to_zero[GO_PITCH] = gyro_to_zero_temp[G_PITCH]; - gyro_to_zero[GO_YAW] = gyro_to_zero_temp[G_YAW]; - #endif - return FALSE; -} - - diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu_util.h b/sw/airborne/subsystems/ahrs/dcm/analogimu_util.h deleted file mode 100644 index cffb67e6f5..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu_util.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * $Id: analogimu_imu.h $ - * - * Copyright (C) 2010 Christoph Niemann - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * 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. - * - */ - -/** \file analogimu_util.h - * \brief Analog IMU Utilities - * - */ - -#ifndef _ANALOGIMU_UTIL_H_ -#define _ANALOGIMU_UTIL_H_ - -#include "std.h" - -// defines -/** defines for gyro[] indicies */ -enum gyro_idx_t { G_ROLL, G_PITCH, G_YAW, G_LAST }; -/** defines for accel[] indicies */ -enum accel_idx_t { ACC_X, ACC_Y, ACC_Z, ACC_LAST }; -/** defines for stick[] indicies */ -enum stick_idx_t { STICK_ROLL, STICK_PITCH, STICK_YAW, STICK_THRUST, - STICK_SWR, STICK_SWL, STICK_CH6, STICK_CH7, STICK_LAST }; -/** defines for angle[] indicies */ -enum angle_idx_t { ANG_ROLL, ANG_PITCH, ANG_YAW, ANG_LAST }; -/** defines for gyro_to_zero[] indicies */ -enum gyro_to_zero_idx_t { GO_ROLL, GO_PITCH, GO_YAW, GO_LAST }; - -// variables -extern volatile float gyro[]; -/** acceleration in ms2 */ -extern volatile float accel[]; -/** angle in rad */ -extern volatile float angle[]; -/** magnet heading \todo heading ? */ -extern volatile float heading; -/** analog_raw[] analog downlink arry */ -extern uint16_t analog_raw[]; - -extern volatile float gyro_to_zero[]; - -extern bool_t analog_imu_reset( void ); - -void analogimu_delay( void ); - - -#endif diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.c b/sw/airborne/subsystems/ahrs/dcm/dcm.c deleted file mode 100644 index 26a038f46e..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/dcm.c +++ /dev/null @@ -1,330 +0,0 @@ - -#include "std.h" - -#include "dcm.h" - -// 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 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}}; - -struct FloatEulers euler= {0, 0, 0}; -float speed_3d = 0; - - -/**************************************************/ - -// Algebra - -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]; -} - -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]); -} - -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; - 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]); - 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); - - 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); - - 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); - - 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 = FALSE; - } -} - -/**************************************************/ - extern short gps_course; - extern short gps_gspeed; - extern short gps_climb; - extern short gps_mode; - -#ifdef USE_MAGNETOMETER -float MAG_Heading; -#endif - - -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; - float ground_speed; // This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots - float ground_course; //This is the runaway direction of you "plane" in degrees - float COGX; //Course overground X axis - float COGY; //Course overground Y axis - - // hwarm - ground_course=gps_course/10.-180.; - ground_speed=gps_gspeed/100.; - float ground_climb=gps_climb/100.; - speed_3d = sqrt(ground_speed*ground_speed+ground_climb*ground_climb); - //*****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 = 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_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); - - //*****YAW*************** - - #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 - #else // Use GPS Ground course to correct yaw gyro drift - if(gps_mode==3 && ground_speed>= 0.5) //hwarm - { - - COGX = cos(RadOfDeg(ground_course)); - 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 - } - #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 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) //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 - } - - - #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 - Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x - Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y - Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x - 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[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[2][2]=0; - #endif - - Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c - - for(int x=0; x<3; x++) //Matrix Addition (update) - { - for(int y=0; y<3; y++) - { - DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; - } - } -} - -void Euler_angles(void) -{ - #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - euler.phi = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) - euler.theta = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x) - euler.psi = 0; - #else - euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - euler.theta = -asin(DCM_Matrix[2][0]); - euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - euler.psi += 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 deleted file mode 100644 index 0cb1dda865..0000000000 --- a/sw/airborne/subsystems/ahrs/dcm/dcm.h +++ /dev/null @@ -1,48 +0,0 @@ - -#include "math/pprz_algebra_float.h" - -// Inputs for DCM -extern float Gyro_Vector[3]; -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); -extern struct FloatEulers euler; - -// DCM Parameters - -//#define Kp_ROLLPITCH 0.2 -#define Kp_ROLLPITCH 0.015 -#define Ki_ROLLPITCH 0.000010 -#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution! -#define Ki_YAW 0.00005 - -#define GRAVITY 9.81 - - -#define OUTPUTMODE 1 -// Mode 0 = DCM integration without Ki gyro bias -// Mode 1 = DCM integration with Kp and Ki -// Mode 2 = direct accelerometer -> euler - -#define MAGNETOMETER 1 -extern float MAG_Heading; - -#define PERFORMANCE_REPORTING 1 -#if PERFORMANCE_REPORTING == 1 -extern int renorm_sqrt_count; -extern int renorm_blowup_count; -extern float imu_health; -#endif - - - diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index b41aeaa53f..e6baede3e8 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -23,8 +23,6 @@ #include "subsystems/imu.h" -#include "generated/airframe.h" - struct Imu imu; void imu_init(void) { @@ -32,6 +30,7 @@ void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); + //FIXME should not assume that every imu has a mag and this id defined? VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); /* diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h index cffa2b6f4a..57d1c4135c 100644 --- a/sw/airborne/subsystems/imu.h +++ b/sw/airborne/subsystems/imu.h @@ -26,6 +26,7 @@ #include "math/pprz_algebra_int.h" #include "math/pprz_algebra_float.h" +#include "generated/airframe.h" /* must be defined by underlying hardware */ extern void imu_impl_init(void); diff --git a/sw/airborne/subsystems/imu/imu_analog.c b/sw/airborne/subsystems/imu/imu_analog.c index 0e9b0230e8..32295cdf9d 100644 --- a/sw/airborne/subsystems/imu/imu_analog.c +++ b/sw/airborne/subsystems/imu/imu_analog.c @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. * @@ -22,13 +20,10 @@ */ #include "imu_analog.h" -#include "generated/airframe.h" #include "mcu_periph/adc.h" -#include "subsystems/imu.h" #include "mcu_periph/uart.h" volatile bool_t analog_imu_available; -uint16_t analog_imu_values[NB_ANALOG_IMU_ADC]; static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC]; @@ -46,10 +41,12 @@ void imu_impl_init(void) { } void imu_periodic(void) { - uint8_t i; - for(i = 0; i < NB_ANALOG_IMU_ADC; i++) { - analog_imu_values[i] = analog_imu_adc_buf[i].sum / analog_imu_adc_buf[i].av_nb_sample; - } - + imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES; + imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES; + imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES; + imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES; + imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES; + imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES; + analog_imu_available = TRUE; } diff --git a/sw/airborne/subsystems/imu/imu_analog.h b/sw/airborne/subsystems/imu/imu_analog.h index 28de9be489..c0ef110251 100644 --- a/sw/airborne/subsystems/imu/imu_analog.h +++ b/sw/airborne/subsystems/imu/imu_analog.h @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. * @@ -28,30 +26,23 @@ #define NB_ANALOG_IMU_ADC 6 -extern uint16_t analog_imu_values[NB_ANALOG_IMU_ADC]; extern volatile bool_t analog_imu_available; -#define ImuEvent(_gyro_accel_handler, _mag_handler) { \ - if (ADSanalog_imu_available) { \ - analog_imu_available = FALSE; \ - imu.gyro_unscaled.p = analog_imu_values[0]; \ - imu.gyro_unscaled.q = analog_imu_values[1]; \ - imu.gyro_unscaled.r = analog_imu_values[2]; \ - imu.accel_unscaled.x = analog_imu_values[3]; \ - imu.accel_unscaled.y = analog_imu_values[4]; \ - imu.accel_unscaled.z = analog_imu_values[5]; \ - _gyro_accel_handler(); \ - } \ - ImuMagEvent(_mag_handler); \ +#define ImuEvent(_gyro_accel_handler, _mag_handler) { \ + if (analog_imu_available) { \ + analog_imu_available = FALSE; \ + _gyro_accel_handler(); \ + } \ + ImuMagEvent(_mag_handler); \ } -#define ImuMagEvent(_mag_handler) { \ - if (false) { \ - _mag_handler(); \ - } \ +#define ImuMagEvent(_mag_handler) { \ + if (0) { \ + _mag_handler(); \ + } \ } -#endif /* IMU_CRISTA_H */ +#endif /* IMU_ANALOG_H */