Vincent Soubirane
/
Projet_ATTITUDE_IMU
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // Réalisation du projet ATTITUDE IMU 00002 // Dernière Modification le 30.10.2021 00003 // Thanks to Jason Mar for his library and Sebastian Madgwick for his filter algorithm 00004 00005 00006 00007 #include "LSM9DS1.h" 00008 #include "mbed.h" 00009 #include <math.h> 00010 #define PI 3.14f 00011 00012 #include "Fusion.h" 00013 #include <stdio.h> 00014 00015 00016 00017 Serial pc(SERIAL_TX, SERIAL_RX); // sans la carte bluetooth 00018 //Serial pc(PC_10,PC_11); //avec la carte bluetooth 00019 00020 CircularBuffer <unsigned char,1024> buf; 00021 Timer temps; 00022 DigitalOut led1(LED1); 00023 DigitalOut led2(LED2); 00024 LSM9DS1 lol(PB_9,PB_8, 0xD4, 0x38); 00025 double angle; 00026 00027 Ticker calcul; 00028 float testmove; 00029 int flag = 0; 00030 int flug = 0; 00031 int u = 0; 00032 float my_ini,mx_ini,mz_ini; 00033 float correction = 0; 00034 float correct_yaw; 00035 float affi_correct_yaw; 00036 00037 00038 FusionBias fusionBias; 00039 FusionAhrs fusionAhrs; 00040 00041 void mesure (void){ 00042 flag = 1; 00043 } 00044 00045 float samplePeriod = 0.01f; 00046 00047 FusionVector3 gyroscopeSensitivity = { 00048 .axis.x = 1.0f, 00049 .axis.y = 1.0f, 00050 .axis.z = 1.0f, 00051 }; 00052 00053 FusionVector3 accelerometerSensitivity = { 00054 .axis.x = 1.0f, 00055 .axis.y = 1.0f, 00056 .axis.z = 1.0f, 00057 }; 00058 00059 FusionVector3 hardIronBias = { 00060 .axis.x = 0.0f, 00061 .axis.y = 0.0f, 00062 .axis.z = 0.0f, 00063 }; 00064 00065 /////////////////////////////////////////////////////////////////////////// 00066 void serial_receive() 00067 { 00068 unsigned char c=pc.getc(); // attention à protéger les appels pc 00069 buf.push(c); 00070 00071 } 00072 00073 void setup () 00074 { 00075 00076 lol.begin(); 00077 lol.calibrate(); 00078 00079 } 00080 void yaw_correct_val_ini ()/////////////////////////////////////////////création des variables initiales du magnéto 00081 { 00082 00083 00084 lol.readMag(); 00085 my_ini=lol.calcMag(lol.my)*10; 00086 mx_ini=lol.calcMag(lol.mx)*10; 00087 mz_ini=lol.calcMag(lol.mz)*10; 00088 flug = 1; 00089 } 00090 00091 00092 //////////////////////////////////////////////////////////////////////// 00093 int main() 00094 { 00095 00096 setup(); 00097 00098 char l1,l2,l3,l4; 00099 00100 00101 00102 00103 pc.attach(&serial_receive); // isr sur reception char 00104 00105 00106 00107 int i = 0; 00108 float f=10; 00109 int j=0; 00110 char str[20]; 00111 00112 00113 // Initialise gyroscope bias correction algorithm 00114 FusionBiasInitialise(&fusionBias, 20.0f, samplePeriod); 00115 00116 // Initialise AHRS algorithm 00117 FusionAhrsInitialise(&fusionAhrs, 0.5f); 00118 00119 // Set optional magnetic field limits 00120 FusionAhrsSetMagneticField(&fusionAhrs, 20.0f, 70.0f); // valid magnetic field range = 20 uT to 70 uT 00121 // The contents of this do while loop should be called for each time new sensor measurements are available 00122 pc.baud(115200); //vit de comunication 00123 00124 calcul.attach(&mesure,0.01); 00125 //////////////////////// //////////// ///////////////////Partie Transmission 00126 while(1) { 00127 00128 00129 00130 00131 00132 // envoi de données 00133 00134 int x=temps.read_ms(); 00135 if (flug == 0){ 00136 yaw_correct_val_ini (); 00137 } 00138 00139 if(flag == 1) { 00140 00141 lol.readGyro(); 00142 lol.readAccel(); 00143 lol.readMag(); 00144 00145 00146 00147 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00148 // Calibrate gyroscope 00149 FusionVector3 uncalibratedGyroscope = { 00150 .axis.x = -lol.calcGyro(lol.gx), 00151 .axis.y = -lol.calcGyro(lol.gy), 00152 .axis.z = lol.calcGyro(lol.gz), 00153 }; 00154 00155 FusionVector3 calibratedGyroscope = FusionCalibrationInertial(uncalibratedGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gyroscopeSensitivity, FUSION_VECTOR3_ZERO); 00156 00157 // Calibrate accelerometer 00158 FusionVector3 uncalibratedAccelerometer = { 00159 .axis.x = lol.calcAccel(lol.ax), 00160 .axis.y = lol.calcAccel(lol.ay), 00161 .axis.z = lol.calcAccel(lol.az), 00162 }; 00163 FusionVector3 calibratedAccelerometer = FusionCalibrationInertial(uncalibratedAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, accelerometerSensitivity, FUSION_VECTOR3_ZERO); 00164 00165 // Calibrate magnetometer 00166 FusionVector3 uncalibratedMagnetometer = { 00167 .axis.x = lol.calcMag(lol.mx), 00168 .axis.y = lol.calcMag(lol.my), 00169 .axis.z = lol.calcMag(lol.mz), 00170 }; 00171 FusionVector3 calibratedMagnetometer = FusionCalibrationMagnetic(uncalibratedMagnetometer, FUSION_ROTATION_MATRIX_IDENTITY, hardIronBias); 00172 00173 00174 // Update gyroscope bias correction algorithm 00175 calibratedGyroscope = FusionBiasUpdate(&fusionBias, calibratedGyroscope); 00176 00177 // Update AHRS algorithm 00178 FusionAhrsUpdate(&fusionAhrs, calibratedGyroscope, calibratedAccelerometer, calibratedMagnetometer, samplePeriod); 00179 00180 // Print Euler angles 00181 FusionEulerAngles eulerAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&fusionAhrs)); 00182 u = u+1; 00183 testmove = testmove + eulerAngles.angle.yaw; 00184 00185 00186 ///////////////////////////////////////////////////////////////////////////////////////////////////////repérage de pas de mouvement et application de la correction 00187 00188 if (u == 10 ) 00189 { 00190 if ((testmove/10) > eulerAngles.angle.yaw -0.02 && (testmove/10) < eulerAngles.angle.yaw +0.02) 00191 { 00192 if ((lol.calcMag(lol.my)*10) > my_ini-0.1 && ((lol.calcMag(lol.my)*10 < my_ini+0.1))) 00193 { 00194 if (((lol.calcMag(lol.mx)*10) > mx_ini-0.25 && ((lol.calcMag(lol.mx)*10) < mx_ini+0.25))) 00195 { 00196 if (((lol.calcMag(lol.mz)*10) > mz_ini-0.25 && ((lol.calcMag(lol.mz)*10) < mz_ini+0.25))) 00197 { 00198 ///////////////////////////////////////////////////////////////////////////////////////////////////////// correction progressive 00199 if(correct_yaw < 10) 00200 { 00201 correction = correction + 5; 00202 correct_yaw = eulerAngles.angle.yaw + (correction); 00203 } 00204 if(correct_yaw < 2) 00205 { 00206 correction = correction + 1; 00207 correct_yaw = eulerAngles.angle.yaw + (correction); 00208 00209 } 00210 if(correct_yaw > -10) 00211 { 00212 correction = correction - 5; 00213 correct_yaw = eulerAngles.angle.yaw + (correction); 00214 00215 } 00216 if(correct_yaw > -2) 00217 { 00218 correction = correction - 1; 00219 correct_yaw = eulerAngles.angle.yaw + (correction); 00220 00221 00222 } 00223 } 00224 00225 } 00226 } 00227 } 00228 00229 /////////////////////////////////////////////////////////////////////////////////////////////////////// 00230 affi_correct_yaw = eulerAngles.angle.yaw +(correction); 00231 00232 00233 00234 // pc.printf("%6.1f %6.1f %6.1f\n",eulerAngles.angle.roll, eulerAngles.angle.pitch, affi_correct_yaw); //affichage LabVIEW 00235 pc.printf("Roulis : %6.1f Tangage : %6.1f Lacet : %6.1f\n\r",eulerAngles.angle.roll, eulerAngles.angle.pitch, affi_correct_yaw); //affichage TeraTerm 00236 u = 0; 00237 testmove =0; 00238 } 00239 i++; 00240 flag = 0; 00241 } 00242 } 00243 00244 //////////////////////// //////////// /////////////////////////Partie Réception 00245 //reception de données 00246 while(!buf.empty()) { 00247 unsigned char c; 00248 buf.pop(c); 00249 switch(c) { 00250 case '\r' : // fin de message on traite 00251 str[j]=NULL; // fin de chaine 00252 j=0; 00253 // debut traitement chaine recue 00254 sscanf(str,"%d %d %d %d %f",&l1,&l2,&l3,&l4,&f); 00255 00256 led2=l2; 00257 // fin traitement chaine recue 00258 break; 00259 case '\n': // on ignore 00260 break; 00261 default : // on stocke 00262 str[j]=c; 00263 j++; 00264 } 00265 } 00266 } 00267
Generated on Tue Jul 12 2022 21:08:03 by
1.7.2