main.cpp

//#DOC Main, program entry and main loop
#include "main.h"
#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;
    }
}