otStudio: IMU - Code example

 #include <otMath.h>
 #include <otSerial.h>
 #include <otTimer.h> 
 #include <otMalloc.h>
 #include <otPrintf.h>
 #include <otI2cBus.h>
 #include <otMPU9250.h>
 
 // Number of milliseconds that have elapsed since the system was started, up to 49.7 days.
 U32         _ticker = 0;
 
 // IMU class
 otMPU9250   imu;
 
 // Timer 1 callback
 FAST_CODE_2 void _timer1CallBack()
 {
        _ticker++;
 }
 
 // IMU calibrarion and selftest
 void imuCalibrate()
 {
        F32   SelfTest[6];  // Holds results of gyro and accelerometer self test
        imu.reset();
        printf("IMU Self test started ...\n");
        imu.selfTest(SelfTest);
        printf("Acceleration  trim within = X:%6.1f, Y:%6.1f, Z:%6.1f %% of factory value\n", SelfTest[0], SelfTest[1], SelfTest[2]);
        printf("Gyroscope     trim within = X:%6.1f, Y:%6.1f, Z:%6.1f %% of factory value\n", SelfTest[3], SelfTest[4], SelfTest[5]);
        printf("Calibrating gyroscope and accelerometer ...\n");
        imu.accelGyroCalibration(); // Calibrate gyro and accelerometers, load biases in bias registers
        printf("Accelerometer biases (mg) = X:%6.1f, Y:%6.1f, Z:%6.1f\n", 1000. * imu.d->accelBias[0], 1000. * imu.d->accelBias[1], 1000. * imu.d->accelBias[2]);
        printf("Gyroscope     biases (dps)= X:%6.1f, Y:%6.1f, Z:%6.1f\n", imu.d->gyroBias[0], imu.d->gyroBias[1], imu.d->gyroBias[2]);
        imu.setRange(otMPU9250::AFS_2G, otMPU9250::GFS_250DPS, otMPU9250::MFS_16BITS);
        imu.imuInit();
        if(imu.magnetometerCheck())
        {
                // Get magnetometer calibration from AK8963 ROM
                imu.magnetometerInit();
                printf("Magnetometer initialized for active data mode ...\n"); // Initialize device for active mode read of magnetometer
                imu.magnetometerCalibration();
                printf("Magnetometer biases (mG)  = X:%6.1f, Y:%6.1f, Z:%6.1f\n", imu.d->magBias[0], imu.d->magBias[1], imu.d->magBias[2]); 
                printf("Magnetometer scale  (mG)  = X:%6.1f, Y:%6.1f, Z:%6.1f\n", imu.d->magScale[0], imu.d->magScale[1], imu.d->magScale[2]);
        }
 }
 
 void setup()
 {
        UartEnableSignals(UART0);
        UartSetBaud(UART0, 115200);
        
        // Set system tick for every 1ms
        TimerSetInterruptService(TIMER1, _timer1CallBack, TimerRateMs(1));
        TimerEnable(TIMER1, true);
        
        _initMalloc();
        
        setStdout(4096, UART0); // printf output is now directed on UART0
        
        I2cBusReset();
        I2cBusFrequency(100, 40);
        
        DelayMs(1000);          // Wait terminal start
        
        imu.init();
        if(imu.check())
                imuCalibrate();
        else
        {
                printf("Can't find MPU9250\n");
                IDLE;
        }
 }
 
 void loop()
 {
        bool    dump = false;
        U32     st, et, loop = 0;
        F32     deltat;                             // Integration interval for both filter schemes
        F32     ax, ay, az, gx, gy, gz, mx, my, mz; // Variables to hold latest sensor data values
        F32     yaw, pitch, roll;                   // Tait?Bryan angles are also called Cardan angles
        
        st = _ticker;    // Grab start time
        while(1)
        {
                if(imu.ready())
                {
                        imu.accelerometerGyroscope(ax, ay, az, gx, gy, gz);
                        imu.magnetometer(mx, my, mz);
                        et = _ticker;   // Grab end time
                        deltat = ((float) (et - st)) / 1000.f;
                        st = _ticker;   // Grab start time
                        if(dump) printf("[%6u] Integration time  (s) = t:%6.3f\n", _ticker, deltat);
                        // Display results
                        // Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer;
                        // the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro!
                        // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter.
                        // For the MPU9250+MS5637 Mini breakout the +x accel/gyro is North, then -y accel/gyro is East. So if we want te quaternions properly aligned
                        // we need to feed into the Madgwick function Ax, -Ay, -Az, Gx, -Gy, -Gz, My, -Mx, and Mz. But because gravity is by convention
                        // positive down, we need to invert the accel data, so we pass -Ax, Ay, Az, Gx, -Gy, -Gz, My, -Mx, and Mz into the Madgwick
                        // function to get North along the accel +x-axis, East along the accel -y-axis, and Down along the accel -z-axis.
                        // This orientation choice can be modified to allow any convenient (non-NED) orientation convention.
                        // Pass gyro rate as rad/s
                        imu.madgwickQuaternionUpdate(-ax, ay, az, gx * M_PI / 180.0f, -gy * M_PI / 180.0f, -gz * M_PI / 180.0f,  my, -mx, mz, deltat);
                        if(dump) printf("[%6u] Accelerometer (mg)    = X:%7.2f, Y:%7.2f, Z:%7.2f\n", _ticker, 1000.f * ax, 1000.f * ay, 1000.f * az);
                        if(dump) printf("[%6u] Gyroscope     (deg/s) = X:%7.2f, Y:%7.2f, Z:%7.2f\n", _ticker, gx, gy, gz);
                        if(dump) printf("[%6u] Magnetometer  (mG)    = X:%7.2f, Y:%7.2f, Z:%7.2f\n", _ticker, mx, my, mz);
                        
                        if(dump) printf("[%6u] Quaternion  = 0: %5.3f, X: %5.3f,  Y: %5.3f, Z: %5.3f\n", _ticker, imu.d->q[0],  imu.d->q[1],  imu.d->q[2],  imu.d->q[3]);
                        if(dump) printf("[%6u] Temperature = C: %6.1f\n", _ticker, (float) imu.temperature());
                        imu.taitBryanAngles(yaw, pitch, roll);
                        if(dump) 
                        {
                                printf("[%6u] Euler angle = Y: %5d, P: %5d, R: %5d\n", _ticker, (int) yaw, (int) pitch, (int) roll);
                                dump = false;
                        }
                        loop++;
                }
                // Reduce the number of views, otherwise you don't understand anything!
                if((loop % 100) == 0) dump = true;
        }
 }
IMU - Code example [MPU9250 - Inertial Measurement Unit]

IMU - Code example
[MPU9250 - Inertial Measurement Unit]
