Arduino IMU

#include <Joystick.h>

////////////////////////////////////////////////////////////////////////////
//
//  This file is part of RTIMULib-Arduino
//
//  Copyright (c) 2014-2015, richards-tech
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy of
//  this software and associated documentation files (the "Software"), to deal in
//  the Software without restriction, including without limitation the rights to use,
//  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
//  Software, and to permit persons to whom the Software is furnished to do so,
//  subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>

RTIMU *imu;                                           // the IMU object
RTFusionRTQF fusion;                                  // the fusion object
RTIMUSettings settings;                               // the settings object
RTMath mathobj;

//  DISPLAY_INTERVAL sets the rate at which results are displayed

#define DISPLAY_INTERVAL  20                         // interval between pose displays

//  SERIAL_PORT_SPEED defines the speed to use for the debug serial port

#define  SERIAL_PORT_SPEED  115200

#define JOYSTICK 1


unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;

//JOYSTICK STUFF


Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID,
  JOYSTICK_TYPE_MULTI_AXIS, 32, 0,
  true, true, false, false, false, false,
  true, true, false, false, false);

// Set to true to test "Auto Send" mode or false to test "Manual Send" mode.
//const bool testAutoSendMode = true;
const bool testAutoSendMode = false;

const unsigned long gcCycleDelta = 1000;
const unsigned long gcAnalogDelta = 25;
const unsigned long gcButtonDelta = 500;
unsigned long gNextTime = 0;
unsigned int gCurrentStep = 0;
int xAxis;
int yAxis;

float pitch1;
float yaw1;

void setup()
{
    int errcode;

    if (!JOYSTICK) {Serial.begin(SERIAL_PORT_SPEED);}
    Wire.begin();
    imu = RTIMU::createIMU(&settings);                        // create the imu object

    //Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
    if ((errcode = imu->IMUInit()) < 0) {
        //Serial.print("Failed to init IMU: "); Serial.println(errcode);
    }

    //if (imu->getCalibrationValid())
        //Serial.println("Using compass calibration");
    //else
        //Serial.println("No valid compass calibration data");

    lastDisplay = lastRate = millis();
    sampleCount = 0;

    // Slerp power controls the fusion and can be between 0 and 1
    // 0 means that only gyros are used, 1 means that only accels/compass are used
    // In-between gives the fusion mix.  0.02 default

    fusion.setSlerpPower(0.005);

    // use of sensors in the fusion algorithm can be controlled here
    // change any of these to false to disable that sensor

    fusion.setGyroEnable(true);
    fusion.setAccelEnable(true);
    fusion.setCompassEnable(true);

    //JOYSTICK STUFF
  if (JOYSTICK) {
        Joystick.setXAxisRange(-32766, 32766);
        Joystick.setYAxisRange(-32766, 32766);
        Joystick.setZAxisRange(-127, 127);
        Joystick.setThrottleRange(0, 255);
        Joystick.setRudderRange(0, 255);

        if (testAutoSendMode)
        {
          Joystick.begin();
        }
        else
        {
          Joystick.begin(false);
        }

  }
}

void loop()
{
    unsigned long now = millis();
    unsigned long delta;
    int loopCount = 1;

    while (imu->IMURead()) {                                // get the latest data if ready yet
        // this flushes remaining data in case we are falling behind
        if (++loopCount >= 10)
            continue;
        fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
        sampleCount++;
        /*if ((delta = now - lastRate) >= 1000) {
            Serial.print("Sample rate: "); Serial.print(sampleCount);
            if (imu->IMUGyroBiasValid())
                Serial.println(", gyro bias valid");
            else
                Serial.println(", calculating gyro bias");

            sampleCount = 0;
            lastRate = now;
        }*/
        if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
            lastDisplay = now;
//          RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
//          RTMath::display("Accel:", (RTVector3&)imu->getAccel());              // accel data
//          RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
            if (!JOYSTICK) {
            RTMath::displayRollPitchYaw("Pose:", (RTVector3&)fusion.getFusionPose()); // fused output
            Serial.println();
            }
        }
    }

    //JOYSTICK STUFF
    //add 90 to y axis, 180 to z axis
    //Serial.print(","); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE);
    //Serial.print(","); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE);

    if (JOYSTICK) {
          pitch1 = ((RTVector3&)fusion.getFusionPose()).y();
          pitch1 *= RTMATH_RAD_TO_DEGREE;
          pitch1 += 90;
          pitch1 = pitch1 / 180;
          pitch1 *= 65532;
          pitch1 -= 32766;
          yAxis = pitch1;

          yaw1 = ((RTVector3&)fusion.getFusionPose()).z();
          yaw1 *= RTMATH_RAD_TO_DEGREE;
          yaw1 += 90;
          yaw1 = yaw1 / 180;
          yaw1 *= 65532;
          yaw1 -= 32766;
          xAxis = yaw1;

          Joystick.setXAxis(xAxis);
          Joystick.setYAxis(yAxis);

          if (testAutoSendMode == false) {
            Joystick.sendState();
          }
    }

}