E&R S3 prime / Mbed 2 deprecated Fusion3

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.

Dependencies:   mbed LSM6DS33_GR1

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Fusion3.cpp Source File

Fusion3.cpp

00001 //************************************
00002 // test fusion
00003 // ***********************************
00004 //
00005 #include "Fusion.h"
00006 #include "LSM6DS33_GR1.h"
00007 //
00008 float samplePeriod = 0.01f; // replace this value with actual sample period in seconds/
00009 // tracteur
00010 FusionBias TBias;
00011 FusionAhrs TAhrs;
00012 FusionVector3 gTSensitivity = {
00013     .axis.x = 1.0f,
00014     .axis.y = 1.0f,
00015     .axis.z = 1.0f,
00016 }; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
00017 FusionVector3 aTSensitivity = {
00018     .axis.x = 1.0f,
00019     .axis.y = 1.0f,
00020     .axis.z = 1.0f,
00021 }; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
00022 //
00023 // outil
00024 FusionBias OBias;
00025 FusionAhrs OAhrs;
00026 FusionVector3 gOSensitivity = {
00027     .axis.x = 1.0f,
00028     .axis.y = 1.0f,
00029     .axis.z = 1.0f,
00030 }; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
00031 FusionVector3 aOSensitivity = {
00032     .axis.x = 1.0f,
00033     .axis.y = 1.0f,
00034     .axis.z = 1.0f,
00035 }; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
00036 
00037 
00038 
00039 int flag_fusion=0;
00040 Serial pc(SERIAL_TX, SERIAL_RX);  // I/O terminal PC
00041 DigitalOut led(LED1);  // led
00042 LSM6DS33 AGT(PB_9, PB_8);  // accelero gyro tracteur
00043 
00044 
00045 
00046 float gg[3];
00047 float aRes,gRes;
00048 Ticker affich;
00049 //
00050 
00051 //
00052 signed long round(double value)
00053 {
00054     return((long)floor(value+0.5));
00055 }
00056 
00057 //
00058 void aff(void)
00059 {
00060     flag_fusion=1;
00061     // tracteur
00062     // Calibrate gyroscope
00063 
00064 }
00065 //
00066 int main()
00067 {
00068     wait(1);
00069     pc.baud(115200);
00070     pc.printf("Hello Fusion \r\n");
00071     //aRes = 2.0 / 32768.0;
00072     //gRes = 250.0 / 32768.0;
00073     //pi=4*atan(1.0);
00074     //init IMU
00075     AGT.begin(LSM6DS33::G_SCALE_500DPS,LSM6DS33::A_SCALE_8G,LSM6DS33::G_ODR_104,LSM6DS33::A_ODR_104);
00076     //wait(0.5);
00077     AGT.calibration(1000);
00078     //
00079     char rr= AGT.readReg(0x15);
00080     pc.printf(" reg 0x15 : %x \r\n",rr);
00081     rr= AGT.readReg(0x10);
00082     pc.printf(" reg 0x10 : %x \r\n",rr);
00083     AGT.writeReg(0x10,rr|0x03);  // 50HZ
00084     rr= AGT.readReg(0x10);
00085     pc.printf(" reg 0x10 : %x \r\n",rr);
00086     rr= AGT.readReg(0x15);
00087     pc.printf(" reg 0x15 : %x \r\n",rr);
00088     rr= AGT.readReg(0x11);
00089     pc.printf(" reg 0x11 : %x \r\n",rr);
00090     
00091 AGT.writeReg(0x13,0x80);  // BW SCAL
00092 rr= AGT.readReg(0x13);
00093     pc.printf(" reg 0x13 : %x \r\n",rr);
00094     //
00095     // Initialise gyroscope bias correction algorithm
00096     FusionBiasInitialise(&TBias, 20.0f, samplePeriod); // stationary threshold = 0.5 degrees per second
00097     FusionBiasInitialise(&OBias, 20.0f, samplePeriod); // stationary threshold = 0.5 degrees per second
00098 
00099     // Initialise AHRS algorithm
00100     FusionAhrsInitialise(&TAhrs, 0.5f); // gain = 0.5
00101     FusionAhrsInitialise(&OAhrs, 0.5f); // gain = 0.5
00102 
00103     // The contents of this do while loop should be called for each time new sensor measurements are available
00104     //
00105     int cpt=0;
00106     affich.attach(&aff,0.01);
00107     while(1) {
00108         // lecture accelero / gyro
00109 
00110         if(flag_fusion==1) {
00111             cpt++;
00112             AGT.readAccel();
00113             AGT.readGyro();
00114             AGT.readTemp();
00115             FusionVector3 uncalTGyroscope = {
00116                 .axis.x = AGT.gx-AGT.gx_off, /* replace this value with actual gyroscope x axis measurement in lsb */
00117                 .axis.y = AGT.gy-AGT.gy_off, /* replace this value with actual gyroscope y axis measurement in lsb */
00118                 .axis.z = AGT.gz-AGT.gz_off, /* replace this value with actual gyroscope z axis measurement in lsb */
00119                 //.axis.z = 0, /* replace this value with actual gyroscope z axis measurement in lsb */
00120             };
00121             FusionVector3 calTGyroscope = FusionCalibrationInertial(uncalTGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gTSensitivity, FUSION_VECTOR3_ZERO);
00122             // Calibrate accelerometer
00123             FusionVector3 uncalTAccelerometer = {
00124                 .axis.x = AGT.ax, /* replace this value with actual accelerometer x axis measurement in lsb */
00125                 .axis.y = AGT.ay, /* replace this value with actual accelerometer y axis measurement in lsb */
00126                 .axis.z = AGT.az, /* replace this value with actual accelerometer z axis measurement in lsb */
00127             };
00128             FusionVector3 calTAccelerometer = FusionCalibrationInertial(uncalTAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, aTSensitivity, FUSION_VECTOR3_ZERO);
00129             // Update gyroscope bias correction algorithm
00130             calTGyroscope = FusionBiasUpdate(&TBias, calTGyroscope);
00131             led= FusionBiasIsActive(&TBias);
00132             // Update AHRS algorithm
00133             FusionAhrsUpdateWithoutMagnetometer(&TAhrs, calTGyroscope, calTAccelerometer, samplePeriod);
00134 //
00135             // Outil
00136             // Calibrate gyroscope
00137             FusionVector3 uncalOGyroscope = {
00138                 .axis.x = -(AGT.gz-AGT.gz_off), /* replace this value with actual gyroscope x axis measurement in lsb */
00139                 .axis.y = -(AGT.gy-AGT.gy_off), /* replace this value with actual gyroscope y axis measurement in lsb */
00140                 .axis.z = -(AGT.gx-AGT.gx_off), /* replace this value with actual gyroscope z axis measurement in lsb */
00141             };
00142             FusionVector3 calOGyroscope = FusionCalibrationInertial(uncalOGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gOSensitivity, FUSION_VECTOR3_ZERO);
00143             // Calibrate accelerometer
00144             FusionVector3 uncalOAccelerometer = {
00145                 .axis.x = -AGT.az, /* replace this value with actual accelerometer x axis measurement in lsb */
00146                 .axis.y = -AGT.ay, /* replace this value with actual accelerometer y axis measurement in lsb */
00147                 .axis.z = -AGT.ax, /* replace this value with actual accelerometer z axis measurement in lsb */
00148             };
00149             FusionVector3 calOAccelerometer = FusionCalibrationInertial(uncalOAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, aOSensitivity, FUSION_VECTOR3_ZERO);
00150             // Update gyroscope bias correction algorithm
00151             calOGyroscope = FusionBiasUpdate(&OBias, calOGyroscope);
00152             // Update AHRS algorithm
00153             FusionAhrsUpdateWithoutMagnetometer(&OAhrs, calOGyroscope, calOAccelerometer, samplePeriod);
00154 FusionEulerAngles eulerTAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&TAhrs));
00155 
00156         FusionEulerAngles eulerOAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&OAhrs));
00157             flag_fusion=0;
00158             if(cpt%1==0){
00159                 pc.printf("$%f %f;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch);
00160                  //pc.printf("$%f %f %f %f;", uncalTGyroscope.axis.x,calTGyroscope.axis.x,uncalTGyroscope.axis.y,calTGyroscope.axis.y);
00161                 
00162                 }
00163         }
00164         // Print Euler angles
00165         
00166         //printf("Roll = %0.1f, Pitch = %0.1f, Yaw = %0.1f\r\n", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
00167         // pc.printf("$%f %f %f;", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
00168         // pc.printf("$%f %f %f %f;", eulerAngles.angle.roll, eulerAngles.angle.pitch,angle_trac,cc);
00169         //pc.printf("$%f %f %f %f ;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch,eulerOAngles.angle.roll, eulerOAngles.angle.pitch);
00170         //////////// pc.printf("$%f %f %f;", eulerOAngles.angle.roll, eulerOAngles.angle.pitch,AGT.temperature_c);
00171         ////////////pc.printf("$%f %f;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch);
00172         //pc.printf("$%f %f %f;", gyroT_out[0],accT_out[0],angle_trac);
00173         //pc.printf("$%f %f %f;", AGT.gx-AGT.gx_off,AGT.gy-AGT.gy_off,AGT.gz-AGT.gz_off);
00174         //pc.printf("$%f %f %f;", gg[0],gg[1],gg[2]);
00175         //
00176         //wait(0.1);
00177         // pc.printf(">>> %f %f %f  \r\n",GXOT,GYOT,GZOT);
00178         ////pc.printf(">>> %f %f %f  \r\n",AGT.gx_off,AGT.gy_off,AGT.gz_off);
00179         // pc.printf("> %f %f %f %f %f %f \r\n",AXT,AYT,AZT,GXT,GYT,GZT);
00180         ////pc.printf("< %f %f %f %f %f %f \r\n\r\n",AGT.ax,AGT.ay,AGT.az,AGT.gx-AGT.gx_off,AGT.gy,AGT.gz);
00181         // pc.printf("> %f %f %f %f %f %f \r\n",AXM,AYM,AZM,GXM,GYM,GZM);
00182         // pc.printf(">>> %f %f %f  \r\n",GXOM,GYOM,GZOM);
00183         // allichage des differents ecrans
00184 
00185     }
00186 }
Generated on Sat Sep 3 2022 06:36:54 by  doxygen 1.7.2