Dodan quaternion, serijski link

.gitignore

@@ -0,0 +1,2 @@
+\#*\#
+.*.~undo-tree~

README.org

@@ -21,7 +21,7 @@
 
   Ideja izhaja iz zgodnješega projekta abstraktne vizualizacije, ki naj bi odsevala glasbo. Po izgradnji zgodnjega prototipa z vmesnikom na drsnikov na dotik, ki modulirajo prikaz, sem se vprašal zakaj bi program interpretiral glasbo? To namreč počnemo že mi, na poslušanje (lahko) odreagiramo s plesom, gibalni senzor pa bi lahko telesne gibe "ojačal" v oblike in barve, projecirane na površino.
 
-  Z gibalno rokvičko se opravljalka_ec vizualij lahko izogne ujetosti v neroden zaslon na dotitk in potopi raje v izkušnjo zvoka in videa med soustvarjanjem le-te. Organizirana bo tudi delavnica za izgradnjo večih rokavičk, ki se lahko med sabo povežejo v ad-hoc mrežo, kar skupini ljudi omogoči sodelovanje. 
+  Z gibalno rokavičko se opravljalka_ec vizualij lahko izogne ujetosti v neroden zaslon na dotitk in potopi raje v izkušnjo zvoka in videa med soustvarjanjem le-te. Organizirana bo tudi delavnica za izgradnjo večih rokavičk, ki se lahko med sabo povežejo v ad-hoc mrežo, kar skupini ljudi omogoči sodelovanje. 
 
   Rokavica oz. rokavice bodo pošiljale podatke preko OSC (open sound control) protokola, kar odpira tudi možnost rabe za zvočno sintezo/modulacijo ali celo kot splošni vmesnik.
 

kbd/kbd/kbd.ino

@@ -1,23 +0,0 @@
-// Keyboard Matrix Tutorial Example
-// baldengineer.com
-// CC BY-SA 4.0
-  
-// JP1 is an input
-byte keys[] = {16, 17, 5, 18};
-byte pressed[] = {0, 0, 0, 0};
-byte KEYLEN = 4;
- 
-void setup() {
-    Serial.begin(9600);
-    for(int i = 0; i < KEYLEN; i++) {
-      pinMode(keys[i], INPUT_PULLUP);
-    }
-}
-void loop() {
-    for(int i = 0; i < KEYLEN; i++) {
-      pressed[i] = !digitalRead(keys[i]);
-      Serial.print(String(pressed[i]) + " ");
-    }
-    Serial.println();
-    delay(50);
-}

osc32_9255/ArduinoAccel.ino

@@ -0,0 +1,127 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMULib/RTIMUSettings.h"
+#include "RTIMULib/RTIMU.h"
+#include "RTIMULib/RTFusionRTQF.h" 
+#include "CalLib/CalLib.h"
+#include <EEPROM.h>
+
+RTIMU *imu;                                           // the IMU object
+RTFusionRTQF fusion;                                  // the fusion object
+RTIMUSettings settings;                               // the settings object
+
+//  DISPLAY_INTERVAL sets the rate at which results are displayed
+
+#define DISPLAY_INTERVAL  100                         // interval between pose displays
+
+//  SERIAL_PORT_SPEED defines the speed to use for the debug serial port
+
+#define  SERIAL_PORT_SPEED  115200
+
+unsigned long lastDisplay;
+unsigned long lastRate;
+int sampleCount;
+RTQuaternion gravity;
+
+void setup()
+{
+  int errcode;
+  
+  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;
+  
+  gravity.setScalar(0);
+  gravity.setX(0);
+  gravity.setY(0);
+  gravity.setZ(1);
+}
+
+void loop()
+{  
+  unsigned long now = millis();
+  unsigned long delta;
+  RTVector3 realAccel;
+  RTQuaternion rotatedGravity;
+  RTQuaternion fusedConjugate;
+  RTQuaternion qTemp;
+  int loopCount = 0;
+  
+  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());
+    
+    //  do gravity rotation and subtraction
+    
+    // create the conjugate of the pose
+    
+    fusedConjugate = fusion.getFusionQPose().conjugate();
+    
+    // now do the rotation - takes two steps with qTemp as the intermediate variable
+    
+    qTemp = gravity * fusion.getFusionQPose();
+    rotatedGravity = fusedConjugate * qTemp;
+    
+    // now adjust the measured accel and change the signs to make sense
+    
+    realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
+    realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
+    realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
+    
+    sampleCount++;
+    if ((delta = now - lastRate) >= 1000) {
+      Serial.print("Sample rate: "); Serial.print(sampleCount);
+      if (!imu->IMUGyroBiasValid())
+        Serial.println(", calculating gyro bias");
+      else
+        Serial.println();
+        
+      sampleCount = 0;
+      lastRate = now;
+    }
+    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
+      lastDisplay = now;
+      
+      RTMath::display("Accel:", realAccel);
+      Serial.println();
+    }
+  }
+}

osc32_9255/CalLib/CalLib.cpp

@@ -0,0 +1,95 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "CalLib.h"
+#ifdef __SAM3X8E__
+
+// Due version
+
+#include "DueFlash.h"
+
+DueFlash flash;
+
+void calLibErase(byte device)
+{
+    uint32_t data = 0;
+
+    flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, &data, 1); // just destroy the valid byte
+}
+
+void calLibWrite(byte device, CALLIB_DATA *calData)
+{
+    calData->validL = CALLIB_DATA_VALID_LOW;
+    calData->validH = CALLIB_DATA_VALID_HIGH;
+
+    flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, (uint32_t *)calData, sizeof(CALLIB_DATA) / 4);
+}
+
+boolean calLibRead(byte device, CALLIB_DATA *calData)
+{
+    memcpy(calData, CALLIB_START + sizeof(CALLIB_DATA) * device, sizeof(CALLIB_DATA));
+    return calData->valid == CALLIB_DATA_VALID;
+}
+
+#else
+
+// AVR version
+
+#include <EEPROM.h>
+
+void calLibErase(byte device)
+{
+    EEPROM.write(sizeof(CALLIB_DATA) * device, 0); // just destroy the valid byte
+}
+
+void calLibWrite(byte device, CALLIB_DATA *calData)
+{
+  byte *ptr = (byte *)calData;
+  byte length = sizeof(CALLIB_DATA);
+  int eeprom = sizeof(CALLIB_DATA) * device;
+
+  calData->validL = CALLIB_DATA_VALID_LOW;
+  calData->validH = CALLIB_DATA_VALID_HIGH;
+  
+  for (byte i = 0; i < length; i++)
+    EEPROM.write(eeprom + i, *ptr++);
+}
+
+boolean calLibRead(byte device, CALLIB_DATA *calData)
+{
+  byte *ptr = (byte *)calData;
+  byte length = sizeof(CALLIB_DATA);
+  int eeprom = sizeof(CALLIB_DATA) * device;
+
+  calData->magValid = false;
+
+  if ((EEPROM.read(eeprom) != CALLIB_DATA_VALID_LOW) ||
+      (EEPROM.read(eeprom + 1) != CALLIB_DATA_VALID_HIGH)) {
+    return false;                                  // invalid data
+  }
+    
+  for (byte i = 0; i < length; i++)
+    *ptr++ = EEPROM.read(eeprom + i);
+  return true;  
+}
+#endif

osc32_9255/CalLib/CalLib.h

@@ -0,0 +1,58 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _CALLIB_H_
+#define _CALLIB_H_
+
+#include <Arduino.h>
+
+#define CALLIB_DATA_VALID_LOW     0xfc // pattern to detect valid config - low byte
+#define CALLIB_DATA_VALID_HIGH    0x15 // pattern to detect valid config - high byte
+
+#ifdef __SAM3X8E__
+#define CALLIB_START  ((uint32_t *)(IFLASH1_ADDR + IFLASH1_SIZE - IFLASH1_PAGE_SIZE))
+#endif
+
+typedef struct
+{
+  unsigned char validL;                 // should contain the valid pattern if a good config
+  unsigned char validH;                 // should contain the valid pattern if a good config
+  unsigned char magValid;               // true if data valid
+  unsigned char pad;
+  float magMin[3];                      // min values
+  float magMax[3];                      // max values
+} CALLIB_DATA;
+
+//  calLibErase() erases any current data in the EEPROM
+
+void calLibErase(byte device);
+
+//  calLibWrite() writes new data to the EEPROM
+
+void calLibWrite(byte device, CALLIB_DATA * calData);
+
+//  calLibRead() reads existing data and returns true if valid else false in not.
+
+boolean calLibRead(byte device, CALLIB_DATA * calData);
+
+#endif // _CALLIB_H_

osc32_9255/I2CDev/I2Cdev.h

@@ -0,0 +1,269 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
+//
+// Changelog:
+//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+//      2011-10-03 - added automatic Arduino version detection for ease of use
+//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+//      2011-08-02 - added support for 16-bit registers
+//                 - fixed incorrect Doxygen comments on some methods
+//                 - added timeout value for read operations (thanks mem @ Arduino forums)
+//      2011-07-30 - changed read/write function structures to return success or byte counts
+//                 - made all methods static for multi-device memory savings
+//      2011-07-28 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+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.
+===============================================
+*/
+
+#ifndef _I2CDEV_H_
+#define _I2CDEV_H_
+
+// -----------------------------------------------------------------------------
+// I2C interface implementation setting
+// -----------------------------------------------------------------------------
+#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
+//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_FASTWIRE
+
+// comment this out if you are using a non-optimal IDE/implementation setting
+// but want the compiler to shut up about it
+#define I2CDEV_IMPLEMENTATION_WARNINGS
+
+// -----------------------------------------------------------------------------
+// I2C interface implementation options
+// -----------------------------------------------------------------------------
+#define I2CDEV_ARDUINO_WIRE         1 // Wire object from Arduino
+#define I2CDEV_BUILTIN_NBWIRE       2 // Tweaked Wire object from Gene Knight's NBWire project
+                                      // ^^^ NBWire implementation is still buggy w/some interrupts!
+#define I2CDEV_BUILTIN_FASTWIRE     3 // FastWire object from Francesco Ferrara's project
+#define I2CDEV_I2CMASTER_LIBRARY    4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
+
+// -----------------------------------------------------------------------------
+// Arduino-style "Serial.print" debug constant (uncomment to enable)
+// -----------------------------------------------------------------------------
+//#define I2CDEV_SERIAL_DEBUG
+
+#ifdef ARDUINO
+    #if ARDUINO < 100
+        #include "WProgram.h"
+    #else
+        #include "Arduino.h"
+    #endif
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+        #include <Wire.h>
+    #endif
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
+        #include <I2C.h>
+    #endif
+#endif
+
+// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
+#define I2CDEV_DEFAULT_READ_TIMEOUT     1000
+
+class I2Cdev {
+    public:
+        I2Cdev();
+        
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+};
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+    //////////////////////
+    // FastWire 0.24
+    // This is a library to help faster programs to read I2C devices.
+    // Copyright(C) 2012
+    // Francesco Ferrara
+    //////////////////////
+    
+    /* Master */
+    #define TW_START                0x08
+    #define TW_REP_START            0x10
+
+    /* Master Transmitter */
+    #define TW_MT_SLA_ACK           0x18
+    #define TW_MT_SLA_NACK          0x20
+    #define TW_MT_DATA_ACK          0x28
+    #define TW_MT_DATA_NACK         0x30
+    #define TW_MT_ARB_LOST          0x38
+
+    /* Master Receiver */
+    #define TW_MR_ARB_LOST          0x38
+    #define TW_MR_SLA_ACK           0x40
+    #define TW_MR_SLA_NACK          0x48
+    #define TW_MR_DATA_ACK          0x50
+    #define TW_MR_DATA_NACK         0x58
+
+    #define TW_OK                   0
+    #define TW_ERROR                1
+
+    class Fastwire {
+        private:
+            static boolean waitInt();
+
+        public:
+            static void setup(int khz, boolean pullup);
+            static byte beginTransmission(byte device);
+            static byte write(byte value);
+            static byte writeBuf(byte device, byte address, byte *data, byte num);
+            static byte readBuf(byte device, byte address, byte *data, byte num);
+            static void reset();
+            static byte stop();
+    };
+#endif
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
+    // NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
+    // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
+    // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
+
+    #define NBWIRE_BUFFER_LENGTH 32
+    
+    class TwoWire {
+        private:
+            static uint8_t rxBuffer[];
+            static uint8_t rxBufferIndex;
+            static uint8_t rxBufferLength;
+        
+            static uint8_t txAddress;
+            static uint8_t txBuffer[];
+            static uint8_t txBufferIndex;
+            static uint8_t txBufferLength;
+        
+            // static uint8_t transmitting;
+            static void (*user_onRequest)(void);
+            static void (*user_onReceive)(int);
+            static void onRequestService(void);
+            static void onReceiveService(uint8_t*, int);
+    
+        public:
+            TwoWire();
+            void begin();
+            void begin(uint8_t);
+            void begin(int);
+            void beginTransmission(uint8_t);
+            //void beginTransmission(int);
+            uint8_t endTransmission(uint16_t timeout=0);
+            void nbendTransmission(void (*function)(int)) ;
+            uint8_t requestFrom(uint8_t, int, uint16_t timeout=0);
+            //uint8_t requestFrom(int, int);
+            void nbrequestFrom(uint8_t, int, void (*function)(int));
+            void send(uint8_t);
+            void send(uint8_t*, uint8_t);
+            //void send(int);
+            void send(char*);
+            uint8_t available(void);
+            uint8_t receive(void);
+            void onReceive(void (*)(int));
+            void onRequest(void (*)(void));
+    };
+    
+    #define TWI_READY   0
+    #define TWI_MRX     1
+    #define TWI_MTX     2
+    #define TWI_SRX     3
+    #define TWI_STX     4
+    
+    #define TW_WRITE    0
+    #define TW_READ     1
+    
+    #define TW_MT_SLA_NACK      0x20
+    #define TW_MT_DATA_NACK     0x30
+    
+    #define CPU_FREQ            16000000L
+    #define TWI_FREQ            100000L
+    #define TWI_BUFFER_LENGTH   32
+    
+    /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
+    
+    #define TW_STATUS_MASK              (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
+    #define TW_STATUS                   (TWSR & TW_STATUS_MASK)
+    #define TW_START                    0x08
+    #define TW_REP_START                0x10
+    #define TW_MT_SLA_ACK               0x18
+    #define TW_MT_SLA_NACK              0x20
+    #define TW_MT_DATA_ACK              0x28
+    #define TW_MT_DATA_NACK             0x30
+    #define TW_MT_ARB_LOST              0x38
+    #define TW_MR_ARB_LOST              0x38
+    #define TW_MR_SLA_ACK               0x40
+    #define TW_MR_SLA_NACK              0x48
+    #define TW_MR_DATA_ACK              0x50
+    #define TW_MR_DATA_NACK             0x58
+    #define TW_ST_SLA_ACK               0xA8
+    #define TW_ST_ARB_LOST_SLA_ACK      0xB0
+    #define TW_ST_DATA_ACK              0xB8
+    #define TW_ST_DATA_NACK             0xC0
+    #define TW_ST_LAST_DATA             0xC8
+    #define TW_SR_SLA_ACK               0x60
+    #define TW_SR_ARB_LOST_SLA_ACK      0x68
+    #define TW_SR_GCALL_ACK             0x70
+    #define TW_SR_ARB_LOST_GCALL_ACK    0x78
+    #define TW_SR_DATA_ACK              0x80
+    #define TW_SR_DATA_NACK             0x88
+    #define TW_SR_GCALL_DATA_ACK        0x90
+    #define TW_SR_GCALL_DATA_NACK       0x98
+    #define TW_SR_STOP                  0xA0
+    #define TW_NO_INFO                  0xF8
+    #define TW_BUS_ERROR                0x00
+    
+    //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr))
+    //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
+    
+    #ifndef sbi // set bit
+        #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+    #endif // sbi
+    
+    #ifndef cbi // clear bit
+        #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+    #endif // cbi
+    
+    extern TwoWire Wire;
+
+#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
+
+#endif /* _I2CDEV_H_ */

osc32_9255/RTArduLink/RTArduLink.cpp

@@ -0,0 +1,231 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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 "RTArduLink.h"
+#include "RTArduLinkHAL.h"
+#include "RTArduLinkUtils.h"
+
+#include <string.h>
+
+RTArduLink::RTArduLink()
+{
+}
+
+RTArduLink::~RTArduLink()
+{
+}
+
+void RTArduLink::begin(const char *identitySuffix)
+{
+    RTARDULINK_PORT *portInfo;
+
+    if (!RTArduLinkHALEEPROMValid())
+        RTArduLinkHALEEPROMDefault();
+
+    m_identitySuffix = identitySuffix;
+
+    // now set up host and subsystem ports based on EEPROM configuration
+
+    for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++) {
+        portInfo = m_ports + i;
+        portInfo->index = i;
+        portInfo->inUse = RTArduLinkHALConfigurePort(&(portInfo->portHAL), i);
+        RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer));
+    }
+    m_hostPort = m_ports;
+}
+
+
+void RTArduLink::background()
+{
+    unsigned char index;
+    RTARDULINK_PORT *portInfo;
+
+    for (index = 0; index < RTARDULINKHAL_MAX_PORTS; index++) {
+        portInfo = m_ports + index;
+        if (!portInfo->inUse)
+            continue;
+
+        while (RTArduLinkHALPortAvailable(&(portInfo->portHAL))) {
+            if (!RTArduLinkReassemble(&(portInfo->RXFrame), RTArduLinkHALPortRead(&(portInfo->portHAL)))) {
+                sendDebugMessage("Reassembly error");
+            } else {
+                if (portInfo->RXFrame.complete) {
+                    processReceivedMessage(portInfo);
+                    RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer));
+                }
+            }
+        }
+    }
+}
+
+void RTArduLink::processReceivedMessage(RTARDULINK_PORT *portInfo)
+{
+    RTARDULINK_MESSAGE *message;                            // a pointer to the message part of the frame
+    unsigned int address;
+
+    message = &(portInfo->RXFrameBuffer.message);           // get the message pointer
+    address = RTArduLinkConvertUC2ToUInt(message->messageAddress);
+
+    switch (portInfo->index) {
+        case  RTARDULINK_HOST_PORT:
+            processHostMessage();                           // came from this upstream link
+            return;
+
+        case RTARDULINK_DAISY_PORT:                         // came from dasiy chain port
+            if (address != RTARDULINK_HOST_PORT)            // true if it came from a daisy chained subsystem, not a directly connected subsystem
+                RTArduLinkConvertIntToUC2(address + RTARDULINKHAL_MAX_PORTS, message->messageAddress);
+             else
+                RTArduLinkConvertIntToUC2(RTARDULINK_DAISY_PORT, message->messageAddress);
+            break;
+
+        default:
+            RTArduLinkConvertIntToUC2(address + portInfo->index, message->messageAddress);
+            break;
+    }
+
+    // if get here, need to forward to host port
+
+    sendFrame(m_hostPort, &(portInfo->RXFrameBuffer), portInfo->RXFrameBuffer.messageLength);
+}
+
+
+void RTArduLink::processHostMessage()
+{
+    RTARDULINK_MESSAGE *message;                            // a pointer to the message part of the frame
+    int identityLength;
+    int suffixLength;
+    unsigned int address;
+
+    message = &(m_hostPort->RXFrameBuffer.message);         // get the message pointer
+    address = RTArduLinkConvertUC2ToUInt(message->messageAddress);
+
+    if (address == RTARDULINK_BROADCAST_ADDRESS) {          // need to forward to downstream ports also
+        for (int i = RTARDULINK_HOST_PORT + 1; i < RTARDULINKHAL_MAX_PORTS; i++) {
+            if (m_ports[i].inUse)
+                sendFrame(m_ports + i, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
+        }
+    }
+    if ((address == RTARDULINK_MY_ADDRESS) || (address == RTARDULINK_BROADCAST_ADDRESS)) {  // it's for me
+        switch (message->messageType)
+        {
+            case RTARDULINK_MESSAGE_POLL:
+            case RTARDULINK_MESSAGE_ECHO:
+                RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
+                sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);   // just send the frame back as received
+                break;
+
+            case RTARDULINK_MESSAGE_IDENTITY:
+                identityLength = strlen(RTArduLinkHALConfig.identity);
+                suffixLength = strlen(m_identitySuffix);
+
+                memcpy(message->data, RTArduLinkHALConfig.identity, identityLength + 1);    // copy in identity
+
+                if ((identityLength + suffixLength) < RTARDULINK_DATA_MAX_LEN - 1) {
+                    memcpy(message->data + identityLength, m_identitySuffix, suffixLength + 1); // copy in suffix
+                } else {
+                    suffixLength = 0;
+                }
+                RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
+                message->data[RTARDULINK_DATA_MAX_LEN - 1] = 0;     // make sure zero terminated if it was truncated
+                sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + identityLength + suffixLength + 1);
+                break;
+
+            default:
+                if (message->messageType < RTARDULINK_MESSAGE_CUSTOM) {	// illegal code
+                    message->data[0] = RTARDULINK_RESPONSE_ILLEGAL_COMMAND;
+                    message->data[1] = message->messageType;        // this is the offending code
+                    message->messageType = RTARDULINK_MESSAGE_ERROR;
+                    RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
+                    sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + 2);
+                    break;
+                }
+                processCustomMessage(message->messageType, message->messageParam, message->data,
+                        m_hostPort->RXFrameBuffer.messageLength - RTARDULINK_MESSAGE_HEADER_LEN);	// see if anyone wants to process it
+                break;
+        }
+        return;
+    }
+
+    if (address >= RTARDULINKHAL_MAX_PORTS) {               // need to pass it to the first subsystem
+        if (!m_ports[RTARDULINK_DAISY_PORT].inUse)
+            return;                                         // there is no daisy chain port
+        RTArduLinkConvertIntToUC2(address - RTARDULINKHAL_MAX_PORTS, message->messageAddress); // adjust the address
+        sendFrame(m_ports +RTARDULINK_DAISY_PORT, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
+        return;
+    }
+
+    // if get here, needs to go to a local subsystem port
+
+    if (m_ports[address].inUse) {
+        RTArduLinkConvertIntToUC2(0, message->messageAddress);     // indicates that the target should process it
+        sendFrame(m_ports + address, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
+    }
+}
+
+void RTArduLink::sendDebugMessage(const char *debugMessage)
+{
+    RTARDULINK_FRAME frame;
+    int stringLength;
+
+    stringLength = strlen(debugMessage);
+    if (stringLength >= RTARDULINK_DATA_MAX_LEN)
+        stringLength = RTARDULINK_DATA_MAX_LEN-1;
+    memcpy(frame.message.data, debugMessage, stringLength);
+    frame.message.data[stringLength] = 0;
+    frame.message.messageType = RTARDULINK_MESSAGE_DEBUG;
+    RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress);
+    sendFrame(m_hostPort, &frame, RTARDULINK_MESSAGE_HEADER_LEN + stringLength + 1);
+}
+
+void RTArduLink::sendMessage(unsigned char messageType, unsigned char messageParam, unsigned char *data, int length)
+{
+    RTARDULINK_FRAME frame;
+  
+    RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress);
+    frame.message.messageType = messageType;
+    frame.message.messageParam = messageParam;
+
+    if (length > RTARDULINK_DATA_MAX_LEN)
+        length = RTARDULINK_DATA_MAX_LEN;
+    memcpy(frame.message.data, data, length);
+
+    sendFrame(m_hostPort, &frame, length + RTARDULINK_MESSAGE_HEADER_LEN);
+}
+
+void RTArduLink::sendFrame(RTARDULINK_PORT *portInfo, RTARDULINK_FRAME *frame, int length)
+{
+    frame->sync0 = RTARDULINK_MESSAGE_SYNC0;
+    frame->sync1 = RTARDULINK_MESSAGE_SYNC1;
+    frame->messageLength = length;                          // set length
+    RTArduLinkSetChecksum(frame);                           // compute checksum
+    RTArduLinkHALPortWrite(&(portInfo->portHAL), (unsigned char *)frame, frame->messageLength + RTARDULINK_FRAME_HEADER_LEN);
+}
+

osc32_9255/RTArduLink/RTArduLink.h

@@ -0,0 +1,81 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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.
+
+#ifndef _RTARDULINK_H
+#define _RTARDULINK_H
+
+#include "RTArduLinkDefs.h"
+#include "RTArduLinkHAL.h"
+
+#define	RTARDULINK_HOST_PORT                0               // host port is always 0
+#define	RTARDULINK_DAISY_PORT               1               // daisy chain port is always 1
+
+typedef struct
+{
+    int index;                                              // port index
+    bool inUse;                                             // true if in use
+    RTARDULINK_RXFRAME RXFrame;                             // structure to maintain receive frame state
+    RTARDULINK_FRAME RXFrameBuffer;                         // used to assemble received frames
+    RTARDULINKHAL_PORT portHAL;                             // the actual hardware port interface
+} RTARDULINK_PORT;
+
+class RTArduLink
+{
+public:
+    RTArduLink();
+    virtual ~RTArduLink();
+
+    void begin(const char *identitySuffix);                 // should be called in setup() code
+    void background();                                      // should be called once per loop()
+    void sendDebugMessage(const char *debugMesssage);       // sends a debug message to the host port
+    void sendMessage(unsigned char messageType, unsigned char messageParam,
+        unsigned char *data, int length);                   // sends a message to the host port
+
+protected:
+//  These are functions that can be overridden
+
+    virtual void processCustomMessage(unsigned char messageType, 
+        unsigned char messageParam, unsigned char *data, int dataLength) {}
+
+    RTARDULINK_PORT m_ports[RTARDULINKHAL_MAX_PORTS];       // port array
+    RTARDULINK_PORT *m_hostPort;                            // a link to the entry for the host port
+
+
+private:
+    void processReceivedMessage(RTARDULINK_PORT *port);     // process a completed message
+    void processHostMessage();                              // special case for stuff received from the host port
+    void sendFrame(RTARDULINK_PORT *portInfo, RTARDULINK_FRAME *frame, int length);	// send a frame to the host. length is length of data field
+
+    const char *m_identitySuffix;                           // what to add to the EEPROM identity string
+
+};
+
+#endif // _RTARDULINK_H
+

osc32_9255/RTArduLink/RTArduLinkDefs.h

@@ -0,0 +1,199 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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.
+
+//  The RTArduLink communications protocol works by exchanging frames across the host <-> subsystem interface.
+//  The structure RTARDULINK_FRAME defines the frame structure. There is a 4 byte header for frame level control
+//  while the remainder is available for higher level message. Note that the structure implies fixed length
+//  buffers which works well with the subsystem however only the number of bytes actually used are transferred
+//  across the interface.
+//
+//  Note that there is no flow control at the frame level - it is assumed that the higher level interactions host -> subsystem
+//  are window 1 acknowledged transfers so that the maximum possible unprocessed frames is equal to the number
+//  of higher level services. Subsystem -> host transfers are always either responses to host commands or else regular
+//  status updates so the rate from subsystem to host is controlled by configuration.
+//
+//  The frame integrity is protected by a single byte checksum. To keep things very simple, it is the 2s complement
+//  of the 8 bit sum of all the bytes in the message array. This is used in conjunction with 0xAA and 0x55 bytes
+//  to determine correct sync (in case of lost bytes which should not really happen!).
+//
+//  Frame sync is obtained by reading bytes until the 0xAA pattern is seen. If the next byte is not 0x55, keep
+//  scanning. If it is, assume this byte is messageLength and the next one is the frameCksm value. Read in the
+//  message array based on messageLength and then calculate the checksum. If the checksum is correct, sync has been
+//  obtained and the message is valid. Otherwise, start looking for an 0xAA value again.
+
+#ifndef _RTARDULINKDEFS_H
+#define _RTARDULINKDEFS_H
+
+//  Some defines to cope with compiler differences
+
+#ifndef __cplusplus
+#ifndef false
+#define false 0
+#endif
+
+#ifndef true
+#define true 1
+#endif
+
+typedef unsigned char bool;
+#endif
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+
+//	Some general purpose typedefs - used especially for transferring values greater than
+//	8 bits across the link and avoids endian issues. Assumes processor has 32 bit ints!
+
+typedef	unsigned char RTARDULINK_UC2[2];                    // an array of two unsigned chars
+typedef	unsigned char RTARDULINK_UC4[4];                    // an array of four unsigned chars
+
+//	Port speed codes
+
+#define	RTARDULINK_PORT_SPEED_OFF       0                   // port is unused
+#define	RTARDULINK_PORT_SPEED_9600      1                   // 9600 baud
+#define	RTARDULINK_PORT_SPEED_19200     2                   // 19200 baud
+#define	RTARDULINK_PORT_SPEED_38400     3                   // 38400 baud
+#define	RTARDULINK_PORT_SPEED_57600     4                   // 57600 baud
+#define	RTARDULINK_PORT_SPEED_115200    5                   // 115200 baud
+
+#define	RTARDULINK_PORT_SPEED_COUNT     6                   // six codes total
+
+extern unsigned long RTArduLinkSpeedMap[];
+
+//------------------------------------------------------------------------------------------------------
+//
+//	Frame level defs and structure
+
+#define	RTARDULINK_FRAME_MAX_LEN        64                  // maximum possible length of a frame
+#define	RTARDULINK_FRAME_HEADER_LEN     4                   // 4 bytes in frame header (must correspond with the structure below!)
+#define	RTARDULINK_MESSAGE_HEADER_LEN   4                   // 4 bytes in message header (must correspond with the structure below!)
+#define	RTARDULINK_MESSAGE_MAX_LEN      (RTARDULINK_FRAME_MAX_LEN - RTARDULINK_FRAME_HEADER_LEN)    // max length of message
+#define	RTARDULINK_DATA_MAX_LEN         (RTARDULINK_MESSAGE_MAX_LEN - RTARDULINK_MESSAGE_HEADER_LEN)// max length of data field
+
+#define	RTARDULINK_MESSAGE_SYNC0        0xAA
+#define	RTARDULINK_MESSAGE_SYNC1        0x55
+
+#define	RTARDULINK_MY_ADDRESS           0                   // the subsystem address for local processing
+#define	RTARDULINK_BROADCAST_ADDRESS    0xffff              // the subsystem address for all subsystems
+#define	RTARDULINK_ADDRESSES            0x1000              // number of addresses
+
+//  RTARDULINK_MESSAGE is carried in the RTARDULINK_FRAME
+//
+//  The messageAddress field allows subsystems to be daisy-chained. Valid addresses are 0 to 65534.
+//  Address 65535 is a broadcast and goes to all subsystems.
+//  Every message has the messageType and messageParam bytes but there can be from 0 to 56 bytes of data
+
+typedef struct
+{
+    RTARDULINK_UC2 messageAddress;                          // subsystem message address
+    unsigned char messageType;                              // message type code
+    unsigned char messageParam;                             // an optional parameter to the message type
+    unsigned char data[RTARDULINK_DATA_MAX_LEN];            // the actual data! Length is computed from messageLength.
+} RTARDULINK_MESSAGE;
+
+//  RTARDULINK_FRAME is the lowest level structure used across the RTArduLink
+
+typedef struct
+{
+    unsigned char sync0;                                    // sync0 code
+    unsigned char sync1;                                    // sync1 code
+    unsigned char messageLength;                            // the length of the message in the message field - between 4 and 60 bytes
+    unsigned char frameChecksum;                            // checksum for frame
+    RTARDULINK_MESSAGE message;                             // the actual message
+} RTARDULINK_FRAME;
+
+//  RTARDULINK_RXFRAME is a type that is used to reassemble a frame from a stream of bytes in conjunction with RTArduLinkReassemble()
+
+typedef struct
+{
+    RTARDULINK_FRAME *frameBuffer;                          // the frame buffer pointer
+    int length;                                             // current length of frame
+    int bytesLeft;                                          // number of bytes needed to complete
+    bool complete;                                          // true if frame is complete and correct (as far as checksum goes)
+} RTARDULINK_RXFRAME;
+
+//  Message types
+
+//  RTARDULINK_MESSAGE_POLL
+//
+//  The host should poll the RTArduLink at every RTARDULINK_POLL_INTERVAL.
+//  The subsystem will respond by echoing the poll message as received.
+
+#define	RTARDULINK_MESSAGE_POLL         0                   // poll message
+
+//  RTARDULINK_MESSAGE_IDENTIFY
+//
+//  The host can send this message to request an identity string from the subsystem.
+//  Only the messageType field is used in the request host -> subsystem. The subsystem
+//  responds with an identity string in the data field.
+
+#define	RTARDULINK_MESSAGE_IDENTITY     1                   // identity message
+
+//  RTARDULINK_MESSAGE_DEBUG
+//
+//  This can be used to send a debug message up to the host. The data field contains a debug message
+
+#define	RTARDULINK_MESSAGE_DEBUG        2                   // debug message
+
+//  RTARDULINK_MESSAGE_INFO
+//
+//  This can be used to send an info message up to the host. The data field contains the message
+
+#define	RTARDULINK_MESSAGE_INFO         3                   // info message
+
+//  RTARDULINK_MESSAGE_ERROR
+//
+//  This code is returned by the subsystem if it received a message with an illegal message type
+//  The first byte of the data is the error code. The rest of the data field depends on the error.
+
+#define	RTARDULINK_MESSAGE_ERROR        4                   // illegal message type response
+
+//  RTARDULINK_MESSAGE_ECHO
+//
+//  This message can be used to test link performance. The addressed subsystem just returns
+//  the entire message to the host.
+
+#define	RTARDULINK_MESSAGE_ECHO         5                   // echo message
+
+//  RTARDULINK_MESSAGE_CUSTOM
+//
+//  This is the first message code that should be used for custom messages 16-255 are available.
+
+#define	RTARDULINK_MESSAGE_CUSTOM       16                  // start of custom messages
+
+//  RTArduLink response codes
+
+#define	RTARDULINK_RESPONSE_OK                      0       // means things worked
+#define	RTARDULINK_RESPONSE_ILLEGAL_COMMAND         1       // not a supported message type, data[1] has offending type
+
+
+#endif  // _RTARDULINKDEFS_H

osc32_9255/RTArduLink/RTArduLinkDemoDefs.h

@@ -0,0 +1,66 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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.
+
+#ifndef RTARDULINKDEMODEFS_H_
+#define RTARDULINKDEMODEFS_H_
+
+#include "RTArduLinkUtils.h"
+
+#define SERVO_COUNT    2                                   // 2 servo channels
+#define PWM_COUNT      3                                   // 3 pwm channels
+#define INPUT_COUNT    2                                   // 2 inputs
+#define OUTPUT_COUNT   2                                   // 2 outputs
+
+#define SERVO_MIN_VALUE   1000                             // min servo value
+#define SERVO_CTR_VALUE   1500                             // center servo value
+#define SERVO_MAX_VALUE   2000                             // max servo value
+
+#define PWM_MIN_VALUE     0                                // min pwm value
+#define PWM_CTR_VALUE     128                              // center pwm value
+#define PWM_MAX_VALUE     255                              // max pwm value
+
+//    The command structure is sent from the host to the subsystem
+
+typedef struct
+{
+    RTARDULINK_UC2 servoPos[SERVO_COUNT];                  // the servo positions
+    unsigned char pwmValue[PWM_COUNT];                     // PWM values
+    unsigned char outputValue[OUTPUT_COUNT];               // the output pin values (true=high, false=low)
+} RTARDULINKDEMO_COMMAND;
+
+//    the response structure is sent from the subsystem to the host
+
+typedef struct
+{
+    unsigned char inputValue[INPUT_COUNT];                // the input pin values (true=high, false=low)
+} RTARDULINKDEMO_RESPONSE;
+
+
+#endif /* RTARDULINKDEMODEFS_H_ */

osc32_9255/RTArduLink/RTArduLinkHAL.cpp

@@ -0,0 +1,194 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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 <string.h>
+#include "RTArduLinkHAL.h"
+
+//----------------------------------------------------------
+//
+//  Arduino HAL
+
+
+//  The global config structure
+
+RTARDULINKHAL_EEPROM RTArduLinkHALConfig;
+
+bool RTArduLinkHALAddHardwarePort(RTARDULINKHAL_PORT *port, long portSpeed, unsigned char hardwarePort);
+
+//  Port speed map array
+
+unsigned long RTArduLinkHALSpeedMap[] = {0, 9600, 19200, 38400, 57600, 115200};
+
+
+bool RTArduLinkHALConfigurePort(RTARDULINKHAL_PORT *port, int portIndex)
+{
+    if (RTArduLinkHALConfig.portSpeed[portIndex] == RTARDULINK_PORT_SPEED_OFF)
+        return false;                                       // port is not enabled
+
+    return RTArduLinkHALAddHardwarePort(port, RTArduLinkHALSpeedMap[RTArduLinkHALConfig.portSpeed[portIndex]],
+                    RTArduLinkHALConfig.hardwarePort[portIndex]);
+}
+
+int RTArduLinkHALPortAvailable(RTARDULINKHAL_PORT *port)
+{
+    return port->serialPort->available();
+}
+
+unsigned char RTArduLinkHALPortRead(RTARDULINKHAL_PORT *port)
+{
+    return port->serialPort->read();
+}
+
+void RTArduLinkHALPortWrite(RTARDULINKHAL_PORT *port, unsigned char *data, unsigned char length)
+{
+    port->serialPort->write(data, length);
+}
+
+
+bool RTArduLinkHALAddHardwarePort(RTARDULINKHAL_PORT *port, long portSpeed, unsigned char hardwarePort)
+{
+    HardwareSerial *hardPort;
+
+    switch (hardwarePort) {
+        case 0:
+#if defined(USBCON)
+            /* Leonardo support */
+            hardPort = &Serial1;
+#else
+            hardPort = &Serial;
+#endif
+            break;
+
+        case 1:
+#if defined(UBRR1H)
+            hardPort = &Serial1;
+#else
+            return false;
+#endif
+            break;
+
+        case 2:
+#if defined(UBRR2H)
+            hardPort = &Serial2;
+#else
+            return false;
+#endif
+            break;
+
+        case 3:
+#if defined(UBRR3H)
+            hardPort = &Serial3;
+#else
+            return false;
+#endif
+            break;
+
+        default:
+            return false;
+    }
+
+    port->serialPort = hardPort;
+    hardPort->begin(portSpeed);                             // start the port
+    return true;
+}
+
+
+bool RTArduLinkHALEEPROMValid()
+{
+    RTArduLinkHALEEPROMRead();                              // see what it really is
+    return (RTArduLinkHALConfig.sig0 == RTARDULINKHAL_SIG0) && 
+        (RTArduLinkHALConfig.sig1 == RTARDULINKHAL_SIG1);
+}
+
+void RTArduLinkHALEEPROMDisplay()
+{
+    Serial.println();
+
+    if ((RTArduLinkHALConfig.sig0 != RTARDULINKHAL_SIG0) || 
+        (RTArduLinkHALConfig.sig1 != RTARDULINKHAL_SIG1)) {
+        Serial.println("Invalid config");
+        return;
+    }
+    Serial.print("Identity: ");
+    Serial.println(RTArduLinkHALConfig.identity);
+
+    for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++)
+        RTArduLinkHALEEPROMDisplayPort(i, true);
+}
+
+void RTArduLinkHALEEPROMDisplayPort(int index, bool suppress)
+{
+    if (suppress && (RTArduLinkHALConfig.portSpeed[index] == RTARDULINK_PORT_SPEED_OFF))
+        return;
+    Serial.print("Port index ");
+    Serial.print(index);
+    Serial.print(" speed=");
+    Serial.print(RTArduLinkHALConfig.portSpeed[index]);
+    Serial.print(", ");
+    Serial.print("hardware port number=");
+    Serial.println(RTArduLinkHALConfig.hardwarePort[index]);
+}
+
+
+void RTArduLinkHALEEPROMDefault()
+{
+    RTArduLinkHALConfig.sig0 = RTARDULINKHAL_SIG0;       // set to valid signature
+    RTArduLinkHALConfig.sig1 = RTARDULINKHAL_SIG1;                        
+    strcpy(RTArduLinkHALConfig.identity, "RTArduLink_Arduino");
+
+    RTArduLinkHALConfig.portSpeed[0] = RTARDULINK_PORT_SPEED_115200;
+    for (int i = 1; i < RTARDULINKHAL_MAX_PORTS; i++)
+        RTArduLinkHALConfig.portSpeed[i] = RTARDULINK_PORT_SPEED_OFF;
+
+    for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++)
+        RTArduLinkHALConfig.hardwarePort[i] = i;
+
+    RTArduLinkHALEEPROMWrite();
+}
+
+void RTArduLinkHALEEPROMRead()
+{
+    unsigned char *data;
+
+    data = (unsigned char *)&RTArduLinkHALConfig;
+
+    for (int i = 0; i < (int)sizeof(RTARDULINKHAL_EEPROM); i++)
+        *data++ = EEPROM.read(i + RTARDULINKHAL_EEPROM_OFFSET);
+}
+
+void RTArduLinkHALEEPROMWrite()
+{
+    unsigned char *data;
+
+    data = (unsigned char *)&RTArduLinkHALConfig;
+
+    for (int i = 0; i < (int)sizeof(RTARDULINKHAL_EEPROM); i++)
+        EEPROM.write(i + RTARDULINKHAL_EEPROM_OFFSET, *data++);
+}

osc32_9255/RTArduLink/RTArduLinkHAL.h

@@ -0,0 +1,141 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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.
+
+#ifndef _RTARDULINKHAL_H
+#define _RTARDULINKHAL_H
+
+
+//----------------------------------------------------------
+//  Target-specific includes
+//
+//
+//  Arduino HAL
+
+#include <RTArduLinkDefs.h>
+#include <HardwareSerial.h>
+#include <EEPROM.h>
+
+#define	RTARDULINKHAL_MAX_SUBSYSTEM_PORTS	3               // maximum number of subsystem ports
+#define RTARDULINKHAL_MAX_PORTS             (RTARDULINKHAL_MAX_SUBSYSTEM_PORTS + 1)	// max total ports (including host)
+#define RTARDULINKHAL_EEPROM_OFFSET         256             // where the config starts in EEPROM
+
+//  RTARDULINKHAL_PORT should be modified as appropriate for the target.
+//  There is one copy of this per port. It contains all state needed about
+//  a serial port.
+
+typedef struct
+{
+    HardwareSerial *serialPort;                             // the serial port structure
+} RTARDULINKHAL_PORT;
+
+//  RTARDULINKHAL_EEPROM is the target-specific structure used to
+//  store configs in EEPROM
+
+//  Signature bytes indicating valid config
+
+#define RTARDULINKHAL_SIG0                  0x38          
+#define RTARDULINKHAL_SIG1                  0xc1           
+
+typedef struct
+{
+    unsigned char sig0;                                     // signature byte 0
+    unsigned char sig1;                                     // signature byte 1
+    char identity[RTARDULINK_DATA_MAX_LEN];                 // identity string
+    unsigned char portSpeed[RTARDULINKHAL_MAX_PORTS];       // port speed codes
+    unsigned char hardwarePort[RTARDULINKHAL_MAX_PORTS];    // port number for hardware serial
+} RTARDULINKHAL_EEPROM;
+
+//  The global config structure
+
+extern RTARDULINKHAL_EEPROM RTArduLinkHALConfig;
+
+
+
+//----------------------------------------------------------
+//
+// These functions must be provided the RTArduLinkHAL for all implementations
+
+//  RTArduLinkHALConfigurePort() activates the specified port configuration specified by portIndex in port structure port.
+
+    bool RTArduLinkHALConfigurePort(RTARDULINKHAL_PORT *port, int portIndex);
+
+
+//  RTArduLinkHALPortAvailable() returns the number of bytes availabel on the specified port.
+
+    int RTArduLinkHALPortAvailable(RTARDULINKHAL_PORT *port);
+
+
+//  RTArduLinkHALPortRead() returns the next available byte from a port. Always check available bytes first
+
+    unsigned char RTArduLinkHALPortRead(RTARDULINKHAL_PORT *port);
+
+
+//  RTArduLinkHALPortWrite() writes length bytes of the block pointed to by data to the specified port.
+
+    void RTArduLinkHALPortWrite(RTARDULINKHAL_PORT *port, unsigned char *data, unsigned char length);
+
+
+//  RTArduLinkHALEEPROMValid() returns true if the EEPROM contains a valid configuration,
+//  false otherwise.
+
+    bool RTArduLinkHALEEPROMValid();                        // returns true if a valid config
+
+
+//  RTArduLinkHALEEPROMDisplay() displays the current configuration
+
+    void RTArduLinkHALEEPROMDisplay();                      // display the config
+
+
+//  RTArduLinkHALEEPROMDisplayPort() displays the configuration for a single port
+//  If suppress is true, nothing is displayed if the port is not enabled. If false
+//  the port's data will be displayed regardless.
+
+    void RTArduLinkHALEEPROMDisplayPort(int port, bool suppress);   // display the port config
+
+
+//  RTArduLinkHALEEPROMDefault() writes a default config to EEPROM
+
+    void RTArduLinkHALEEPROMDefault();                      // write and load default settings
+
+
+//  RTArduLinkHALEEPROMRead() loads the EEPROM config into the RTArduLinkHALConfig
+//  global structure.
+
+    void RTArduLinkHALEEPROMRead();                         // to load the config
+
+
+//  RTArduLinkHALEEPROMWrite() writes the config in the RTArduLinkHALConfig
+//  global structure back to EEPROM.
+
+    void RTArduLinkHALEEPROMWrite();                        // to write the config
+
+
+#endif // _RTARDULINKHAL_H
+

osc32_9255/RTArduLink/RTArduLinkUtils.cpp

@@ -0,0 +1,181 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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 "RTArduLinkUtils.h"
+
+//	RTArduLinkRXFrameInit initializes the structure for a new frame using frameBuffer for storage
+
+void RTArduLinkRXFrameInit(RTARDULINK_RXFRAME *RXFrame, RTARDULINK_FRAME *frameBuffer)
+{
+    RXFrame->complete = false;
+    RXFrame->length = 0;
+    RXFrame->bytesLeft = 0;
+    RXFrame->frameBuffer = frameBuffer;
+}
+
+//  RTArduLinkReassemble takes a sequence of received bytes and tries to complete a frame. It returns true if ok
+//  false if error. The caller can determine if the frame is complete by checking the complete flag.
+
+bool RTArduLinkReassemble(RTARDULINK_RXFRAME *RXFrame, unsigned char data)
+{
+    bool flag = true;
+
+    ((unsigned char *)(RXFrame->frameBuffer))[RXFrame->length] = data;  // save byte in correct place
+    switch (RXFrame->length) {
+        case 0:                                             // waiting for sync0
+            if (RXFrame->frameBuffer->sync0 == RTARDULINK_MESSAGE_SYNC0) {
+                RXFrame->length = 1;
+            }
+            break;
+
+        case 1:                                             // waiting for sync1
+            if (RXFrame->frameBuffer->sync1 == RTARDULINK_MESSAGE_SYNC1) {
+                RXFrame->length = 2;
+            } else {
+                RXFrame->length = 0;                        // try again if not correct two byte sequence
+            }
+            break;
+
+        case 2:                                             // should be message length
+            if (RXFrame->frameBuffer->messageLength <= RTARDULINK_MESSAGE_MAX_LEN) {
+                RXFrame->length = 3;
+                RXFrame->bytesLeft = RXFrame->frameBuffer->messageLength + 1;   // +1 to allow for the checksum
+            } else {
+                RXFrame->length = 0;                        // discard this and resync frame
+                flag = false;
+            }
+            break;
+
+        default:
+            RXFrame->length++;
+            RXFrame->bytesLeft--;
+            if (RXFrame->bytesLeft == 0) {                  // a complete frame!
+                if (!RTArduLinkCheckChecksum(RXFrame->frameBuffer)) {
+                    RTArduLinkRXFrameInit(RXFrame, RXFrame->frameBuffer);
+                    flag = false;                           // flag the error
+                } else {
+                    //  this is a valid frame (so far)
+                    RXFrame->complete = true;
+                }
+            }
+            break;
+    }
+    return flag;
+}
+
+//  RTArduLinkSetChecksum correctly sets the checksum field on an RCP frame prior to transmission
+//
+
+void RTArduLinkSetChecksum(RTARDULINK_FRAME *frame)
+{
+    int cksm;
+    int i;
+    unsigned char *data;
+
+    for (i = 0, cksm = 0, data = (unsigned char *)&(frame->message); i < frame->messageLength; i++)
+        cksm += *data++;                                    // add up checksum
+    frame->frameChecksum = (255 - cksm) + 1;                // 2s complement
+}
+
+
+//  RTArduLinkCheckChecksum checks a received frame's checksum.
+//
+//  It adds up all the bytes from the nFrameCksm byte to the end of the frame. The result should be 0.
+
+bool RTArduLinkCheckChecksum(RTARDULINK_FRAME *frame)
+{
+    int length;
+    int i;
+    unsigned char *data;
+    unsigned char cksm;
+
+    length = frame->messageLength + 1;
+    cksm = 0;
+    data = (unsigned char *)&(frame->frameChecksum);
+
+    for (i = 0; i < length; i++)
+        cksm += *data++;
+
+    return cksm == 0;
+}
+
+//  UC2 and UC4 Conversion routines
+//
+
+long RTArduLinkConvertUC4ToLong(RTARDULINK_UC4 UC4)
+{
+    long val;
+
+    val = UC4[3];
+    val += (long)UC4[2] << 8;
+    val += (long)UC4[1] << 16;
+    val += (long)UC4[0] << 24;
+    return val;
+}
+
+void RTArduLinkConvertLongToUC4(long val, RTARDULINK_UC4 UC4)
+{
+    UC4[3] = val & 0xff;
+    UC4[2] = (val >> 8) & 0xff;
+    UC4[1] = (val >> 16) & 0xff;
+    UC4[0] = (val >> 24) & 0xff;
+}
+
+int RTArduLinkConvertUC2ToInt(RTARDULINK_UC2 UC2)
+{
+    int val;
+
+    val = UC2[1];
+    val += (int)UC2[0] << 8;
+    return val;
+}
+
+unsigned int RTArduLinkConvertUC2ToUInt(RTARDULINK_UC2 UC2)
+{
+    unsigned int val;
+
+    val = UC2[1];
+    val += (unsigned int)UC2[0] << 8;
+    return val;
+}
+
+
+void RTArduLinkConvertIntToUC2(int val, RTARDULINK_UC2 UC2)
+{
+    UC2[1] = val & 0xff;
+    UC2[0] = (val >> 8) & 0xff;
+}
+
+
+void RTArduLinkCopyUC2(RTARDULINK_UC2 destUC2, RTARDULINK_UC2 sourceUC2)
+{
+    destUC2[0] = sourceUC2[0];
+    destUC2[1] = sourceUC2[1];
+}

osc32_9255/RTArduLink/RTArduLinkUtils.h

@@ -0,0 +1,55 @@
+///////////////////////////////////////////////////////////
+//
+//  This file is part of RTArduLink
+//
+//  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.
+
+#ifndef _RTARDULINKUTILS_H
+#define _RTARDULINKUTILS_H
+
+#include "RTArduLinkDefs.h"
+
+//  Function defs
+
+void RTArduLinkRXFrameInit(RTARDULINK_RXFRAME *RXFrame, RTARDULINK_FRAME *frameBuffer);	// initializes RTARDULINK_RXFRAME for a new frame
+bool RTArduLinkReassemble(RTARDULINK_RXFRAME *RXFrame, unsigned char data);	// adds a byte to the reassembly, returns false if error
+
+//  Checksum utilities
+
+void RTArduLinkSetChecksum(RTARDULINK_FRAME *frame);        // sets the checksum field prior to transmission
+bool RTArduLinkCheckChecksum(RTARDULINK_FRAME *frame);      // checks the checksum field after reception - returns true if ok, false if error
+
+//  Type conversion utilities
+
+long RTArduLinkConvertUC4ToLong(RTARDULINK_UC4 uc4);        // converts a 4 byte array to a signed long
+void RTArduLinkConvertLongToUC4(long val, RTARDULINK_UC4 uc4);  // converts a long to a four byte array
+int	 RTArduLinkConvertUC2ToInt(RTARDULINK_UC2 uc2);         // converts a 2 byte array to a signed integer
+unsigned int RTArduLinkConvertUC2ToUInt(RTARDULINK_UC2 uc2);// converts a 2 byte array to an unsigned integer
+void RTArduLinkConvertIntToUC2(int val, RTARDULINK_UC2 uc2);// converts an integer to a two byte array
+void RTArduLinkCopyUC2(RTARDULINK_UC2 destUC2, RTARDULINK_UC2 sourceUC2); // copies a UC2
+
+#endif // _RTARDULINKUTILS_H

osc32_9255/RTIMULib/RTFusionRTQF.cpp

@@ -0,0 +1,235 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef RTARDULINK_MODE
+
+#include "RTFusionRTQF.h"
+
+#ifdef USE_SLERP
+//  The slerp power valule controls the influence of the measured state to correct the predicted state
+//  0 = measured state ignored (just gyros), 1 = measured state overrides predicted state.
+//  In between 0 and 1 mixes the two conditions
+
+#define RTQF_SLERP_POWER (RTFLOAT)0.02;
+
+#else
+//  The QVALUE affects the gyro response.
+
+#define RTQF_QVALUE	(RTFLOAT)0.001
+
+//  The RVALUE controls the influence of the accels and compass.
+//  The bigger the value, the more sluggish the response.
+
+#define RTQF_RVALUE	(RTFLOAT)0.0005
+#endif
+
+RTFusionRTQF::RTFusionRTQF()
+{
+#ifdef USE_SLERP
+    m_slerpPower = RTQF_SLERP_POWER;
+#else
+    m_Q = RTQF_QVALUE;
+    m_R = RTQF_RVALUE;
+#endif
+    m_enableGyro = true;
+    m_enableAccel = true;
+    m_enableCompass = true;
+    reset();
+}
+
+RTFusionRTQF::~RTFusionRTQF()
+{
+}
+
+void RTFusionRTQF::reset()
+{
+    m_firstTime = true;
+    m_fusionPose = RTVector3();
+    m_fusionQPose.fromEuler(m_fusionPose);
+    m_measuredPose = RTVector3();
+    m_measuredQPose.fromEuler(m_measuredPose);
+}
+
+void RTFusionRTQF::newIMUData(const RTVector3& gyro, const RTVector3& accel, const RTVector3& compass, unsigned long timestamp)
+{
+    RTVector3 fusionGyro;
+
+    if (m_firstTime) {
+        m_lastFusionTime = timestamp;
+        calculatePose(accel, compass);
+
+        //  initialize the poses
+
+        m_fusionQPose.fromEuler(m_measuredPose);
+        m_fusionPose = m_measuredPose;
+        m_firstTime = false;
+    } else {
+        m_timeDelta = (RTFLOAT)(timestamp - m_lastFusionTime) / (RTFLOAT)1000;
+        m_lastFusionTime = timestamp;
+        if (m_timeDelta <= 0)
+            return;
+
+        calculatePose(accel, compass);
+
+//      predict();
+
+        RTFLOAT x2, y2, z2;
+        RTFLOAT qs, qx, qy,qz;
+
+        qs = m_fusionQPose.scalar();
+        qx = m_fusionQPose.x();
+        qy = m_fusionQPose.y();
+        qz = m_fusionQPose.z();
+
+        if (m_enableGyro)
+            fusionGyro = gyro;
+        else
+            fusionGyro = RTVector3();
+
+        x2 = fusionGyro.x() / (RTFLOAT)2.0;
+        y2 = fusionGyro.y() / (RTFLOAT)2.0;
+        z2 = fusionGyro.z() / (RTFLOAT)2.0;
+
+        // Predict new state
+
+        m_fusionQPose.setScalar(qs + (-x2 * qx - y2 * qy - z2 * qz) * m_timeDelta);
+        m_fusionQPose.setX(qx + (x2 * qs + z2 * qy - y2 * qz) * m_timeDelta);
+        m_fusionQPose.setY(qy + (y2 * qs - z2 * qx + x2 * qz) * m_timeDelta);
+        m_fusionQPose.setZ(qz + (z2 * qs + y2 * qx - x2 * qy) * m_timeDelta);
+
+//      update();
+
+#ifdef USE_SLERP
+        if (m_enableCompass || m_enableAccel) {
+
+            // calculate rotation delta
+
+            m_rotationDelta = m_fusionQPose.conjugate() * m_measuredQPose;
+            m_rotationDelta.normalize();
+
+            // take it to the power (0 to 1) to give the desired amount of correction
+
+            RTFLOAT theta = acos(m_rotationDelta.scalar());
+
+            RTFLOAT sinPowerTheta = sin(theta * m_slerpPower);
+            RTFLOAT cosPowerTheta = cos(theta * m_slerpPower);
+
+            m_rotationUnitVector.setX(m_rotationDelta.x());
+            m_rotationUnitVector.setY(m_rotationDelta.y());
+            m_rotationUnitVector.setZ(m_rotationDelta.z());
+            m_rotationUnitVector.normalize();
+
+            m_rotationPower.setScalar(cosPowerTheta);
+            m_rotationPower.setX(sinPowerTheta * m_rotationUnitVector.x());
+            m_rotationPower.setY(sinPowerTheta * m_rotationUnitVector.y());
+            m_rotationPower.setZ(sinPowerTheta * m_rotationUnitVector.z());
+            m_rotationPower.normalize();
+
+            //  multiple this by predicted value to get result
+
+            m_fusionQPose *= m_rotationPower;
+        }
+#else
+        if (m_enableCompass || m_enableAccel) {
+            m_stateQError = m_measuredQPose - m_fusionQPose;
+        } else {
+            m_stateQError = RTQuaternion();
+        }
+        // make new state estimate
+
+        RTFLOAT qt = m_Q * m_timeDelta;
+
+        m_fusionQPose += m_stateQError * (qt / (qt + m_R));
+#endif
+
+        m_fusionQPose.normalize();
+
+        m_fusionQPose.toEuler(m_fusionPose);
+    }
+}
+
+void RTFusionRTQF::calculatePose(const RTVector3& accel, const RTVector3& mag)
+{
+    RTQuaternion m;
+    RTQuaternion q;
+
+    bool compassValid = (mag.x() != 0) || (mag.y() != 0) || (mag.z() != 0);
+
+    if (m_enableAccel) {
+        accel.accelToEuler(m_measuredPose);
+    } else {
+        m_measuredPose = m_fusionPose;
+    }
+
+    if (m_enableCompass && compassValid) {
+        RTFLOAT cosX2 = cos(m_measuredPose.x() / 2.0f);
+        RTFLOAT sinX2 = sin(m_measuredPose.x() / 2.0f);
+        RTFLOAT cosY2 = cos(m_measuredPose.y() / 2.0f);
+        RTFLOAT sinY2 = sin(m_measuredPose.y() / 2.0f);
+
+        q.setScalar(cosX2 * cosY2);
+        q.setX(sinX2 * cosY2);
+        q.setY(cosX2 * sinY2);
+        q.setZ( - sinX2 * sinY2);
+        
+        //   normalize();
+
+        m.setScalar(0);
+        m.setX(mag.x());
+        m.setY(mag.y());
+        m.setZ(mag.z());
+
+        m = q * m * q.conjugate();
+        m_measuredPose.setZ(-atan2(m.y(), m.x()));
+    } else {
+        m_measuredPose.setZ(m_fusionPose.z());
+    }
+
+    m_measuredQPose.fromEuler(m_measuredPose);
+
+    //  check for quaternion aliasing. If the quaternion has the wrong sign
+    //  the kalman filter will be very unhappy.
+
+    int maxIndex = -1;
+    RTFLOAT maxVal = -1000;
+
+    for (int i = 0; i < 4; i++) {
+        if (fabs(m_measuredQPose.data(i)) > maxVal) {
+            maxVal = fabs(m_measuredQPose.data(i));
+            maxIndex = i;
+        }
+    }
+
+    //  if the biggest component has a different sign in the measured and kalman poses,
+    //  change the sign of the measured pose to match.
+
+    if (((m_measuredQPose.data(maxIndex) < 0) && (m_fusionQPose.data(maxIndex) > 0)) ||
+            ((m_measuredQPose.data(maxIndex) > 0) && (m_fusionQPose.data(maxIndex) < 0))) {
+        m_measuredQPose.setScalar(-m_measuredQPose.scalar());
+        m_measuredQPose.setX(-m_measuredQPose.x());
+        m_measuredQPose.setY(-m_measuredQPose.y());
+        m_measuredQPose.setZ(-m_measuredQPose.z());
+        m_measuredQPose.toEuler(m_measuredPose);
+    }
+}
+#endif // #ifndef RTARDULINK_MODE

osc32_9255/RTIMULib/RTFusionRTQF.h

@@ -0,0 +1,101 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTFUSIONRTQF_H
+#define	_RTFUSIONRTQF_H
+
+#ifndef RTARDULINK_MODE
+
+#include "RTMath.h"
+
+//  Define this symbol to use more scientific prediction correction
+
+#define USE_SLERP
+
+class RTFusionRTQF
+{
+public:
+    RTFusionRTQF();
+    ~RTFusionRTQF();
+
+    //  reset() resets the state but keeps any setting changes (such as enables)
+
+    void reset();
+
+    //  newIMUData() should be called for subsequent updates
+    //  deltaTime is in units of seconds
+
+    void newIMUData(const RTVector3& gyro, const RTVector3& accel, const RTVector3& compass, unsigned long timestamp);
+
+    //  the following three functions control the influence of the gyro, accel and compass sensors
+
+    void setGyroEnable(bool enable) { m_enableGyro = enable;}
+    void setAccelEnable(bool enable) { m_enableAccel = enable; }
+    void setCompassEnable(bool enable) { m_enableCompass = enable;}
+
+#ifdef USE_SLERP
+    //  the following function can be called to set the SLERP power
+    void setSlerpPower(RTFLOAT power) { m_slerpPower = power; }
+#else
+    //  the following two functions can be called to customize the noise covariance
+
+    void setQ(RTFLOAT Q) {  m_Q = Q; reset();}
+    void setR(RTFLOAT R) { if (R > 0) m_R = R; reset();}
+#endif
+    inline const RTVector3& getMeasuredPose() {return m_measuredPose;}
+    inline const RTQuaternion& getMeasuredQPose() {return m_measuredQPose;}
+    inline const RTVector3& getFusionPose() {return m_fusionPose;}
+    inline const RTQuaternion& getFusionQPose() {return m_fusionQPose;}
+
+private:
+    void calculatePose(const RTVector3& accel, const RTVector3& mag); // generates pose from accels and heading
+
+    RTFLOAT m_timeDelta;                                    // time between predictions
+
+    RTQuaternion m_stateQError;                             // difference between stateQ and measuredQ
+
+#ifdef USE_SLERP
+    RTFLOAT m_slerpPower;                                   // a value 0 to 1 that controls measured state influence
+    RTQuaternion m_rotationDelta;                           // amount by which measured state differs from predicted
+    RTQuaternion m_rotationPower;                           // delta raised to the appopriate power
+    RTVector3 m_rotationUnitVector;                         // the vector part of the rotation delta
+#else
+    RTFLOAT m_Q;                                            // process noise covariance
+    RTFLOAT m_R;                                            // the measurement noise covariance
+#endif
+    RTQuaternion m_measuredQPose;       					// quaternion form of pose from measurement
+    RTVector3 m_measuredPose;								// vector form of pose from measurement
+    RTQuaternion m_fusionQPose;                             // quaternion form of pose from fusion
+    RTVector3 m_fusionPose;                                 // vector form of pose from fusion
+
+    bool m_enableGyro;                                      // enables gyro as input
+    bool m_enableAccel;                                     // enables accel as input
+    bool m_enableCompass;                                   // enables compass a input
+
+    bool m_firstTime;                                       // if first time after reset
+    unsigned long m_lastFusionTime;                         // for delta time calculation
+};
+
+#endif // #ifndef RTARDULINK_MODE
+
+#endif // _RTFUSIONRTQF_H

osc32_9255/RTIMULib/RTIMU.cpp

@@ -0,0 +1,357 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMU.h"
+#include "RTIMUSettings.h"
+#include "CalLib.h"
+
+//  this sets the learning rate for compass running average calculation
+
+#define COMPASS_ALPHA                   (RTFLOAT)0.2
+
+//  this defines the accelerometer noise level
+
+#define RTIMU_FUZZY_GYRO_ZERO           (RTFLOAT)0.20
+
+#define RTIMU_FUZZY_GYRO_ZERO_SQUARED   (RTIMU_FUZZY_GYRO_ZERO * RTIMU_FUZZY_GYRO_ZERO)
+
+//  this defines the accelerometer noise level
+
+#define RTIMU_FUZZY_ACCEL_ZERO          (RTFLOAT)0.05
+
+#define RTIMU_FUZZY_ACCEL_ZERO_SQUARED   (RTIMU_FUZZY_ACCEL_ZERO * RTIMU_FUZZY_ACCEL_ZERO)
+
+//  Axis rotation array
+
+#ifdef RTIMU_XNORTH_YEAST
+RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+#endif
+
+#ifdef RTIMU_XEAST_YSOUTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, -1, 0, 1, 0, 0, 0, 0, 1};
+#endif
+
+#ifdef RTIMU_XSOUTH_YWEST
+RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, -1, 0, 0, 0, 1};
+#endif
+
+#ifdef RTIMU_XWEST_YNORTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, -1, 0, 0, 0, 0, 1};
+#endif
+
+#ifdef RTIMU_XNORTH_YWEST
+RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
+#endif
+
+#ifdef RTIMU_XEAST_YNORTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, 1, 0, 0, 0, 0, -1};
+#endif
+
+#ifdef RTIMU_XSOUTH_YEAST
+RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1};
+#endif
+
+#ifdef RTIMU_XWEST_YSOUTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, -1, 0, -1, 0, 0, 0, 0, -1};
+#endif
+
+#ifdef RTIMU_XUP_YNORTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, 0, 0, -1, -1, 0, 0};
+#endif
+
+#ifdef RTIMU_XUP_YEAST
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, 0, 1, 0, -1, 0, 0};
+#endif
+
+#ifdef RTIMU_XUP_YSOUTH
+    {0, -1, 0, 0, 0, 1, -1, 0, 0};
+#endif
+
+#ifdef RTIMU_XUP_YWEST
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, 0, -1, 0, -1, 0, 0};
+#endif
+
+#ifdef RTIMU_XDOWN_YNORTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 1, 0, 0, 0, 1, 1, 0, 0};
+#endif
+
+#ifdef RTIMU_XDOWN_YEAST
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, 0, 1, 0, 1, 0, 0};
+#endif
+
+#ifdef RTIMU_XDOWN_YSOUTH
+RTFLOAT RTIMU::m_axisRotation[9] = {0, -1, 0, 0, 0, -1, 1, 0, 0};
+#endif
+
+#ifdef RTIMU_XDOWN_YWEST
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, 0, -1, 0, 1, 0, 0};
+#endif
+
+#ifdef RTIMU_XNORTH_YUP
+RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, 0, 1, 0, -1, 0};
+#endif
+
+#ifdef RTIMU_XEAST_YUP
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, 1, 0, 0, 0, -1, 0};
+#endif
+
+#ifdef RTIMU_XSOUTH_YUP
+RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, 0, -1, 0, -1, 0};
+#endif
+
+#ifdef RTIMU_XWEST_YUP
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, -1, 0, 0, 0, -1, 0};
+#endif
+
+#ifdef RTIMU_XNORTH_YDOWN
+RTFLOAT RTIMU::m_axisRotation[9] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
+#endif
+
+#ifdef RTIMU_XEAST_YDOWN
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, 1, 1, 0, 0, 0, 1, 0};
+#endif
+
+#ifdef RTIMU_XSOUTH_YDOWN
+RTFLOAT RTIMU::m_axisRotation[9] = {-1, 0, 0, 0, 0, 1, 0, 1, 0};
+#endif
+
+#ifdef RTIMU_XWEST_YDOWN
+RTFLOAT RTIMU::m_axisRotation[9] = {0, 0, -1, -1, 0, 0, 0, 1, 0};
+#endif
+
+
+#if defined(MPU9150_68) || defined(MPU9150_69)
+#include "RTIMUMPU9150.h"
+#endif
+
+#if defined(MPU9250_68) || defined(MPU9250_69)
+#include "RTIMUMPU9250.h"
+#endif
+
+#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
+#include "RTIMULSM9DS0.h"
+#endif
+
+#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
+#include "RTIMUGD20HM303D.h"
+#endif
+
+#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
+#include "RTIMUGD20M303DLHC.h"
+#endif
+
+#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
+#include "RTIMUGD20HM303DLHC.h"
+#endif
+
+#if defined(BNO055_28) || defined(BNO055_29)
+#include "RTIMUBNO055.h"
+#endif
+
+RTIMU *RTIMU::createIMU(RTIMUSettings *settings)
+{
+#if defined(MPU9150_68) || defined(MPU9150_69)
+    return new RTIMUMPU9150(settings);
+#endif
+#if defined(MPU9250_68) || defined(MPU9250_69)
+    return new RTIMUMPU9250(settings);
+#endif
+#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
+    return new RTIMULSM9DS0(settings);
+#endif
+#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
+    return new RTIMUGD20HM303D(settings);
+#endif
+#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
+    return new RTIMUGD20M303DLHC(settings);
+#endif
+#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
+    return new RTIMUGD20HM303DLHC(settings);
+#endif
+#if defined(BNO055_28) || defined(BNO055_29)
+    return new RTIMUBNO055(settings);
+#endif
+}
+
+
+RTIMU::RTIMU(RTIMUSettings *settings)
+{
+    m_settings = settings;
+
+    m_calibrationMode = false;
+    m_calibrationValid = false;
+    m_gyroBiasValid = false;
+}
+
+RTIMU::~RTIMU()
+{
+}
+
+void RTIMU::setCalibrationData()
+{
+    float maxDelta = -1;
+    float delta;
+    CALLIB_DATA calData;
+
+    m_calibrationValid = false;
+
+    if (calLibRead(0, &calData)) {
+        if (calData.magValid != 1) {
+            return;
+        }
+
+        //  find biggest range
+
+        for (int i = 0; i < 3; i++) {
+            if ((calData.magMax[i] - calData.magMin[i]) > maxDelta)
+                maxDelta = calData.magMax[i] - calData.magMin[i];
+        }
+        if (maxDelta < 0) {
+            return;
+        }
+        maxDelta /= 2.0f;                                       // this is the max +/- range
+
+        for (int i = 0; i < 3; i++) {
+            delta = (calData.magMax[i] - calData.magMin[i]) / 2.0f;
+            m_compassCalScale[i] = maxDelta / delta;            // makes everything the same range
+            m_compassCalOffset[i] = (calData.magMax[i] + calData.magMin[i]) / 2.0f;
+        }
+        m_calibrationValid = true;
+    }
+}
+
+void RTIMU::gyroBiasInit()
+{
+    m_gyroAlpha = 2.0f / m_sampleRate;
+    m_gyroSampleCount = 0;
+}
+
+//  Note - code assumes that this is the first thing called after axis swapping
+//  for each specific IMU chip has occurred.
+
+void RTIMU::handleGyroBias()
+{
+    // do axis rotation if necessary
+#ifndef  RTIMU_XNORTH_YEAST
+    // need to do an axis rotation
+    float *matrix = m_axisRotation;
+    RTVector3 tempGyro = m_gyro;
+    RTVector3 tempAccel = m_accel;
+    RTVector3 tempCompass = m_compass;
+
+    // do new x value
+    if (matrix[0] != 0) {
+        m_gyro.setX(tempGyro.x() * matrix[0]);
+        m_accel.setX(tempAccel.x() * matrix[0]);
+        m_compass.setX(tempCompass.x() * matrix[0]);
+    } else if (matrix[1] != 0) {
+        m_gyro.setX(tempGyro.y() * matrix[1]);
+        m_accel.setX(tempAccel.y() * matrix[1]);
+        m_compass.setX(tempCompass.y() * matrix[1]);
+    } else if (matrix[2] != 0) {
+        m_gyro.setX(tempGyro.z() * matrix[2]);
+        m_accel.setX(tempAccel.z() * matrix[2]);
+        m_compass.setX(tempCompass.z() * matrix[2]);
+    }
+
+    // do new y value
+    if (matrix[3] != 0) {
+        m_gyro.setY(tempGyro.x() * matrix[3]);
+        m_accel.setY(tempAccel.x() * matrix[3]);
+        m_compass.setY(tempCompass.x() * matrix[3]);
+    } else if (matrix[4] != 0) {
+        m_gyro.setY(tempGyro.y() * matrix[4]);
+        m_accel.setY(tempAccel.y() * matrix[4]);
+        m_compass.setY(tempCompass.y() * matrix[4]);
+    } else if (matrix[5] != 0) {
+        m_gyro.setY(tempGyro.z() * matrix[5]);
+        m_accel.setY(tempAccel.z() * matrix[5]);
+        m_compass.setY(tempCompass.z() * matrix[5]);
+    }
+
+    // do new z value
+    if (matrix[6] != 0) {
+        m_gyro.setZ(tempGyro.x() * matrix[6]);
+        m_accel.setZ(tempAccel.x() * matrix[6]);
+        m_compass.setZ(tempCompass.x() * matrix[6]);
+    } else if (matrix[7] != 0) {
+        m_gyro.setZ(tempGyro.y() * matrix[7]);
+        m_accel.setZ(tempAccel.y() * matrix[7]);
+        m_compass.setZ(tempCompass.y() * matrix[7]);
+    } else if (matrix[8] != 0) {
+        m_gyro.setZ(tempGyro.z() * matrix[8]);
+        m_accel.setZ(tempAccel.z() * matrix[8]);
+        m_compass.setZ(tempCompass.z() * matrix[8]);
+    }
+#endif
+    
+    if (!m_gyroBiasValid) {
+        RTVector3 deltaAccel = m_previousAccel;
+        deltaAccel -= m_accel;   // compute difference
+        m_previousAccel = m_accel;
+
+        if ((deltaAccel.squareLength() < RTIMU_FUZZY_ACCEL_ZERO_SQUARED) &&
+            (m_gyro.squareLength() < RTIMU_FUZZY_GYRO_ZERO_SQUARED)) {
+            // what we are seeing on the gyros should be bias only so learn from this
+            m_gyroBias.setX((1.0 - m_gyroAlpha) * m_gyroBias.x() + m_gyroAlpha * m_gyro.x());
+            m_gyroBias.setY((1.0 - m_gyroAlpha) * m_gyroBias.y() + m_gyroAlpha * m_gyro.y());
+            m_gyroBias.setZ((1.0 - m_gyroAlpha) * m_gyroBias.z() + m_gyroAlpha * m_gyro.z());
+
+            if (m_gyroSampleCount < (5 * m_sampleRate)) {
+                m_gyroSampleCount++;
+
+                if (m_gyroSampleCount == (5 * m_sampleRate)) {
+                    m_gyroBiasValid = true;
+                }
+            }
+        }
+    }
+
+    m_gyro -= m_gyroBias;
+}
+
+void RTIMU::calibrateAverageCompass()
+{
+    //  calibrate if required
+
+    if (!m_calibrationMode && m_calibrationValid) {
+        m_compass.setX((m_compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
+        m_compass.setY((m_compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
+        m_compass.setZ((m_compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
+    }
+
+    //  update running average
+
+    m_compassAverage.setX(m_compass.x() * COMPASS_ALPHA + m_compassAverage.x() * (1.0 - COMPASS_ALPHA));
+    m_compassAverage.setY(m_compass.y() * COMPASS_ALPHA + m_compassAverage.y() * (1.0 - COMPASS_ALPHA));
+    m_compassAverage.setZ(m_compass.z() * COMPASS_ALPHA + m_compassAverage.z() * (1.0 - COMPASS_ALPHA));
+
+    m_compass = m_compassAverage;
+}
+
+bool RTIMU::IMUGyroBiasValid()
+{
+    return m_gyroBiasValid;
+}
+
+

osc32_9255/RTIMULib/RTIMU.h

@@ -0,0 +1,114 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMU_H
+#define	_RTIMU_H
+
+#include "RTMath.h"
+#include "RTIMULibDefs.h"
+#include "I2Cdev.h"
+
+#define I2CWrite(x, y, z) I2Cdev::writeByte(x, y, z)
+#define I2CRead(w, x, y, z) I2Cdev::readBytes(w, x, y, z)
+
+class RTIMUSettings;
+
+class RTIMU
+{
+public:
+    //  IMUs should always be created with the following call
+
+    static RTIMU *createIMU(RTIMUSettings *settings);
+
+    //  Constructor/destructor
+
+    RTIMU(RTIMUSettings *settings);
+    virtual ~RTIMU();
+
+    //  These functions must be provided by sub classes
+
+    virtual const char *IMUName() = 0;                      // the name of the IMU
+    virtual int IMUType() = 0;                              // the type code of the IMU
+    virtual int IMUInit() = 0;                              // set up the IMU
+    virtual int IMUGetPollInterval() = 0;                   // returns the recommended poll interval in mS
+    virtual bool IMURead() = 0;                             // get a sample
+
+    //  This one wanted a similar name but isn't pure virtual
+
+    virtual bool IMUCompassCalValid() { return m_calibrationValid; }
+
+    //  setCalibrationMode() turns off use of cal data so that raw data can be accumulated
+    //  to derive calibration data
+
+    void setCalibrationMode(bool enable) { m_calibrationMode = enable; }
+
+    //  setCalibrationData configured the cal data and also enables use if valid
+
+    void setCalibrationData();
+
+    //  getCalibrationValid() returns true if the calibration data is being used
+
+    bool getCalibrationValid() { return !m_calibrationMode && m_calibrationValid; }
+
+    // returns true if enough samples for valid data
+
+    virtual bool IMUGyroBiasValid();
+
+    inline const RTVector3& getGyro() { return m_gyro; }            // gets gyro rates in radians/sec
+    inline const RTVector3& getAccel() { return m_accel; }          // get accel data in gs
+    inline const RTVector3& getCompass() { return m_compass; }      // gets compass data in uT
+    inline unsigned long getTimestamp() { return m_timestamp; }     // and the timestamp for it
+
+protected:
+    void gyroBiasInit();                                    // sets up gyro bias calculation
+    void handleGyroBias();                                  // adjust gyro for bias
+    void calibrateAverageCompass();                         // calibrate and smooth compass
+    bool m_calibrationMode;                                 // true if cal mode so don't use cal data!
+    bool m_calibrationValid;                                // tru if call data is valid and can be used
+
+    RTVector3 m_gyro;                                       // the gyro readings
+    RTVector3 m_accel;                                      // the accel readings
+    RTVector3 m_compass;                                    // the compass readings
+    unsigned long m_timestamp;                              // the timestamp
+
+    RTIMUSettings *m_settings;                              // the settings object pointer
+
+    int m_sampleRate;                                       // samples per second
+    uint64_t m_sampleInterval;                              // interval between samples in microseonds
+
+    RTFLOAT m_gyroAlpha;                                    // gyro bias learning rate
+    int m_gyroSampleCount;                                  // number of gyro samples used
+    bool m_gyroBiasValid;                                   // true if the recorded gyro bias is valid
+    RTVector3 m_gyroBias;                                   // the recorded gyro bias
+
+    RTVector3 m_previousAccel;                              // previous step accel for gyro learning
+
+    RTFLOAT m_compassCalOffset[3];
+    RTFLOAT m_compassCalScale[3];
+    RTVector3 m_compassAverage;                             // a running average to smooth the mag outputs
+
+    static RTFLOAT m_axisRotation[9];                         // axis rotation matrix
+
+ };
+
+#endif // _RTIMU_H

osc32_9255/RTIMULib/RTIMUBNO055.cpp

@@ -0,0 +1,180 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+//  Based on the Adafruit BNO055 driver:
+
+/***************************************************************************
+  This is a library for the BNO055 orientation sensor
+  Designed specifically to work with the Adafruit BNO055 Breakout.
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/products
+  These sensors use I2C to communicate, 2 pins are required to interface.
+  Adafruit invests time and resources providing this open source code,
+  please support Adafruit andopen-source hardware by purchasing products
+  from Adafruit!
+  Written by KTOWN for Adafruit Industries.
+  MIT license, all text above must be included in any redistribution
+ ***************************************************************************/
+
+#include "RTIMUBNO055.h"
+#include "RTIMUSettings.h"
+#if defined(BNO055_28) || defined(BNO055_29)
+
+RTIMUBNO055::RTIMUBNO055(RTIMUSettings *settings) : RTIMU(settings)
+{
+    m_sampleRate = 100;
+    m_sampleInterval = (unsigned long)1000 / m_sampleRate;
+}
+
+RTIMUBNO055::~RTIMUBNO055()
+{
+}
+
+int RTIMUBNO055::IMUInit()
+{
+    unsigned char result;
+
+    m_slaveAddr = m_settings->m_I2CSlaveAddress;
+    m_lastReadTime = millis();
+
+    if (!I2Cdev::readByte(m_slaveAddr, BNO055_WHO_AM_I, &result))
+        return -1;
+
+    if (result != BNO055_ID) {
+        return -2;
+    }
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_CONFIG))
+        return -3;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_SYS_TRIGGER, 0x20))
+        return -4;
+
+    delay(50);
+
+    while (1) {
+        if (!I2Cdev::readByte(m_slaveAddr, BNO055_WHO_AM_I, &result))
+            continue;
+        if (result == BNO055_ID)
+            break;
+        delay(50);
+    }
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_PWR_MODE, BNO055_PWR_MODE_NORMAL))
+        return -5;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_PAGE_ID, 0))
+        return -6;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_SYS_TRIGGER, 0x00))
+        return -7;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_UNIT_SEL, 0x87))
+        return -8;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, BNO055_OPER_MODE, BNO055_OPER_MODE_NDOF))
+        return -9;
+
+    delay(50);
+
+    return 1;
+}
+
+int RTIMUBNO055::IMUGetPollInterval()
+{
+    return (7);
+}
+
+bool RTIMUBNO055::IMURead()
+{
+    unsigned char buffer[24];
+
+    if ((millis() - m_lastReadTime) < m_sampleInterval)
+        return false;                                       // too soon
+
+    m_lastReadTime = millis();
+    if (!I2Cdev::readBytes(m_slaveAddr, BNO055_ACCEL_DATA, 24, buffer))
+        return false;
+
+    int16_t x, y, z;
+
+    // process accel data
+
+    x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
+    y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
+    z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
+
+    m_accel.setX((RTFLOAT)y / 1000.0);
+    m_accel.setY((RTFLOAT)x / 1000.0);
+    m_accel.setZ((RTFLOAT)z / 1000.0);
+
+    // process mag data
+
+    x = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
+    y = (((uint16_t)buffer[9]) << 8) | ((uint16_t)buffer[8]);
+    z = (((uint16_t)buffer[11]) << 8) | ((uint16_t)buffer[10]);
+
+    m_compass.setX(-(RTFLOAT)y / 16.0);
+    m_compass.setY(-(RTFLOAT)x / 16.0);
+    m_compass.setZ(-(RTFLOAT)z / 16.0);
+
+    // process gyro data
+
+    x = (((uint16_t)buffer[13]) << 8) | ((uint16_t)buffer[12]);
+    y = (((uint16_t)buffer[15]) << 8) | ((uint16_t)buffer[14]);
+    z = (((uint16_t)buffer[17]) << 8) | ((uint16_t)buffer[16]);
+
+    m_gyro.setX(-(RTFLOAT)y / 900.0);
+    m_gyro.setY(-(RTFLOAT)x / 900.0);
+    m_gyro.setZ(-(RTFLOAT)z / 900.0);
+
+    // process euler angles
+
+    x = (((uint16_t)buffer[19]) << 8) | ((uint16_t)buffer[18]);
+    y = (((uint16_t)buffer[21]) << 8) | ((uint16_t)buffer[20]);
+    z = (((uint16_t)buffer[23]) << 8) | ((uint16_t)buffer[22]);
+
+    //  put in structure and do axis remap
+
+    m_fusionPose.setX((RTFLOAT)y / 900.0);
+    m_fusionPose.setY((RTFLOAT)z / 900.0);
+    m_fusionPose.setZ((RTFLOAT)x / 900.0);
+
+    m_fusionQPose.fromEuler(m_fusionPose);
+
+    m_timestamp = millis();
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMUBNO055.h

@@ -0,0 +1,85 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+
+#ifndef _RTIMUBNO055_H
+#define	_RTIMUBNO055_H
+
+#include "RTIMU.h"
+
+//  I2C Slave Addresses
+
+#define BNO055_ADDRESS0             0x28
+#define BNO055_ADDRESS1             0x29
+#define BNO055_ID                   0xa0
+
+//  Register map
+
+#define BNO055_WHO_AM_I             0x00
+#define BNO055_PAGE_ID              0x07
+#define BNO055_ACCEL_DATA           0x08
+#define BNO055_MAG_DATA             0x0e
+#define BNO055_GYRO_DATA            0x14
+#define BNO055_FUSED_EULER          0x1a
+#define BNO055_FUSED_QUAT           0x20
+#define BNO055_UNIT_SEL             0x3b
+#define BNO055_OPER_MODE            0x3d
+#define BNO055_PWR_MODE             0x3e
+#define BNO055_SYS_TRIGGER          0x3f
+#define BNO055_AXIS_MAP_CONFIG      0x41
+#define BNO055_AXIS_MAP_SIGN        0x42
+
+//  Operation modes
+
+#define BNO055_OPER_MODE_CONFIG     0x00
+#define BNO055_OPER_MODE_NDOF       0x0c
+
+//  Power modes
+
+#define BNO055_PWR_MODE_NORMAL      0x00
+
+class RTIMUBNO055 : public RTIMU
+{
+public:
+    RTIMUBNO055(RTIMUSettings *settings);
+    ~RTIMUBNO055();
+
+    inline const RTVector3& getFusionPose() { return m_fusionPose; }
+    inline const RTQuaternion& getFusionQPose() { return m_fusionQPose; }
+
+    virtual const char *IMUName() { return "BNO055"; }
+    virtual int IMUType() { return RTIMU_TYPE_BNO055; }
+    virtual int IMUInit();
+    virtual int IMUGetPollInterval();
+    virtual bool IMURead();
+
+private:
+    unsigned char m_slaveAddr;                              // I2C address of BNO055
+
+    uint64_t m_lastReadTime;
+
+    RTQuaternion m_fusionQPose;
+    RTVector3 m_fusionPose;
+};
+
+#endif // _RTIMUBNO055_H

osc32_9255/RTIMULib/RTIMUGD20HM303D.cpp

@@ -0,0 +1,402 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMUGD20HM303D.h"
+#include "RTIMUSettings.h"
+
+#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
+
+RTIMUGD20HM303D::RTIMUGD20HM303D(RTIMUSettings *settings) : RTIMU(settings)
+{
+    m_sampleRate = 100;
+}
+
+RTIMUGD20HM303D::~RTIMUGD20HM303D()
+{
+}
+
+int RTIMUGD20HM303D::IMUInit()
+{
+    unsigned char result;
+
+    //  configure IMU
+
+    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+    if (m_gyroSlaveAddr == L3GD20H_ADDRESS0)
+        m_accelCompassSlaveAddr = LSM303D_ADDRESS0;
+    else
+        m_accelCompassSlaveAddr = LSM303D_ADDRESS1;
+
+    setCalibrationData();
+
+    //  Set up the gyro
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04))
+        return -1;
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80))
+        return -2;
+
+    if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result))
+        return -3;
+
+    if (result != L3GD20H_ID) {
+        return -4;
+    }
+
+    if (!setGyroSampleRate())
+            return -5;
+
+    if (!setGyroCTRL2())
+            return -6;
+
+    if (!setGyroCTRL4())
+            return -7;
+
+    //  Set up the accel/compass
+
+    if (!I2CRead(m_accelCompassSlaveAddr, LSM303D_WHO_AM_I, 1, &result))
+        return -8;
+
+    if (result != LSM303D_ID) {
+        return -9;
+    }
+
+    if (!setAccelCTRL1())
+        return -10;
+
+    if (!setAccelCTRL2())
+        return -11;
+
+    if (!setCompassCTRL5())
+        return -12;
+
+    if (!setCompassCTRL6())
+        return -13;
+
+    if (!setCompassCTRL7())
+        return -14;
+
+    if (!setGyroCTRL5())
+            return -16;
+
+    gyroBiasInit();
+
+    return true;
+}
+
+bool RTIMUGD20HM303D::setGyroSampleRate()
+{
+    unsigned char ctrl1;
+    unsigned char lowOdr = 0;
+
+    switch (m_settings->m_GD20HM303DGyroSampleRate) {
+    case L3GD20H_SAMPLERATE_12_5:
+        ctrl1 = 0x0f;
+        lowOdr = 1;
+        m_sampleRate = 13;
+        break;
+
+    case L3GD20H_SAMPLERATE_25:
+        ctrl1 = 0x4f;
+        lowOdr = 1;
+        m_sampleRate = 25;
+        break;
+
+    case L3GD20H_SAMPLERATE_50:
+        ctrl1 = 0x8f;
+        lowOdr = 1;
+        m_sampleRate = 50;
+        break;
+
+    case L3GD20H_SAMPLERATE_100:
+        ctrl1 = 0x0f;
+        m_sampleRate = 100;
+        break;
+
+    case L3GD20H_SAMPLERATE_200:
+        ctrl1 = 0x4f;
+        m_sampleRate = 200;
+        break;
+
+    case L3GD20H_SAMPLERATE_400:
+        ctrl1 = 0x8f;
+        m_sampleRate = 400;
+        break;
+
+    case L3GD20H_SAMPLERATE_800:
+        ctrl1 = 0xcf;
+        m_sampleRate = 800;
+        break;
+
+    default:
+        return false;
+    }
+
+    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+    switch (m_settings->m_GD20HM303DGyroBW) {
+    case L3GD20H_BANDWIDTH_0:
+        ctrl1 |= 0x00;
+        break;
+
+    case L3GD20H_BANDWIDTH_1:
+        ctrl1 |= 0x10;
+        break;
+
+    case L3GD20H_BANDWIDTH_2:
+        ctrl1 |= 0x20;
+        break;
+
+    case L3GD20H_BANDWIDTH_3:
+        ctrl1 |= 0x30;
+        break;
+
+    }
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, lowOdr))
+        return false;
+
+    return (I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL1, ctrl1));
+}
+
+bool RTIMUGD20HM303D::setGyroCTRL2()
+{
+    if ((m_settings->m_GD20HM303DGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DGyroHpf > L3GD20H_HPF_9)) {
+        return false;
+    }
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20H_CTRL2, m_settings->m_GD20HM303DGyroHpf);
+}
+
+bool RTIMUGD20HM303D::setGyroCTRL4()
+{
+    unsigned char ctrl4;
+
+    switch (m_settings->m_GD20HM303DGyroFsr) {
+    case L3GD20H_FSR_245:
+        ctrl4 = 0x00;
+        m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case L3GD20H_FSR_500:
+        ctrl4 = 0x10;
+        m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case L3GD20H_FSR_2000:
+        ctrl4 = 0x20;
+        m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20H_CTRL4, ctrl4);
+}
+
+
+bool RTIMUGD20HM303D::setGyroCTRL5()
+{
+    unsigned char ctrl5;
+
+    //  Turn on hpf
+
+    ctrl5 = 0x10;
+
+#ifdef GD20HM303D_CACHE_MODE
+    //  turn on fifo
+
+    ctrl5 |= 0x40;
+#endif
+
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20H_CTRL5, ctrl5);
+}
+
+
+bool RTIMUGD20HM303D::setAccelCTRL1()
+{
+    unsigned char ctrl1;
+
+    if ((m_settings->m_GD20HM303DAccelSampleRate < 0) || (m_settings->m_GD20HM303DAccelSampleRate > 10)) {
+        return false;
+    }
+
+    ctrl1 = (m_settings->m_GD20HM303DAccelSampleRate << 4) | 0x07;
+
+    return I2CWrite(m_accelCompassSlaveAddr,  LSM303D_CTRL1, ctrl1);
+}
+
+bool RTIMUGD20HM303D::setAccelCTRL2()
+{
+    unsigned char ctrl2;
+
+    if ((m_settings->m_GD20HM303DAccelLpf < 0) || (m_settings->m_GD20HM303DAccelLpf > 3)) {
+        return false;
+    }
+
+    switch (m_settings->m_GD20HM303DAccelFsr) {
+    case LSM303D_ACCEL_FSR_2:
+        m_accelScale = (RTFLOAT)0.000061;
+        break;
+
+    case LSM303D_ACCEL_FSR_4:
+        m_accelScale = (RTFLOAT)0.000122;
+        break;
+
+    case LSM303D_ACCEL_FSR_6:
+        m_accelScale = (RTFLOAT)0.000183;
+        break;
+
+    case LSM303D_ACCEL_FSR_8:
+        m_accelScale = (RTFLOAT)0.000244;
+        break;
+
+    case LSM303D_ACCEL_FSR_16:
+        m_accelScale = (RTFLOAT)0.000732;
+        break;
+
+    default:
+        return false;
+    }
+
+    ctrl2 = (m_settings->m_GD20HM303DAccelLpf << 6) | (m_settings->m_GD20HM303DAccelFsr << 3);
+
+    return I2CWrite(m_accelCompassSlaveAddr,  LSM303D_CTRL2, ctrl2);
+}
+
+
+bool RTIMUGD20HM303D::setCompassCTRL5()
+{
+    unsigned char ctrl5;
+
+    if ((m_settings->m_GD20HM303DCompassSampleRate < 0) || (m_settings->m_GD20HM303DCompassSampleRate > 5)) {
+        return false;
+    }
+
+    ctrl5 = (m_settings->m_GD20HM303DCompassSampleRate << 2);
+
+#ifdef GD20HM303D_CACHE_MODE
+    //  enable fifo
+
+    ctrl5 |= 0x40;
+#endif
+
+    return I2CWrite(m_accelCompassSlaveAddr,  LSM303D_CTRL5, ctrl5);
+}
+
+bool RTIMUGD20HM303D::setCompassCTRL6()
+{
+    unsigned char ctrl6;
+
+    //  convert FSR to uT
+
+    switch (m_settings->m_GD20HM303DCompassFsr) {
+    case LSM303D_COMPASS_FSR_2:
+        ctrl6 = 0;
+        m_compassScale = (RTFLOAT)0.008;
+        break;
+
+    case LSM303D_COMPASS_FSR_4:
+        ctrl6 = 0x20;
+        m_compassScale = (RTFLOAT)0.016;
+        break;
+
+    case LSM303D_COMPASS_FSR_8:
+        ctrl6 = 0x40;
+        m_compassScale = (RTFLOAT)0.032;
+        break;
+
+    case LSM303D_COMPASS_FSR_12:
+        ctrl6 = 0x60;
+        m_compassScale = (RTFLOAT)0.0479;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2CWrite(m_accelCompassSlaveAddr,  LSM303D_CTRL6, ctrl6);
+}
+
+bool RTIMUGD20HM303D::setCompassCTRL7()
+{
+     return I2CWrite(m_accelCompassSlaveAddr,  LSM303D_CTRL7, 0x60);
+}
+
+
+int RTIMUGD20HM303D::IMUGetPollInterval()
+{
+    return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20HM303D::IMURead()
+{
+    unsigned char status;
+    unsigned char gyroData[6];
+    unsigned char accelData[6];
+    unsigned char compassData[6];
+
+    if (!I2CRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status))
+        return false;
+
+    if ((status & 0x8) == 0)
+        return false;
+
+    if (!I2CRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData))
+        return false;
+
+    m_timestamp = millis();
+
+    if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_A, 6, accelData))
+        return false;
+
+    if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM303D_OUT_X_L_M, 6, compassData))
+        return false;
+
+    RTMath::convertToVector(gyroData, m_gyro, m_gyroScale, false);
+    RTMath::convertToVector(accelData, m_accel, m_accelScale, false);
+    RTMath::convertToVector(compassData, m_compass, m_compassScale, false);
+
+    //  sort out gyro axes
+
+    m_gyro.setY(-m_gyro.y());
+    m_gyro.setZ(-m_gyro.z());
+
+    //  sort out accel data;
+
+    m_accel.setX(-m_accel.x());
+
+    //  sort out compass axes
+
+    m_compass.setY(-m_compass.y());
+    m_compass.setZ(-m_compass.z());
+
+    //  now do standard processing
+
+    handleGyroBias();
+    calibrateAverageCompass();
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMUGD20HM303D.h

@@ -0,0 +1,238 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMUGD20HM303D_H
+#define	_RTIMUGD20HM303D_H
+
+#include "RTIMU.h"
+
+//  I2C Slave Addresses
+
+#define L3GD20H_ADDRESS0        0x69
+#define L3GD20H_ADDRESS1        0x6e
+#define L3GD20H_ID              0xd7
+
+#define LSM303D_ADDRESS0        0x19
+#define LSM303D_ADDRESS1        0x1e
+#define LSM303D_ID              0x49
+
+//  L3GD20H Register map
+
+#define L3GD20H_WHO_AM_I        0x0f
+#define L3GD20H_CTRL1           0x20
+#define L3GD20H_CTRL2           0x21
+#define L3GD20H_CTRL3           0x22
+#define L3GD20H_CTRL4           0x23
+#define L3GD20H_CTRL5           0x24
+#define L3GD20H_OUT_TEMP        0x26
+#define L3GD20H_STATUS          0x27
+#define L3GD20H_OUT_X_L         0x28
+#define L3GD20H_OUT_X_H         0x29
+#define L3GD20H_OUT_Y_L         0x2a
+#define L3GD20H_OUT_Y_H         0x2b
+#define L3GD20H_OUT_Z_L         0x2c
+#define L3GD20H_OUT_Z_H         0x2d
+#define L3GD20H_FIFO_CTRL       0x2e
+#define L3GD20H_FIFO_SRC        0x2f
+#define L3GD20H_IG_CFG          0x30
+#define L3GD20H_IG_SRC          0x31
+#define L3GD20H_IG_THS_XH       0x32
+#define L3GD20H_IG_THS_XL       0x33
+#define L3GD20H_IG_THS_YH       0x34
+#define L3GD20H_IG_THS_YL       0x35
+#define L3GD20H_IG_THS_ZH       0x36
+#define L3GD20H_IG_THS_ZL       0x37
+#define L3GD20H_IG_DURATION     0x38
+#define L3GD20H_LOW_ODR         0x39
+
+//  Gyro sample rate defines
+
+#define L3GD20H_SAMPLERATE_12_5 0
+#define L3GD20H_SAMPLERATE_25   1
+#define L3GD20H_SAMPLERATE_50   2
+#define L3GD20H_SAMPLERATE_100  3
+#define L3GD20H_SAMPLERATE_200  4
+#define L3GD20H_SAMPLERATE_400  5
+#define L3GD20H_SAMPLERATE_800  6
+
+//  Gyro banwidth defines
+
+#define L3GD20H_BANDWIDTH_0     0
+#define L3GD20H_BANDWIDTH_1     1
+#define L3GD20H_BANDWIDTH_2     2
+#define L3GD20H_BANDWIDTH_3     3
+
+//  Gyro FSR defines
+
+#define L3GD20H_FSR_245         0
+#define L3GD20H_FSR_500         1
+#define L3GD20H_FSR_2000        2
+
+//  Gyro high pass filter defines
+
+#define L3GD20H_HPF_0           0
+#define L3GD20H_HPF_1           1
+#define L3GD20H_HPF_2           2
+#define L3GD20H_HPF_3           3
+#define L3GD20H_HPF_4           4
+#define L3GD20H_HPF_5           5
+#define L3GD20H_HPF_6           6
+#define L3GD20H_HPF_7           7
+#define L3GD20H_HPF_8           8
+#define L3GD20H_HPF_9           9
+
+//  LSM303D Register Map
+
+#define LSM303D_TEMP_OUT_L      0x05
+#define LSM303D_TEMP_OUT_H      0x06
+#define LSM303D_STATUS_M        0x07
+#define LSM303D_OUT_X_L_M       0x08
+#define LSM303D_OUT_X_H_M       0x09
+#define LSM303D_OUT_Y_L_M       0x0a
+#define LSM303D_OUT_Y_H_M       0x0b
+#define LSM303D_OUT_Z_L_M       0x0c
+#define LSM303D_OUT_Z_H_M       0x0d
+#define LSM303D_WHO_AM_I        0x0f
+#define LSM303D_INT_CTRL_M      0x12
+#define LSM303D_INT_SRC_M       0x13
+#define LSM303D_INT_THS_L_M     0x14
+#define LSM303D_INT_THS_H_M     0x15
+#define LSM303D_OFFSET_X_L_M    0x16
+#define LSM303D_OFFSET_X_H_M    0x17
+#define LSM303D_OFFSET_Y_L_M    0x18
+#define LSM303D_OFFSET_Y_H_M    0x19
+#define LSM303D_OFFSET_Z_L_M    0x1a
+#define LSM303D_OFFSET_Z_H_M    0x1b
+#define LSM303D_REFERENCE_X     0x1c
+#define LSM303D_REFERENCE_Y     0x1d
+#define LSM303D_REFERENCE_Z     0x1e
+#define LSM303D_CTRL0           0x1f
+#define LSM303D_CTRL1           0x20
+#define LSM303D_CTRL2           0x21
+#define LSM303D_CTRL3           0x22
+#define LSM303D_CTRL4           0x23
+#define LSM303D_CTRL5           0x24
+#define LSM303D_CTRL6           0x25
+#define LSM303D_CTRL7           0x26
+#define LSM303D_STATUS_A        0x27
+#define LSM303D_OUT_X_L_A       0x28
+#define LSM303D_OUT_X_H_A       0x29
+#define LSM303D_OUT_Y_L_A       0x2a
+#define LSM303D_OUT_Y_H_A       0x2b
+#define LSM303D_OUT_Z_L_A       0x2c
+#define LSM303D_OUT_Z_H_A       0x2d
+#define LSM303D_FIFO_CTRL       0x2e
+#define LSM303D_FIFO_SRC        0x2f
+#define LSM303D_IG_CFG1         0x30
+#define LSM303D_IG_SRC1         0x31
+#define LSM303D_IG_THS1         0x32
+#define LSM303D_IG_DUR1         0x33
+#define LSM303D_IG_CFG2         0x34
+#define LSM303D_IG_SRC2         0x35
+#define LSM303D_IG_THS2         0x36
+#define LSM303D_IG_DUR2         0x37
+#define LSM303D_CLICK_CFG       0x38
+#define LSM303D_CLICK_SRC       0x39
+#define LSM303D_CLICK_THS       0x3a
+#define LSM303D_TIME_LIMIT      0x3b
+#define LSM303D_TIME_LATENCY    0x3c
+#define LSM303D_TIME_WINDOW     0x3d
+#define LSM303D_ACT_THIS        0x3e
+#define LSM303D_ACT_DUR         0x3f
+
+//  Accel sample rate defines
+
+#define LSM303D_ACCEL_SAMPLERATE_3_125 1
+#define LSM303D_ACCEL_SAMPLERATE_6_25 2
+#define LSM303D_ACCEL_SAMPLERATE_12_5 3
+#define LSM303D_ACCEL_SAMPLERATE_25   4
+#define LSM303D_ACCEL_SAMPLERATE_50   5
+#define LSM303D_ACCEL_SAMPLERATE_100  6
+#define LSM303D_ACCEL_SAMPLERATE_200  7
+#define LSM303D_ACCEL_SAMPLERATE_400  8
+#define LSM303D_ACCEL_SAMPLERATE_800  9
+#define LSM303D_ACCEL_SAMPLERATE_1600 10
+
+//  Accel FSR
+
+#define LSM303D_ACCEL_FSR_2     0
+#define LSM303D_ACCEL_FSR_4     1
+#define LSM303D_ACCEL_FSR_6     2
+#define LSM303D_ACCEL_FSR_8     3
+#define LSM303D_ACCEL_FSR_16    4
+
+//  Accel filter bandwidth
+
+#define LSM303D_ACCEL_LPF_773   0
+#define LSM303D_ACCEL_LPF_194   1
+#define LSM303D_ACCEL_LPF_362   2
+#define LSM303D_ACCEL_LPF_50    3
+
+//  Compass sample rate defines
+
+#define LSM303D_COMPASS_SAMPLERATE_3_125    0
+#define LSM303D_COMPASS_SAMPLERATE_6_25     1
+#define LSM303D_COMPASS_SAMPLERATE_12_5     2
+#define LSM303D_COMPASS_SAMPLERATE_25       3
+#define LSM303D_COMPASS_SAMPLERATE_50       4
+#define LSM303D_COMPASS_SAMPLERATE_100      5
+
+//  Compass FSR
+
+#define LSM303D_COMPASS_FSR_2   0
+#define LSM303D_COMPASS_FSR_4   1
+#define LSM303D_COMPASS_FSR_8   2
+#define LSM303D_COMPASS_FSR_12  3
+
+class RTIMUGD20HM303D : public RTIMU
+{
+public:
+    RTIMUGD20HM303D(RTIMUSettings *settings);
+    ~RTIMUGD20HM303D();
+
+    virtual const char *IMUName() { return "L3GD20H + LSM303D"; }
+    virtual int IMUType() { return RTIMU_TYPE_GD20HM303D; }
+    virtual int IMUInit();
+    virtual int IMUGetPollInterval();
+    virtual bool IMURead();
+
+private:
+    bool setGyroSampleRate();
+    bool setGyroCTRL2();
+    bool setGyroCTRL4();
+    bool setGyroCTRL5();
+    bool setAccelCTRL1();
+    bool setAccelCTRL2();
+    bool setCompassCTRL5();
+    bool setCompassCTRL6();
+    bool setCompassCTRL7();
+
+    unsigned char m_gyroSlaveAddr;                          // I2C address of L3GD20H
+    unsigned char m_accelCompassSlaveAddr;                  // I2C address of LSM303D
+
+    RTFLOAT m_gyroScale;
+    RTFLOAT m_accelScale;
+    RTFLOAT m_compassScale;
+};
+
+#endif // _RTIMUGD20HM303D_H

osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.cpp

@@ -0,0 +1,409 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMUGD20HM303DLHC.h"
+#include "RTIMUSettings.h"
+
+#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
+
+RTIMUGD20HM303DLHC::RTIMUGD20HM303DLHC(RTIMUSettings *settings) : RTIMU(settings)
+{
+    m_sampleRate = 100;
+}
+
+RTIMUGD20HM303DLHC::~RTIMUGD20HM303DLHC()
+{
+}
+
+int RTIMUGD20HM303DLHC::IMUInit()
+{
+    unsigned char result;
+
+    //  configure IMU
+
+    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+    m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
+    m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
+
+    setCalibrationData();
+
+    //  Set up the gyro
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04))
+        return -1;
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80))
+        return -2;
+
+    if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result))
+        return -3;
+
+    if (result != L3GD20H_ID) {
+        return -4;
+    }
+
+    if (!setGyroSampleRate())
+            return -5;
+
+    if (!setGyroCTRL2())
+            return -6;
+
+    if (!setGyroCTRL4())
+            return -7;
+
+    //  Set up the accel
+
+    if (!setAccelCTRL1())
+        return -8;
+
+    if (!setAccelCTRL4())
+        return -9;
+
+    //  Set up the compass
+
+    if (!setCompassCRA())
+        return -10;
+
+    if (!setCompassCRB())
+        return -11;
+
+    if (!setCompassCRM())
+        return -12;
+
+    if (!setGyroCTRL5())
+            return -13;
+
+    gyroBiasInit();
+
+    return true;
+}
+
+bool RTIMUGD20HM303DLHC::setGyroSampleRate()
+{
+    unsigned char ctrl1;
+    unsigned char lowOdr = 0;
+
+    switch (m_settings->m_GD20HM303DLHCGyroSampleRate) {
+    case L3GD20H_SAMPLERATE_12_5:
+        ctrl1 = 0x0f;
+        lowOdr = 1;
+        m_sampleRate = 13;
+        break;
+
+    case L3GD20H_SAMPLERATE_25:
+        ctrl1 = 0x4f;
+        lowOdr = 1;
+        m_sampleRate = 25;
+        break;
+
+    case L3GD20H_SAMPLERATE_50:
+        ctrl1 = 0x8f;
+        lowOdr = 1;
+        m_sampleRate = 50;
+        break;
+
+    case L3GD20H_SAMPLERATE_100:
+        ctrl1 = 0x0f;
+        m_sampleRate = 100;
+        break;
+
+    case L3GD20H_SAMPLERATE_200:
+        ctrl1 = 0x4f;
+        m_sampleRate = 200;
+        break;
+
+    case L3GD20H_SAMPLERATE_400:
+        ctrl1 = 0x8f;
+        m_sampleRate = 400;
+        break;
+
+    case L3GD20H_SAMPLERATE_800:
+        ctrl1 = 0xcf;
+        m_sampleRate = 800;
+        break;
+
+    default:
+        return false;
+    }
+
+    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+    switch (m_settings->m_GD20HM303DLHCGyroBW) {
+    case L3GD20H_BANDWIDTH_0:
+        ctrl1 |= 0x00;
+        break;
+
+    case L3GD20H_BANDWIDTH_1:
+        ctrl1 |= 0x10;
+        break;
+
+    case L3GD20H_BANDWIDTH_2:
+        ctrl1 |= 0x20;
+        break;
+
+    case L3GD20H_BANDWIDTH_3:
+        ctrl1 |= 0x30;
+        break;
+
+    }
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, lowOdr))
+        return false;
+
+    return (I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL1, ctrl1));
+}
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL2()
+{
+    if ((m_settings->m_GD20HM303DLHCGyroHpf < L3GD20H_HPF_0) || (m_settings->m_GD20HM303DLHCGyroHpf > L3GD20H_HPF_9)) {
+        return false;
+    }
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20H_CTRL2, m_settings->m_GD20HM303DLHCGyroHpf);
+}
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL4()
+{
+    unsigned char ctrl4;
+
+    switch (m_settings->m_GD20HM303DLHCGyroFsr) {
+    case L3GD20H_FSR_245:
+        ctrl4 = 0x00;
+        m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case L3GD20H_FSR_500:
+        ctrl4 = 0x10;
+        m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case L3GD20H_FSR_2000:
+        ctrl4 = 0x20;
+        m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20H_CTRL4, ctrl4);
+}
+
+
+bool RTIMUGD20HM303DLHC::setGyroCTRL5()
+{
+    unsigned char ctrl5;
+
+    //  Turn on hpf
+
+    ctrl5 = 0x10;
+
+#ifdef GD20HM303DLHC_CACHE_MODE
+    //  turn on fifo
+
+    ctrl5 |= 0x40;
+#endif
+
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20H_CTRL5, ctrl5);
+}
+
+
+bool RTIMUGD20HM303DLHC::setAccelCTRL1()
+{
+    unsigned char ctrl1;
+
+    if ((m_settings->m_GD20HM303DLHCAccelSampleRate < 1) || (m_settings->m_GD20HM303DLHCAccelSampleRate > 7)) {
+        return false;
+    }
+
+    ctrl1 = (m_settings->m_GD20HM303DLHCAccelSampleRate << 4) | 0x07;
+
+    return I2CWrite(m_accelSlaveAddr,  LSM303DLHC_CTRL1_A, ctrl1);
+}
+
+bool RTIMUGD20HM303DLHC::setAccelCTRL4()
+{
+    unsigned char ctrl4;
+
+    switch (m_settings->m_GD20HM303DLHCAccelFsr) {
+    case LSM303DLHC_ACCEL_FSR_2:
+        m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)64;
+        break;
+
+    case LSM303DLHC_ACCEL_FSR_4:
+        m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)64;
+        break;
+
+    case LSM303DLHC_ACCEL_FSR_8:
+        m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)64;
+        break;
+
+    case LSM303DLHC_ACCEL_FSR_16:
+        m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)64;
+        break;
+
+    default:
+        return false;
+    }
+
+    ctrl4 = (m_settings->m_GD20HM303DLHCAccelFsr << 4);
+
+    return I2CWrite(m_accelSlaveAddr,  LSM303DLHC_CTRL2_A, ctrl4);
+}
+
+
+bool RTIMUGD20HM303DLHC::setCompassCRA()
+{
+    unsigned char cra;
+
+    if ((m_settings->m_GD20HM303DLHCCompassSampleRate < 0) || (m_settings->m_GD20HM303DLHCCompassSampleRate > 7)) {
+        return false;
+    }
+
+    cra = (m_settings->m_GD20HM303DLHCCompassSampleRate << 2);
+
+    return I2CWrite(m_compassSlaveAddr,  LSM303DLHC_CRA_M, cra);
+}
+
+bool RTIMUGD20HM303DLHC::setCompassCRB()
+{
+    unsigned char crb;
+
+    //  convert FSR to uT
+
+    switch (m_settings->m_GD20HM303DLHCCompassFsr) {
+    case LSM303DLHC_COMPASS_FSR_1_3:
+        crb = 0x20;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)1100;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)980;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_1_9:
+        crb = 0x40;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)855;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)760;
+       break;
+
+    case LSM303DLHC_COMPASS_FSR_2_5:
+        crb = 0x60;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)670;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)600;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_4:
+        crb = 0x80;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)450;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)400;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_4_7:
+        crb = 0xa0;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)400;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)355;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_5_6:
+        crb = 0xc0;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)330;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)295;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_8_1:
+        crb = 0xe0;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)230;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)205;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2CWrite(m_compassSlaveAddr,  LSM303DLHC_CRB_M, crb);
+}
+
+bool RTIMUGD20HM303DLHC::setCompassCRM()
+{
+     return I2CWrite(m_compassSlaveAddr,  LSM303DLHC_CRM_M, 0x00);
+}
+
+
+int RTIMUGD20HM303DLHC::IMUGetPollInterval()
+{
+    return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20HM303DLHC::IMURead()
+{
+    unsigned char status;
+    unsigned char gyroData[6];
+    unsigned char accelData[6];
+    unsigned char compassData[6];
+
+     if (!I2CRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status))
+        return false;
+
+    if ((status & 0x8) == 0)
+        return false;
+
+    if (!I2CRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData))
+        return false;
+
+    m_timestamp = millis();
+
+    if (!I2CRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData))
+        return false;
+
+    if (!I2CRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData))
+        return false;
+
+    RTMath::convertToVector(gyroData, m_gyro, m_gyroScale, false);
+    RTMath::convertToVector(accelData, m_accel, m_accelScale, false);
+
+    m_compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY);
+    m_compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY);
+    m_compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ);
+
+    //  sort out gyro axes
+
+    m_gyro.setY(-m_gyro.y());
+    m_gyro.setZ(-m_gyro.z());
+
+    //  sort out accel data;
+
+    m_accel.setX(-m_accel.x());
+
+    //  sort out compass axes
+
+    RTFLOAT temp;
+
+    temp = m_compass.z();
+    m_compass.setZ(-m_compass.y());
+    m_compass.setY(-temp);
+
+    //  now do standard processing
+
+    handleGyroBias();
+    calibrateAverageCompass();
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.h

@@ -0,0 +1,208 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMUGD20HM303DLHC_H
+#define	_RTIMUGD20HM303DLHC_H
+
+#include "RTIMU.h"
+
+//  I2C Slave Addresses
+
+#define L3GD20H_ADDRESS0        0x6a
+#define L3GD20H_ADDRESS1        0x6b
+#define L3GD20H_ID              0xd7
+
+#define LSM303DLHC_ACCEL_ADDRESS    0x19
+#define LSM303DLHC_COMPASS_ADDRESS  0x1e
+
+//  L3GD20H Register map
+
+#define L3GD20H_WHO_AM_I        0x0f
+#define L3GD20H_CTRL1           0x20
+#define L3GD20H_CTRL2           0x21
+#define L3GD20H_CTRL3           0x22
+#define L3GD20H_CTRL4           0x23
+#define L3GD20H_CTRL5           0x24
+#define L3GD20H_OUT_TEMP        0x26
+#define L3GD20H_STATUS          0x27
+#define L3GD20H_OUT_X_L         0x28
+#define L3GD20H_OUT_X_H         0x29
+#define L3GD20H_OUT_Y_L         0x2a
+#define L3GD20H_OUT_Y_H         0x2b
+#define L3GD20H_OUT_Z_L         0x2c
+#define L3GD20H_OUT_Z_H         0x2d
+#define L3GD20H_FIFO_CTRL       0x2e
+#define L3GD20H_FIFO_SRC        0x2f
+#define L3GD20H_IG_CFG          0x30
+#define L3GD20H_IG_SRC          0x31
+#define L3GD20H_IG_THS_XH       0x32
+#define L3GD20H_IG_THS_XL       0x33
+#define L3GD20H_IG_THS_YH       0x34
+#define L3GD20H_IG_THS_YL       0x35
+#define L3GD20H_IG_THS_ZH       0x36
+#define L3GD20H_IG_THS_ZL       0x37
+#define L3GD20H_IG_DURATION     0x38
+#define L3GD20H_LOW_ODR         0x39
+
+//  Gyro sample rate defines
+
+#define L3GD20H_SAMPLERATE_12_5 0
+#define L3GD20H_SAMPLERATE_25   1
+#define L3GD20H_SAMPLERATE_50   2
+#define L3GD20H_SAMPLERATE_100  3
+#define L3GD20H_SAMPLERATE_200  4
+#define L3GD20H_SAMPLERATE_400  5
+#define L3GD20H_SAMPLERATE_800  6
+
+//  Gyro banwidth defines
+
+#define L3GD20H_BANDWIDTH_0     0
+#define L3GD20H_BANDWIDTH_1     1
+#define L3GD20H_BANDWIDTH_2     2
+#define L3GD20H_BANDWIDTH_3     3
+
+//  Gyro FSR defines
+
+#define L3GD20H_FSR_245         0
+#define L3GD20H_FSR_500         1
+#define L3GD20H_FSR_2000        2
+
+//  Gyro high pass filter defines
+
+#define L3GD20H_HPF_0           0
+#define L3GD20H_HPF_1           1
+#define L3GD20H_HPF_2           2
+#define L3GD20H_HPF_3           3
+#define L3GD20H_HPF_4           4
+#define L3GD20H_HPF_5           5
+#define L3GD20H_HPF_6           6
+#define L3GD20H_HPF_7           7
+#define L3GD20H_HPF_8           8
+#define L3GD20H_HPF_9           9
+
+//  LSM303DLHC Accel Register Map
+
+#define LSM303DLHC_CTRL1_A         0x20
+#define LSM303DLHC_CTRL2_A         0x21
+#define LSM303DLHC_CTRL3_A         0x22
+#define LSM303DLHC_CTRL4_A         0x23
+#define LSM303DLHC_CTRL5_A         0x24
+#define LSM303DLHC_CTRL6_A         0x25
+#define LSM303DLHC_REF_A           0x26
+#define LSM303DLHC_STATUS_A        0x27
+#define LSM303DLHC_OUT_X_L_A       0x28
+#define LSM303DLHC_OUT_X_H_A       0x29
+#define LSM303DLHC_OUT_Y_L_A       0x2a
+#define LSM303DLHC_OUT_Y_H_A       0x2b
+#define LSM303DLHC_OUT_Z_L_A       0x2c
+#define LSM303DLHC_OUT_Z_H_A       0x2d
+#define LSM303DLHC_FIFO_CTRL_A     0x2e
+#define LSM303DLHC_FIFO_SRC_A      0x2f
+
+//  LSM303DLHC Compass Register Map
+
+#define LSM303DLHC_CRA_M            0x00
+#define LSM303DLHC_CRB_M            0x01
+#define LSM303DLHC_CRM_M            0x02
+#define LSM303DLHC_OUT_X_H_M        0x03
+#define LSM303DLHC_OUT_X_L_M        0x04
+#define LSM303DLHC_OUT_Y_H_M        0x05
+#define LSM303DLHC_OUT_Y_L_M        0x06
+#define LSM303DLHC_OUT_Z_H_M        0x07
+#define LSM303DLHC_OUT_Z_L_M        0x08
+#define LSM303DLHC_STATUS_M         0x09
+#define LSM303DLHC_TEMP_OUT_L_M     0x31
+#define LSM303DLHC_TEMP_OUT_H_M     0x32
+
+//  Accel sample rate defines
+
+#define LSM303DLHC_ACCEL_SAMPLERATE_1       1
+#define LSM303DLHC_ACCEL_SAMPLERATE_10      2
+#define LSM303DLHC_ACCEL_SAMPLERATE_25      3
+#define LSM303DLHC_ACCEL_SAMPLERATE_50      4
+#define LSM303DLHC_ACCEL_SAMPLERATE_100     5
+#define LSM303DLHC_ACCEL_SAMPLERATE_200     6
+#define LSM303DLHC_ACCEL_SAMPLERATE_400     7
+
+//  Accel FSR
+
+#define LSM303DLHC_ACCEL_FSR_2     0
+#define LSM303DLHC_ACCEL_FSR_4     1
+#define LSM303DLHC_ACCEL_FSR_8     2
+#define LSM303DLHC_ACCEL_FSR_16    3
+
+//  Compass sample rate defines
+
+#define LSM303DLHC_COMPASS_SAMPLERATE_0_75      0
+#define LSM303DLHC_COMPASS_SAMPLERATE_1_5       1
+#define LSM303DLHC_COMPASS_SAMPLERATE_3         2
+#define LSM303DLHC_COMPASS_SAMPLERATE_7_5       3
+#define LSM303DLHC_COMPASS_SAMPLERATE_15        4
+#define LSM303DLHC_COMPASS_SAMPLERATE_30        5
+#define LSM303DLHC_COMPASS_SAMPLERATE_75        6
+#define LSM303DLHC_COMPASS_SAMPLERATE_220       7
+
+//  Compass FSR
+
+#define LSM303DLHC_COMPASS_FSR_1_3      1
+#define LSM303DLHC_COMPASS_FSR_1_9      2
+#define LSM303DLHC_COMPASS_FSR_2_5      3
+#define LSM303DLHC_COMPASS_FSR_4        4
+#define LSM303DLHC_COMPASS_FSR_4_7      5
+#define LSM303DLHC_COMPASS_FSR_5_6      6
+#define LSM303DLHC_COMPASS_FSR_8_1      7
+
+class RTIMUGD20HM303DLHC : public RTIMU
+{
+public:
+    RTIMUGD20HM303DLHC(RTIMUSettings *settings);
+    ~RTIMUGD20HM303DLHC();
+
+    virtual const char *IMUName() { return "L3GD20H + LSM303DLHC"; }
+    virtual int IMUType() { return RTIMU_TYPE_GD20HM303DLHC; }
+    virtual int IMUInit();
+    virtual int IMUGetPollInterval();
+    virtual bool IMURead();
+
+private:
+    bool setGyroSampleRate();
+    bool setGyroCTRL2();
+    bool setGyroCTRL4();
+    bool setGyroCTRL5();
+    bool setAccelCTRL1();
+    bool setAccelCTRL4();
+    bool setCompassCRA();
+    bool setCompassCRB();
+    bool setCompassCRM();
+
+    unsigned char m_gyroSlaveAddr;                          // I2C address of L3GD20
+    unsigned char m_accelSlaveAddr;                         // I2C address of LSM303DLHC accel
+    unsigned char m_compassSlaveAddr;                       // I2C address of LSM303DLHC compass
+
+    RTFLOAT m_gyroScale;
+    RTFLOAT m_accelScale;
+    RTFLOAT m_compassScaleXY;
+    RTFLOAT m_compassScaleZ;
+};
+
+#endif // _RTIMUGD20HM303DLHC_H

osc32_9255/RTIMULib/RTIMUGD20M303DLHC.cpp

@@ -0,0 +1,382 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMUGD20M303DLHC.h"
+#include "RTIMUSettings.h"
+
+#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
+
+RTIMUGD20M303DLHC::RTIMUGD20M303DLHC(RTIMUSettings *settings) : RTIMU(settings)
+{
+    m_sampleRate = 100;
+}
+
+RTIMUGD20M303DLHC::~RTIMUGD20M303DLHC()
+{
+}
+
+int RTIMUGD20M303DLHC::IMUInit()
+{
+    unsigned char result;
+
+    //  configure IMU
+
+    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+    m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
+    m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
+
+    setCalibrationData();
+
+    //  Set up the gyro
+
+    if (!I2CWrite(m_gyroSlaveAddr, L3GD20_CTRL5, 0x80))
+        return -1;
+
+    if (!I2CRead(m_gyroSlaveAddr, L3GD20_WHO_AM_I, 1, &result))
+        return -2;
+
+    
+
+    if (!setGyroSampleRate())
+            return -4;
+
+    if (!setGyroCTRL2())
+            return -5;
+
+    if (!setGyroCTRL4())
+            return -6;
+
+    //  Set up the accel
+
+    if (!setAccelCTRL1())
+        return -7;
+
+    if (!setAccelCTRL4())
+        return -8;
+
+    //  Set up the compass
+
+    if (!setCompassCRA())
+        return -9;
+
+    if (!setCompassCRB())
+        return -10;
+
+    if (!setCompassCRM())
+        return -11;
+
+    if (!setGyroCTRL5())
+            return -12;
+
+    gyroBiasInit();
+
+    return true;
+}
+
+bool RTIMUGD20M303DLHC::setGyroSampleRate()
+{
+    unsigned char ctrl1;
+
+    switch (m_settings->m_GD20M303DLHCGyroSampleRate) {
+    case L3GD20_SAMPLERATE_95:
+        ctrl1 = 0x0f;
+        m_sampleRate = 95;
+        break;
+
+    case L3GD20_SAMPLERATE_190:
+        ctrl1 = 0x4f;
+        m_sampleRate = 190;
+        break;
+
+    case L3GD20_SAMPLERATE_380:
+        ctrl1 = 0x8f;
+        m_sampleRate = 380;
+        break;
+
+    case L3GD20_SAMPLERATE_760:
+        ctrl1 = 0xcf;
+        m_sampleRate = 760;
+        break;
+
+    default:
+        return false;
+    }
+
+    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+    switch (m_settings->m_GD20M303DLHCGyroBW) {
+    case L3GD20_BANDWIDTH_0:
+        ctrl1 |= 0x00;
+        break;
+
+    case L3GD20_BANDWIDTH_1:
+        ctrl1 |= 0x10;
+        break;
+
+    case L3GD20_BANDWIDTH_2:
+        ctrl1 |= 0x20;
+        break;
+
+    case L3GD20_BANDWIDTH_3:
+        ctrl1 |= 0x30;
+        break;
+
+    }
+
+    return (I2CWrite(m_gyroSlaveAddr, L3GD20_CTRL1, ctrl1));
+}
+
+bool RTIMUGD20M303DLHC::setGyroCTRL2()
+{
+    if ((m_settings->m_GD20M303DLHCGyroHpf < L3GD20_HPF_0) || (m_settings->m_GD20M303DLHCGyroHpf > L3GD20_HPF_9)) {
+        return false;
+    }
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20_CTRL2, m_settings->m_GD20M303DLHCGyroHpf);
+}
+
+bool RTIMUGD20M303DLHC::setGyroCTRL4()
+{
+    unsigned char ctrl4;
+
+    switch (m_settings->m_GD20M303DLHCGyroFsr) {
+    case L3GD20_FSR_250:
+        ctrl4 = 0x00;
+        m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case L3GD20_FSR_500:
+        ctrl4 = 0x10;
+        m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case L3GD20_FSR_2000:
+        ctrl4 = 0x20;
+        m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20_CTRL4, ctrl4);
+}
+
+
+bool RTIMUGD20M303DLHC::setGyroCTRL5()
+{
+    unsigned char ctrl5;
+
+    //  Turn on hpf
+
+    ctrl5 = 0x10;
+
+#ifdef GD20M303DLHC_CACHE_MODE
+    //  turn on fifo
+
+    ctrl5 |= 0x40;
+#endif
+
+    return I2CWrite(m_gyroSlaveAddr,  L3GD20_CTRL5, ctrl5);
+}
+
+
+bool RTIMUGD20M303DLHC::setAccelCTRL1()
+{
+    unsigned char ctrl1;
+
+    if ((m_settings->m_GD20M303DLHCAccelSampleRate < 1) || (m_settings->m_GD20M303DLHCAccelSampleRate > 7)) {
+        return false;
+    }
+
+    ctrl1 = (m_settings->m_GD20M303DLHCAccelSampleRate << 4) | 0x07;
+
+    return I2CWrite(m_accelSlaveAddr,  LSM303DLHC_CTRL1_A, ctrl1);
+}
+
+bool RTIMUGD20M303DLHC::setAccelCTRL4()
+{
+    unsigned char ctrl4;
+
+    switch (m_settings->m_GD20M303DLHCAccelFsr) {
+    case LSM303DLHC_ACCEL_FSR_2:
+        m_accelScale = (RTFLOAT)0.001 / (RTFLOAT)64;
+        break;
+
+    case LSM303DLHC_ACCEL_FSR_4:
+        m_accelScale = (RTFLOAT)0.002 / (RTFLOAT)64;
+        break;
+
+    case LSM303DLHC_ACCEL_FSR_8:
+        m_accelScale = (RTFLOAT)0.004 / (RTFLOAT)64;
+        break;
+
+    case LSM303DLHC_ACCEL_FSR_16:
+        m_accelScale = (RTFLOAT)0.012 / (RTFLOAT)64;
+        break;
+
+    default:
+        return false;
+    }
+
+    ctrl4 = (m_settings->m_GD20M303DLHCAccelFsr << 4);
+
+    return I2CWrite(m_accelSlaveAddr,  LSM303DLHC_CTRL2_A, ctrl4);
+}
+
+
+bool RTIMUGD20M303DLHC::setCompassCRA()
+{
+    unsigned char cra;
+
+    if ((m_settings->m_GD20M303DLHCCompassSampleRate < 0) || (m_settings->m_GD20M303DLHCCompassSampleRate > 7)) {
+        return false;
+    }
+
+    cra = (m_settings->m_GD20M303DLHCCompassSampleRate << 2);
+
+    return I2CWrite(m_compassSlaveAddr,  LSM303DLHC_CRA_M, cra);
+}
+
+bool RTIMUGD20M303DLHC::setCompassCRB()
+{
+    unsigned char crb;
+
+    //  convert FSR to uT
+
+    switch (m_settings->m_GD20M303DLHCCompassFsr) {
+    case LSM303DLHC_COMPASS_FSR_1_3:
+        crb = 0x20;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)1100;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)980;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_1_9:
+        crb = 0x40;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)855;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)760;
+       break;
+
+    case LSM303DLHC_COMPASS_FSR_2_5:
+        crb = 0x60;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)670;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)600;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_4:
+        crb = 0x80;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)450;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)400;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_4_7:
+        crb = 0xa0;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)400;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)355;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_5_6:
+        crb = 0xc0;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)330;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)295;
+        break;
+
+    case LSM303DLHC_COMPASS_FSR_8_1:
+        crb = 0xe0;
+        m_compassScaleXY = (RTFLOAT)100 / (RTFLOAT)230;
+        m_compassScaleZ = (RTFLOAT)100 / (RTFLOAT)205;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2CWrite(m_compassSlaveAddr,  LSM303DLHC_CRB_M, crb);
+}
+
+bool RTIMUGD20M303DLHC::setCompassCRM()
+{
+     return I2CWrite(m_compassSlaveAddr,  LSM303DLHC_CRM_M, 0x00);
+}
+
+
+int RTIMUGD20M303DLHC::IMUGetPollInterval()
+{
+    return (400 / m_sampleRate);
+}
+
+bool RTIMUGD20M303DLHC::IMURead()
+{
+    unsigned char status;
+    unsigned char gyroData[6];
+    unsigned char accelData[6];
+    unsigned char compassData[6];
+
+    if (!I2CRead(m_gyroSlaveAddr, L3GD20_STATUS, 1, &status))
+        return false;
+
+    if ((status & 0x8) == 0)
+        return false;
+
+    if (!I2CRead(m_gyroSlaveAddr, 0x80 | L3GD20_OUT_X_L, 6, gyroData))
+        return false;
+
+    m_timestamp = millis();
+
+    if (!I2CRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData))
+        return false;
+
+    if (!I2CRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData))
+        return false;
+
+    RTMath::convertToVector(gyroData, m_gyro, m_gyroScale, false);
+    RTMath::convertToVector(accelData, m_accel, m_accelScale, false);
+
+    m_compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY);
+    m_compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY);
+    m_compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ);
+
+    //  sort out gyro axes
+
+    m_gyro.setY(-m_gyro.y());
+    m_gyro.setZ(-m_gyro.z());
+
+    //  sort out accel data;
+
+    m_accel.setX(-m_accel.x());
+
+    //  sort out compass axes
+
+    RTFLOAT temp;
+
+    temp = m_compass.z();
+    m_compass.setZ(-m_compass.y());
+    m_compass.setY(-temp);
+
+    //  now do standard processing
+
+    handleGyroBias();
+    calibrateAverageCompass();
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMUGD20M303DLHC.h

@@ -0,0 +1,204 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMUGD20M303DLHC_H
+#define	_RTIMUGD20M303DLHC_H
+
+#include "RTIMU.h"
+
+//  I2C Slave Addresses
+// cambiado a 0x69
+#define L3GD20_ADDRESS0             0x69
+#define L3GD20_ADDRESS1             0x69
+#define L3GD20_ID                   0xd4
+
+#define LSM303DLHC_ACCEL_ADDRESS    0x19
+#define LSM303DLHC_COMPASS_ADDRESS  0x1e
+
+//  L3GD20 Register map
+
+#define L3GD20_WHO_AM_I        0x0f
+#define L3GD20_CTRL1           0x20
+#define L3GD20_CTRL2           0x21
+#define L3GD20_CTRL3           0x22
+#define L3GD20_CTRL4           0x23
+#define L3GD20_CTRL5           0x24
+#define L3GD20_OUT_TEMP        0x26
+#define L3GD20_STATUS          0x27
+#define L3GD20_OUT_X_L         0x28
+#define L3GD20_OUT_X_H         0x29
+#define L3GD20_OUT_Y_L         0x2a
+#define L3GD20_OUT_Y_H         0x2b
+#define L3GD20_OUT_Z_L         0x2c
+#define L3GD20_OUT_Z_H         0x2d
+#define L3GD20_FIFO_CTRL       0x2e
+#define L3GD20_FIFO_SRC        0x2f
+#define L3GD20_IG_CFG          0x30
+#define L3GD20_IG_SRC          0x31
+#define L3GD20_IG_THS_XH       0x32
+#define L3GD20_IG_THS_XL       0x33
+#define L3GD20_IG_THS_YH       0x34
+#define L3GD20_IG_THS_YL       0x35
+#define L3GD20_IG_THS_ZH       0x36
+#define L3GD20_IG_THS_ZL       0x37
+#define L3GD20_IG_DURATION     0x38
+
+//  Gyro sample rate defines
+
+#define L3GD20_SAMPLERATE_95    0
+#define L3GD20_SAMPLERATE_190   1
+#define L3GD20_SAMPLERATE_380   2
+#define L3GD20_SAMPLERATE_760   3
+
+//  Gyro banwidth defines
+
+#define L3GD20_BANDWIDTH_0     0
+#define L3GD20_BANDWIDTH_1     1
+#define L3GD20_BANDWIDTH_2     2
+#define L3GD20_BANDWIDTH_3     3
+
+//  Gyro FSR defines
+
+#define L3GD20_FSR_250         0
+#define L3GD20_FSR_500         1
+#define L3GD20_FSR_2000        2
+
+//  Gyro high pass filter defines
+
+#define L3GD20_HPF_0           0
+#define L3GD20_HPF_1           1
+#define L3GD20_HPF_2           2
+#define L3GD20_HPF_3           3
+#define L3GD20_HPF_4           4
+#define L3GD20_HPF_5           5
+#define L3GD20_HPF_6           6
+#define L3GD20_HPF_7           7
+#define L3GD20_HPF_8           8
+#define L3GD20_HPF_9           9
+
+//  LSM303DLHC Accel Register Map
+
+#define LSM303DLHC_CTRL1_A         0x20
+#define LSM303DLHC_CTRL2_A         0x21
+#define LSM303DLHC_CTRL3_A         0x22
+#define LSM303DLHC_CTRL4_A         0x23
+#define LSM303DLHC_CTRL5_A         0x24
+#define LSM303DLHC_CTRL6_A         0x25
+#define LSM303DLHC_REF_A           0x26
+#define LSM303DLHC_STATUS_A        0x27
+#define LSM303DLHC_OUT_X_L_A       0x28
+#define LSM303DLHC_OUT_X_H_A       0x29
+#define LSM303DLHC_OUT_Y_L_A       0x2a
+#define LSM303DLHC_OUT_Y_H_A       0x2b
+#define LSM303DLHC_OUT_Z_L_A       0x2c
+#define LSM303DLHC_OUT_Z_H_A       0x2d
+#define LSM303DLHC_FIFO_CTRL_A     0x2e
+#define LSM303DLHC_FIFO_SRC_A      0x2f
+
+//  LSM303DLHC Compass Register Map
+
+#define LSM303DLHC_CRA_M            0x00
+#define LSM303DLHC_CRB_M            0x01
+#define LSM303DLHC_CRM_M            0x02
+#define LSM303DLHC_OUT_X_H_M        0x03
+#define LSM303DLHC_OUT_X_L_M        0x04
+#define LSM303DLHC_OUT_Y_H_M        0x05
+#define LSM303DLHC_OUT_Y_L_M        0x06
+#define LSM303DLHC_OUT_Z_H_M        0x07
+#define LSM303DLHC_OUT_Z_L_M        0x08
+#define LSM303DLHC_STATUS_M         0x09
+#define LSM303DLHC_TEMP_OUT_L_M     0x31
+#define LSM303DLHC_TEMP_OUT_H_M     0x32
+
+//  Accel sample rate defines
+
+#define LSM303DLHC_ACCEL_SAMPLERATE_1       1
+#define LSM303DLHC_ACCEL_SAMPLERATE_10      2
+#define LSM303DLHC_ACCEL_SAMPLERATE_25      3
+#define LSM303DLHC_ACCEL_SAMPLERATE_50      4
+#define LSM303DLHC_ACCEL_SAMPLERATE_100     5
+#define LSM303DLHC_ACCEL_SAMPLERATE_200     6
+#define LSM303DLHC_ACCEL_SAMPLERATE_400     7
+
+//  Accel FSR
+
+#define LSM303DLHC_ACCEL_FSR_2     0
+#define LSM303DLHC_ACCEL_FSR_4     1
+#define LSM303DLHC_ACCEL_FSR_8     2
+#define LSM303DLHC_ACCEL_FSR_16    3
+
+//  Compass sample rate defines
+
+#define LSM303DLHC_COMPASS_SAMPLERATE_0_75      0
+#define LSM303DLHC_COMPASS_SAMPLERATE_1_5       1
+#define LSM303DLHC_COMPASS_SAMPLERATE_3         2
+#define LSM303DLHC_COMPASS_SAMPLERATE_7_5       3
+#define LSM303DLHC_COMPASS_SAMPLERATE_15        4
+#define LSM303DLHC_COMPASS_SAMPLERATE_30        5
+#define LSM303DLHC_COMPASS_SAMPLERATE_75        6
+#define LSM303DLHC_COMPASS_SAMPLERATE_220       7
+
+//  Compass FSR
+
+#define LSM303DLHC_COMPASS_FSR_1_3      1
+#define LSM303DLHC_COMPASS_FSR_1_9      2
+#define LSM303DLHC_COMPASS_FSR_2_5      3
+#define LSM303DLHC_COMPASS_FSR_4        4
+#define LSM303DLHC_COMPASS_FSR_4_7      5
+#define LSM303DLHC_COMPASS_FSR_5_6      6
+#define LSM303DLHC_COMPASS_FSR_8_1      7
+
+class RTIMUGD20M303DLHC : public RTIMU
+{
+public:
+    RTIMUGD20M303DLHC(RTIMUSettings *settings);
+    ~RTIMUGD20M303DLHC();
+
+    virtual const char *IMUName() { return "L3GD20 + LSM303DLHC"; }
+    virtual int IMUType() { return RTIMU_TYPE_GD20M303DLHC; }
+    virtual int IMUInit();
+    virtual int IMUGetPollInterval();
+    virtual bool IMURead();
+
+private:
+    bool setGyroSampleRate();
+    bool setGyroCTRL2();
+    bool setGyroCTRL4();
+    bool setGyroCTRL5();
+    bool setAccelCTRL1();
+    bool setAccelCTRL4();
+    bool setCompassCRA();
+    bool setCompassCRB();
+    bool setCompassCRM();
+
+    unsigned char m_gyroSlaveAddr;                          // I2C address of L3GD20
+    unsigned char m_accelSlaveAddr;                         // I2C address of LSM303DLHC accel
+    unsigned char m_compassSlaveAddr;                       // I2C address of LSM303DLHC compass
+
+    RTFLOAT m_gyroScale;
+    RTFLOAT m_accelScale;
+    RTFLOAT m_compassScaleXY;
+    RTFLOAT m_compassScaleZ;
+};
+
+#endif // _RTIMUGD20M303DLHC_H

osc32_9255/RTIMULib/RTIMULSM9DS0.cpp

@@ -0,0 +1,363 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMULSM9DS0.h"
+#include "RTIMUSettings.h"
+
+#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
+
+
+RTIMULSM9DS0::RTIMULSM9DS0(RTIMUSettings *settings) : RTIMU(settings)
+{
+    m_sampleRate = 100;
+}
+
+RTIMULSM9DS0::~RTIMULSM9DS0()
+{
+}
+
+int RTIMULSM9DS0::IMUInit()
+{
+    unsigned char result;
+
+    //  configure IMU
+
+    m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
+    if (m_gyroSlaveAddr == LSM9DS0_GYRO_ADDRESS0)
+        m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0;
+    else
+        m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1;
+
+    setCalibrationData();
+
+    //  Set up the gyro
+
+    if (!I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80))
+        return -1;
+
+    if (!I2Cdev::readByte(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, &result))
+        return -2;
+
+    if (result != LSM9DS0_GYRO_ID) {
+        return -3;
+    }
+
+    if (!setGyroSampleRate())
+            return -4;
+
+    if (!setGyroCTRL2())
+            return -5;
+
+    if (!setGyroCTRL4())
+            return -6;
+
+    //  Set up the accel
+
+    if (!I2Cdev::readByte(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, &result))
+        return -7;
+
+    if (result != LSM9DS0_ACCELMAG_ID) {
+        return -8;
+    }
+
+    if (!setAccelCTRL1())
+        return 9;
+
+    if (!setAccelCTRL2())
+        return -10;
+
+    if (!setCompassCTRL5())
+        return 11;
+
+    if (!setCompassCTRL6())
+        return -12;
+
+    if (!setCompassCTRL7())
+        return -13;
+
+    if (!setGyroCTRL5())
+            return -14;
+
+    gyroBiasInit();
+    return 1;
+}
+
+bool RTIMULSM9DS0::setGyroSampleRate()
+{
+    unsigned char ctrl1;
+
+    switch (m_settings->m_LSM9DS0GyroSampleRate) {
+    case LSM9DS0_GYRO_SAMPLERATE_95:
+        ctrl1 = 0x0f;
+        m_sampleRate = 95;
+        break;
+
+    case LSM9DS0_GYRO_SAMPLERATE_190:
+        ctrl1 = 0x4f;
+        m_sampleRate = 190;
+        break;
+
+    case LSM9DS0_GYRO_SAMPLERATE_380:
+        ctrl1 = 0x8f;
+        m_sampleRate = 380;
+        break;
+
+    case LSM9DS0_GYRO_SAMPLERATE_760:
+        ctrl1 = 0xcf;
+        m_sampleRate = 760;
+        break;
+
+    default:
+         return false;
+    }
+
+    m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
+
+    switch (m_settings->m_LSM9DS0GyroBW) {
+    case LSM9DS0_GYRO_BANDWIDTH_0:
+        ctrl1 |= 0x00;
+        break;
+
+    case LSM9DS0_GYRO_BANDWIDTH_1:
+        ctrl1 |= 0x10;
+        break;
+
+    case LSM9DS0_GYRO_BANDWIDTH_2:
+        ctrl1 |= 0x20;
+        break;
+
+    case LSM9DS0_GYRO_BANDWIDTH_3:
+        ctrl1 |= 0x30;
+        break;
+
+    }
+
+    return (I2Cdev::writeByte(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL1, ctrl1));
+}
+
+bool RTIMULSM9DS0::setGyroCTRL2()
+{
+    if ((m_settings->m_LSM9DS0GyroHpf < LSM9DS0_GYRO_HPF_0) || (m_settings->m_LSM9DS0GyroHpf > LSM9DS0_GYRO_HPF_9)) {
+        return false;
+    }
+    return I2Cdev::writeByte(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL2, m_settings->m_LSM9DS0GyroHpf);
+}
+
+bool RTIMULSM9DS0::setGyroCTRL4()
+{
+    unsigned char ctrl4;
+
+    switch (m_settings->m_LSM9DS0GyroFsr) {
+    case LSM9DS0_GYRO_FSR_250:
+        ctrl4 = 0x00;
+        m_gyroScale = (RTFLOAT)0.00875 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case LSM9DS0_GYRO_FSR_500:
+        ctrl4 = 0x10;
+        m_gyroScale = (RTFLOAT)0.0175 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    case LSM9DS0_GYRO_FSR_2000:
+        ctrl4 = 0x20;
+        m_gyroScale = (RTFLOAT)0.07 * RTMATH_DEGREE_TO_RAD;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2Cdev::writeByte(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL4, ctrl4);
+}
+
+
+bool RTIMULSM9DS0::setGyroCTRL5()
+{
+    unsigned char ctrl5;
+
+    //  Turn on hpf
+
+    ctrl5 = 0x10;
+
+    return I2Cdev::writeByte(m_gyroSlaveAddr,  LSM9DS0_GYRO_CTRL5, ctrl5);
+}
+
+
+bool RTIMULSM9DS0::setAccelCTRL1()
+{
+    unsigned char ctrl1;
+
+    if ((m_settings->m_LSM9DS0AccelSampleRate < 0) || (m_settings->m_LSM9DS0AccelSampleRate > 10)) {
+        return false;
+    }
+
+    ctrl1 = (m_settings->m_LSM9DS0AccelSampleRate << 4) | 0x07;
+
+    return I2Cdev::writeByte(m_accelCompassSlaveAddr,  LSM9DS0_CTRL1, ctrl1);
+}
+
+bool RTIMULSM9DS0::setAccelCTRL2()
+{
+    unsigned char ctrl2;
+
+    if ((m_settings->m_LSM9DS0AccelLpf < 0) || (m_settings->m_LSM9DS0AccelLpf > 3)) {
+        return false;
+    }
+
+    switch (m_settings->m_LSM9DS0AccelFsr) {
+    case LSM9DS0_ACCEL_FSR_2:
+        m_accelScale = (RTFLOAT)0.000061;
+        break;
+
+    case LSM9DS0_ACCEL_FSR_4:
+        m_accelScale = (RTFLOAT)0.000122;
+        break;
+
+    case LSM9DS0_ACCEL_FSR_6:
+        m_accelScale = (RTFLOAT)0.000183;
+        break;
+
+    case LSM9DS0_ACCEL_FSR_8:
+        m_accelScale = (RTFLOAT)0.000244;
+        break;
+
+    case LSM9DS0_ACCEL_FSR_16:
+        m_accelScale = (RTFLOAT)0.000732;
+        break;
+
+    default:
+        return false;
+    }
+
+    ctrl2 = (m_settings->m_LSM9DS0AccelLpf << 6) | (m_settings->m_LSM9DS0AccelFsr << 3);
+
+    return I2Cdev::writeByte(m_accelCompassSlaveAddr,  LSM9DS0_CTRL2, ctrl2);
+}
+
+
+bool RTIMULSM9DS0::setCompassCTRL5()
+{
+    unsigned char ctrl5;
+
+    if ((m_settings->m_LSM9DS0CompassSampleRate < 0) || (m_settings->m_LSM9DS0CompassSampleRate > 5)) {
+        return false;
+    }
+
+    ctrl5 = (m_settings->m_LSM9DS0CompassSampleRate << 2);
+
+    return I2Cdev::writeByte(m_accelCompassSlaveAddr,  LSM9DS0_CTRL5, ctrl5);
+}
+
+bool RTIMULSM9DS0::setCompassCTRL6()
+{
+    unsigned char ctrl6;
+
+    //  convert FSR to uT
+
+    switch (m_settings->m_LSM9DS0CompassFsr) {
+    case LSM9DS0_COMPASS_FSR_2:
+        ctrl6 = 0;
+        m_compassScale = (RTFLOAT)0.008;
+        break;
+
+    case LSM9DS0_COMPASS_FSR_4:
+        ctrl6 = 0x20;
+        m_compassScale = (RTFLOAT)0.016;
+        break;
+
+    case LSM9DS0_COMPASS_FSR_8:
+        ctrl6 = 0x40;
+        m_compassScale = (RTFLOAT)0.032;
+        break;
+
+    case LSM9DS0_COMPASS_FSR_12:
+        ctrl6 = 0x60;
+        m_compassScale = (RTFLOAT)0.0479;
+        break;
+
+    default:
+        return false;
+    }
+
+    return I2Cdev::writeByte(m_accelCompassSlaveAddr,  LSM9DS0_CTRL6, ctrl6);
+}
+
+bool RTIMULSM9DS0::setCompassCTRL7()
+{
+     return I2Cdev::writeByte(m_accelCompassSlaveAddr,  LSM9DS0_CTRL7, 0x60);
+}
+
+int RTIMULSM9DS0::IMUGetPollInterval()
+{
+    return (400 / m_sampleRate);
+}
+
+bool RTIMULSM9DS0::IMURead()
+{
+    unsigned char status;
+    unsigned char gyroData[6];
+    unsigned char accelData[6];
+    unsigned char compassData[6];
+
+    if (!I2Cdev::readByte(m_gyroSlaveAddr, LSM9DS0_GYRO_STATUS, &status))
+        return false;
+
+    if ((status & 0x8) == 0)
+        return false;
+
+    if (!I2Cdev::readBytes(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData))
+        return false;
+
+    m_timestamp = millis();
+
+    if (!I2Cdev::readBytes(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData))
+        return false;
+
+    if (!I2Cdev::readBytes(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData))
+        return false;
+
+    RTMath::convertToVector(gyroData, m_gyro, m_gyroScale, false);
+    RTMath::convertToVector(accelData, m_accel, m_accelScale, false);
+    RTMath::convertToVector(compassData, m_compass, m_compassScale, false);
+
+    //  sort out gyro axes
+
+    m_gyro.setY(-m_gyro.y());
+    m_gyro.setZ(-m_gyro.z());
+
+    //  sort out accel data;
+
+    m_accel.setX(-m_accel.x());
+
+    //  sort out compass axes
+
+    m_compass.setY(-m_compass.y());
+
+    //  now do standard processing
+
+    handleGyroBias();
+    calibrateAverageCompass();
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMULSM9DS0.h

@@ -0,0 +1,246 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMULSM9DS0_H
+#define	_RTIMULSM9DS0_H
+
+#include "RTIMU.h"
+
+//  I2C Slave Addresses
+
+#define LSM9DS0_GYRO_ADDRESS0       0x6a
+#define LSM9DS0_GYRO_ADDRESS1       0x6b
+#define LSM9DS0_GYRO_ID             0xd4
+
+#define LSM9DS0_ACCELMAG_ADDRESS0   0x1e
+#define LSM9DS0_ACCELMAG_ADDRESS1   0x1d
+#define LSM9DS0_ACCELMAG_ID         0x49
+
+//  L3GD20 Register map
+
+#define LSM9DS0_GYRO_WHO_AM_I       0x0f
+#define LSM9DS0_GYRO_CTRL1          0x20
+#define LSM9DS0_GYRO_CTRL2          0x21
+#define LSM9DS0_GYRO_CTRL3          0x22
+#define LSM9DS0_GYRO_CTRL4          0x23
+#define LSM9DS0_GYRO_CTRL5          0x24
+#define LSM9DS0_GYRO_OUT_TEMP       0x26
+#define LSM9DS0_GYRO_STATUS         0x27
+#define LSM9DS0_GYRO_OUT_X_L        0x28
+#define LSM9DS0_GYRO_OUT_X_H        0x29
+#define LSM9DS0_GYRO_OUT_Y_L        0x2a
+#define LSM9DS0_GYRO_OUT_Y_H        0x2b
+#define LSM9DS0_GYRO_OUT_Z_L        0x2c
+#define LSM9DS0_GYRO_OUT_Z_H        0x2d
+#define LSM9DS0_GYRO_FIFO_CTRL      0x2e
+#define LSM9DS0_GYRO_FIFO_SRC       0x2f
+#define LSM9DS0_GYRO_IG_CFG         0x30
+#define LSM9DS0_GYRO_IG_SRC         0x31
+#define LSM9DS0_GYRO_IG_THS_XH      0x32
+#define LSM9DS0_GYRO_IG_THS_XL      0x33
+#define LSM9DS0_GYRO_IG_THS_YH      0x34
+#define LSM9DS0_GYRO_IG_THS_YL      0x35
+#define LSM9DS0_GYRO_IG_THS_ZH      0x36
+#define LSM9DS0_GYRO_IG_THS_ZL      0x37
+#define LSM9DS0_GYRO_IG_DURATION    0x38
+
+//  Gyro sample rate defines
+
+#define LSM9DS0_GYRO_SAMPLERATE_95  0
+#define LSM9DS0_GYRO_SAMPLERATE_190 1
+#define LSM9DS0_GYRO_SAMPLERATE_380 2
+#define LSM9DS0_GYRO_SAMPLERATE_760 3
+
+//  Gyro banwidth defines
+
+#define LSM9DS0_GYRO_BANDWIDTH_0    0
+#define LSM9DS0_GYRO_BANDWIDTH_1    1
+#define LSM9DS0_GYRO_BANDWIDTH_2    2
+#define LSM9DS0_GYRO_BANDWIDTH_3    3
+
+//  Gyro FSR defines
+
+#define LSM9DS0_GYRO_FSR_250        0
+#define LSM9DS0_GYRO_FSR_500        1
+#define LSM9DS0_GYRO_FSR_2000       2
+
+//  Gyro high pass filter defines
+
+#define LSM9DS0_GYRO_HPF_0          0
+#define LSM9DS0_GYRO_HPF_1          1
+#define LSM9DS0_GYRO_HPF_2          2
+#define LSM9DS0_GYRO_HPF_3          3
+#define LSM9DS0_GYRO_HPF_4          4
+#define LSM9DS0_GYRO_HPF_5          5
+#define LSM9DS0_GYRO_HPF_6          6
+#define LSM9DS0_GYRO_HPF_7          7
+#define LSM9DS0_GYRO_HPF_8          8
+#define LSM9DS0_GYRO_HPF_9          9
+
+//  Accel/Mag Register Map
+
+#define LSM9DS0_TEMP_OUT_L      0x05
+#define LSM9DS0_TEMP_OUT_H      0x06
+#define LSM9DS0_STATUS_M        0x07
+#define LSM9DS0_OUT_X_L_M       0x08
+#define LSM9DS0_OUT_X_H_M       0x09
+#define LSM9DS0_OUT_Y_L_M       0x0a
+#define LSM9DS0_OUT_Y_H_M       0x0b
+#define LSM9DS0_OUT_Z_L_M       0x0c
+#define LSM9DS0_OUT_Z_H_M       0x0d
+#define LSM9DS0_WHO_AM_I        0x0f
+#define LSM9DS0_INT_CTRL_M      0x12
+#define LSM9DS0_INT_SRC_M       0x13
+#define LSM9DS0_INT_THS_L_M     0x14
+#define LSM9DS0_INT_THS_H_M     0x15
+#define LSM9DS0_OFFSET_X_L_M    0x16
+#define LSM9DS0_OFFSET_X_H_M    0x17
+#define LSM9DS0_OFFSET_Y_L_M    0x18
+#define LSM9DS0_OFFSET_Y_H_M    0x19
+#define LSM9DS0_OFFSET_Z_L_M    0x1a
+#define LSM9DS0_OFFSET_Z_H_M    0x1b
+#define LSM9DS0_REFERENCE_X     0x1c
+#define LSM9DS0_REFERENCE_Y     0x1d
+#define LSM9DS0_REFERENCE_Z     0x1e
+#define LSM9DS0_CTRL0           0x1f
+#define LSM9DS0_CTRL1           0x20
+#define LSM9DS0_CTRL2           0x21
+#define LSM9DS0_CTRL3           0x22
+#define LSM9DS0_CTRL4           0x23
+#define LSM9DS0_CTRL5           0x24
+#define LSM9DS0_CTRL6           0x25
+#define LSM9DS0_CTRL7           0x26
+#define LSM9DS0_STATUS_A        0x27
+#define LSM9DS0_OUT_X_L_A       0x28
+#define LSM9DS0_OUT_X_H_A       0x29
+#define LSM9DS0_OUT_Y_L_A       0x2a
+#define LSM9DS0_OUT_Y_H_A       0x2b
+#define LSM9DS0_OUT_Z_L_A       0x2c
+#define LSM9DS0_OUT_Z_H_A       0x2d
+#define LSM9DS0_FIFO_CTRL       0x2e
+#define LSM9DS0_FIFO_SRC        0x2f
+#define LSM9DS0_IG_CFG1         0x30
+#define LSM9DS0_IG_SRC1         0x31
+#define LSM9DS0_IG_THS1         0x32
+#define LSM9DS0_IG_DUR1         0x33
+#define LSM9DS0_IG_CFG2         0x34
+#define LSM9DS0_IG_SRC2         0x35
+#define LSM9DS0_IG_THS2         0x36
+#define LSM9DS0_IG_DUR2         0x37
+#define LSM9DS0_CLICK_CFG       0x38
+#define LSM9DS0_CLICK_SRC       0x39
+#define LSM9DS0_CLICK_THS       0x3a
+#define LSM9DS0_TIME_LIMIT      0x3b
+#define LSM9DS0_TIME_LATENCY    0x3c
+#define LSM9DS0_TIME_WINDOW     0x3d
+#define LSM9DS0_ACT_THIS        0x3e
+#define LSM9DS0_ACT_DUR         0x3f
+
+//  Accel sample rate defines
+
+#define LSM9DS0_ACCEL_SAMPLERATE_3_125 1
+#define LSM9DS0_ACCEL_SAMPLERATE_6_25 2
+#define LSM9DS0_ACCEL_SAMPLERATE_12_5 3
+#define LSM9DS0_ACCEL_SAMPLERATE_25   4
+#define LSM9DS0_ACCEL_SAMPLERATE_50   5
+#define LSM9DS0_ACCEL_SAMPLERATE_100  6
+#define LSM9DS0_ACCEL_SAMPLERATE_200  7
+#define LSM9DS0_ACCEL_SAMPLERATE_400  8
+#define LSM9DS0_ACCEL_SAMPLERATE_800  9
+#define LSM9DS0_ACCEL_SAMPLERATE_1600 10
+
+//  Accel FSR
+
+#define LSM9DS0_ACCEL_FSR_2     0
+#define LSM9DS0_ACCEL_FSR_4     1
+#define LSM9DS0_ACCEL_FSR_6     2
+#define LSM9DS0_ACCEL_FSR_8     3
+#define LSM9DS0_ACCEL_FSR_16    4
+
+//  Accel filter bandwidth
+
+#define LSM9DS0_ACCEL_LPF_773   0
+#define LSM9DS0_ACCEL_LPF_194   1
+#define LSM9DS0_ACCEL_LPF_362   2
+#define LSM9DS0_ACCEL_LPF_50    3
+
+//  Compass sample rate defines
+
+#define LSM9DS0_COMPASS_SAMPLERATE_3_125    0
+#define LSM9DS0_COMPASS_SAMPLERATE_6_25     1
+#define LSM9DS0_COMPASS_SAMPLERATE_12_5     2
+#define LSM9DS0_COMPASS_SAMPLERATE_25       3
+#define LSM9DS0_COMPASS_SAMPLERATE_50       4
+#define LSM9DS0_COMPASS_SAMPLERATE_100      5
+
+//  Compass FSR
+
+#define LSM9DS0_COMPASS_FSR_2   0
+#define LSM9DS0_COMPASS_FSR_4   1
+#define LSM9DS0_COMPASS_FSR_8   2
+#define LSM9DS0_COMPASS_FSR_12  3
+
+
+class RTIMULSM9DS0 : public RTIMU
+{
+public:
+    RTIMULSM9DS0(RTIMUSettings *settings);
+    ~RTIMULSM9DS0();
+
+    virtual const char *IMUName() { return "LSM9DS0"; }
+    virtual int IMUType() { return RTIMU_TYPE_LSM9DS0; }
+    virtual int IMUInit();
+    virtual int IMUGetPollInterval();
+    virtual bool IMURead();
+ 
+private:
+    bool setGyroSampleRate();
+    bool setGyroCTRL2();
+    bool setGyroCTRL4();
+    bool setGyroCTRL5();
+    bool setAccelCTRL1();
+    bool setAccelCTRL2();
+    bool setCompassCTRL5();
+    bool setCompassCTRL6();
+    bool setCompassCTRL7();
+
+    unsigned char m_gyroSlaveAddr;                          // I2C address of gyro
+    unsigned char m_accelCompassSlaveAddr;                  // I2C address of accel and mag
+    unsigned char m_bus;                                    // I2C bus (usually 1 for Raspberry Pi for example)
+
+    RTFLOAT m_gyroScale;
+    RTFLOAT m_accelScale;
+    RTFLOAT m_compassScale;
+
+#ifdef LSM9DS0_CACHE_MODE
+    bool m_firstTime;                                       // if first sample
+
+    LSM9DS0_CACHE_BLOCK m_cache[LSM9DS0_CACHE_BLOCK_COUNT]; // the cache itself
+    int m_cacheIn;                                          // the in index
+    int m_cacheOut;                                         // the out index
+    int m_cacheCount;                                       // number of used cache blocks
+
+#endif
+};
+
+#endif // _RTIMULSM9DS0_H

osc32_9255/RTIMULib/RTIMULibDefs.h

@@ -0,0 +1,103 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMULIBDEFS_H
+#define	_RTIMULIBDEFS_H
+
+#include "RTMath.h"
+#include "RTPressureDefs.h"
+
+//  IMU enable defs - only one should be enabled, the rest commented out
+
+//#define MPU9150_68                      // MPU9150 at address 0x68
+//#define MPU9250_68                      // MPU9250 at address 0x68
+#define GD20M303DLHC_6b                 // GD20 + M303DLHC at address 0x6b
+
+//#define MPU9150_68                      // MPU9150 at address 0x68
+//#define MPU9150_69                      // MPU9150 at address 0x69
+//#define MPU9250_68                      // MPU9250 at address 0x68
+//#define MPU9250_69                      // MPU9250 at address 0x69
+//#define LSM9DS0_6a                      // LSM9DS0 at address 0x6a
+//#define LSM9DS0_6b                      // LSM9DS0 at address 0x6b
+//#define GD20HM303D_6a                   // GD20H + M303D at address 0x6a
+//#define GD20HM303D_6b                   // GD20H + M303D at address 0x6b
+//#define GD20HM303DLHC_6a                // GD20H + M303DLHC at address 0x6a
+//#define GD20HM303DLHC_6b                // GD20H + M303DLHC at address 0x6b
+//#define GD20M303DLHC_6a                 // GD20 + M303DLHC at address 0x6a
+//#define GD20M303DLHC_6b                 // GD20 + M303DLHC at address 0x6b
+//#define BNO055_28                       // BNO055 at address 0x28
+//#define BNO055_29                       // BNO055 at address 0x29
+
+//  IMU type codes
+
+#define RTIMU_TYPE_MPU9150                  1                   // InvenSense MPU9150
+#define RTIMU_TYPE_LSM9DS0                  2                   // STM LSM9DS0 (eg Sparkfun IMU)
+#define RTIMU_TYPE_GD20HM303D               3                   // STM L3GD20H/LSM303D (Pololu Altimu)
+#define RTIMU_TYPE_GD20M303DLHC             4                   // STM L3GD20/LSM303DHLC (old Adafruit IMU)
+#define RTIMU_TYPE_MPU9250                  5                   // InvenSense MPU9250
+#define RTIMU_TYPE_GD20HM303DLHC            6                   // STM L3GD20H/LSM303DHLC (new Adafruit IMU)
+#define RTIMU_TYPE_BNO055                   7                   // BNO055
+
+//  Pressure enable defs - only one should be enabled, the rest commented out
+
+//#define BMP180                              // BMP180
+//#define LPS25H_5c                           // LPS25H at standard address
+//#define LPS25H_5d                           // LPS25H at option address
+//#define MS5611_76                           // MS5611 at standard address
+//#define MS5611_77                           // MS5611 at option address
+
+//  IMU Axis rotation defs
+//
+//  These allow the IMU to be virtually repositioned if it is in a non-standard configuration
+//  Standard configuration is X pointing at north, Y pointing east and Z pointing down
+//  with the IMU horizontal. There are 24 different possible orientations as defined
+//  below. Setting the axis rotation code to non-zero values performs the repositioning.
+//
+//  Uncomment the one required
+
+#define RTIMU_XNORTH_YEAST              0                   // this is the default identity matrix
+//#define RTIMU_XEAST_YSOUTH              1
+//#define RTIMU_XSOUTH_YWEST              2
+//#define RTIMU_XWEST_YNORTH              3
+//#define RTIMU_XNORTH_YWEST              4
+//#define RTIMU_XEAST_YNORTH              5
+//#define RTIMU_XSOUTH_YEAST              6
+//#define RTIMU_XWEST_YSOUTH              7
+//#define RTIMU_XUP_YNORTH                8
+//#define RTIMU_XUP_YEAST                 9
+//#define RTIMU_XUP_YSOUTH                10
+//#define RTIMU_XUP_YWEST                 11
+//#define RTIMU_XDOWN_YNORTH              12
+//#define RTIMU_XDOWN_YEAST               13
+//#define RTIMU_XDOWN_YSOUTH              14
+//#define RTIMU_XDOWN_YWEST               15
+//#define RTIMU_XNORTH_YUP                16
+//#define RTIMU_XEAST_YUP                 17
+//#define RTIMU_XSOUTH_YUP                18
+//#define RTIMU_XWEST_YUP                 19
+//#define RTIMU_XNORTH_YDOWN              20
+//#define RTIMU_XEAST_YDOWN               21
+//#define RTIMU_XSOUTH_YDOWN              22
+//#define RTIMU_XWEST_YDOWN               23
+
+#endif // _RTIMULIBDEFS_H

osc32_9255/RTIMULib/RTIMUMPU9150.cpp

@@ -0,0 +1,545 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMUMPU9150.h"
+#include "RTIMUSettings.h"
+
+#if defined(MPU9150_68) || defined(MPU9150_69)
+
+RTIMUMPU9150::RTIMUMPU9150(RTIMUSettings *settings) : RTIMU(settings)
+{
+
+}
+
+RTIMUMPU9150::~RTIMUMPU9150()
+{
+}
+
+bool RTIMUMPU9150::setLpf(unsigned char lpf)
+{
+    switch (lpf) {
+    case MPU9150_LPF_256:
+    case MPU9150_LPF_188:
+    case MPU9150_LPF_98:
+    case MPU9150_LPF_42:
+    case MPU9150_LPF_20:
+    case MPU9150_LPF_10:
+    case MPU9150_LPF_5:
+        m_lpf = lpf;
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+
+bool RTIMUMPU9150::setSampleRate(int rate)
+{
+    if ((rate < MPU9150_SAMPLERATE_MIN) || (rate > MPU9150_SAMPLERATE_MAX)) {
+        return false;
+    }
+    m_sampleRate = rate;
+    m_sampleInterval = (unsigned long)1000 / m_sampleRate;
+    if (m_sampleInterval == 0)
+        m_sampleInterval = 1;
+    return true;
+}
+
+bool RTIMUMPU9150::setCompassRate(int rate)
+{
+    if ((rate < MPU9150_COMPASSRATE_MIN) || (rate > MPU9150_COMPASSRATE_MAX)) {
+        return false;
+    }
+    m_compassRate = rate;
+    return true;
+}
+
+bool RTIMUMPU9150::setGyroFsr(unsigned char fsr)
+{
+    switch (fsr) {
+    case MPU9150_GYROFSR_250:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (131.0 * 180.0);
+        return true;
+
+    case MPU9150_GYROFSR_500:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (62.5 * 180.0);
+        return true;
+
+    case MPU9150_GYROFSR_1000:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (32.8 * 180.0);
+        return true;
+
+    case MPU9150_GYROFSR_2000:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (16.4 * 180.0);
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+bool RTIMUMPU9150::setAccelFsr(unsigned char fsr)
+{
+    switch (fsr) {
+    case MPU9150_ACCELFSR_2:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/16384.0;
+        return true;
+
+    case MPU9150_ACCELFSR_4:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/8192.0;
+        return true;
+
+    case MPU9150_ACCELFSR_8:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/4096.0;
+        return true;
+
+    case MPU9150_ACCELFSR_16:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/2048.0;
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+
+int RTIMUMPU9150::IMUInit()
+{
+    unsigned char result;
+
+    m_firstTime = true;
+    m_compassPresent = true;
+
+#ifdef MPU9150_CACHE_MODE
+    m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+    //  configure IMU
+
+    m_slaveAddr = m_settings->m_I2CSlaveAddress;
+    setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate);
+    setCompassRate(m_settings->m_MPU9150CompassSampleRate);
+    setLpf(m_settings->m_MPU9150GyroAccelLpf);
+    setGyroFsr(m_settings->m_MPU9150GyroFsr);
+    setAccelFsr(m_settings->m_MPU9150AccelFsr);
+
+    setCalibrationData();
+
+    //  reset the MPU9150
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80))
+        return -1;
+
+    delay(100);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00))
+        return -4;
+
+    if (!I2Cdev::readByte(m_slaveAddr, MPU9150_WHO_AM_I, &result))
+        return -5;
+
+    if (result != 0x68) {
+         return -6;
+    }
+
+    //  now configure the various components
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf))
+        return -7;
+
+    if (!setSampleRate())
+        return -8;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr))
+        return -9;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr))
+         return -10;
+
+    //  now configure compass
+
+    result = configureCompass();
+    if (result < 0)
+        return result;
+
+    //  enable the sensors
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_1, 1))
+        return -28;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_2, 0))
+         return -29;
+
+    //  select the data to go into the FIFO and enable
+
+    if (!resetFifo())
+        return -30;
+
+    gyroBiasInit();
+    return 1;
+}
+
+bool RTIMUMPU9150::configureCompass()
+{
+    unsigned char asa[3];
+    unsigned char id;
+
+    m_compassIs5883 = false;
+    m_compassDataLength = 8;
+    
+    if (!bypassOn())
+        return -11;
+
+    // get fuse ROM data
+
+    if (!I2Cdev::writeByte(AK8975_ADDRESS, AK8975_CNTL, 0)) {
+        //  check to see if an HMC5883L is fitted
+
+        if (!I2Cdev::readBytes(HMC5883_ADDRESS, HMC5883_ID, 1, &id)) {
+            bypassOff();
+
+            //  this is returning true so that MPU-6050 by itself will work
+
+            m_compassPresent = false;
+            return 1;
+        }
+        if (id != 0x48) {                                   // incorrect id for HMC5883L
+
+            bypassOff();
+
+            //  this is returning true so that MPU-6050 by itself will work
+
+            m_compassPresent = false;
+            return 1;
+        }
+
+        // HMC5883 is present - use that
+
+        if (!I2Cdev::writeByte(HMC5883_ADDRESS, HMC5883_CONFIG_A, 0x38)) {
+            bypassOff();
+            return -12;
+        }
+
+        if (!I2Cdev::writeByte(HMC5883_ADDRESS, HMC5883_CONFIG_B, 0x20)) {
+            bypassOff();
+            return -12;
+        }
+
+        if (!I2Cdev::writeByte(HMC5883_ADDRESS, HMC5883_MODE, 0x00)) {
+            bypassOff();
+            return -12;
+        }
+
+        Serial.println("Detected MPU-6050 with HMC5883");
+
+        m_compassDataLength = 6;
+        m_compassIs5883 = true;
+
+    } else {
+
+        if (!I2Cdev::writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x0f)) {
+            bypassOff();
+            return -13;
+        }
+
+        if (!I2Cdev::readBytes(AK8975_ADDRESS, AK8975_ASAX, 3, asa)) {
+            bypassOff();
+            return -14;
+        }
+
+        //  convert asa to usable scale factor
+
+        m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
+        m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
+        m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
+
+        if (!I2Cdev::writeByte(AK8975_ADDRESS, AK8975_CNTL, 0)) {
+            bypassOff();
+            return -15;
+        }
+    }
+
+    if (!bypassOff())
+        return -16;
+
+    //  now set up MPU9150 to talk to the compass chip
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40))
+        return -17;
+
+    if (m_compassIs5883) {
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | HMC5883_ADDRESS))
+            return -18;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_REG, HMC5883_DATA_X_HI))
+            return -19;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x86))
+            return -20;
+    } else {
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS))
+            return -18;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1))
+            return -19;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88))
+            return -20;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS))
+            return -21;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL))
+            return -22;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81))
+            return -23;
+
+        if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1))
+            return -24;
+    }
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3))
+        return -25;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80))
+        return -26;
+
+    if (!setCompassRate())
+        return -27;
+
+    return 1;
+}
+
+bool RTIMUMPU9150::resetFifo()
+{
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_ENABLE, 0))
+        return false;
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_FIFO_EN, 0))
+        return false;
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, 0))
+        return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, 0x04))
+        return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, 0x60))
+        return false;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_ENABLE, 1))
+        return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_FIFO_EN, 0x78))
+        return false;
+
+    return true;
+}
+
+bool RTIMUMPU9150::bypassOn()
+{
+    unsigned char userControl;
+
+    if (!I2Cdev::readByte(m_slaveAddr, MPU9150_USER_CTRL, &userControl))
+        return false;
+
+    userControl &= ~0x20;
+    userControl |= 2;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, userControl))
+        return false;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x82))
+        return false;
+
+    delay(50);
+    return true;
+}
+
+
+bool RTIMUMPU9150::bypassOff()
+{
+    unsigned char userControl;
+
+    if (!I2Cdev::readByte(m_slaveAddr, MPU9150_USER_CTRL, &userControl))
+        return false;
+
+    userControl |= 0x20;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, userControl))
+        return false;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x80))
+         return false;
+
+    delay(50);
+    return true;
+}
+
+bool RTIMUMPU9150::setSampleRate()
+{
+    int clockRate = 1000;
+
+    if (m_lpf == MPU9150_LPF_256)
+        clockRate = 8000;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_SMPRT_DIV, (unsigned char)(clockRate / m_sampleRate - 1)))
+        return false;
+
+    return true;
+}
+
+bool RTIMUMPU9150::setCompassRate()
+{
+    int rate;
+
+    rate = m_sampleRate / m_compassRate - 1;
+
+    if (rate > 31)
+        rate = 31;
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV4_CTRL, rate))
+         return false;
+    return true;
+}
+
+int RTIMUMPU9150::IMUGetPollInterval()
+{
+    return (400 / m_sampleRate);
+}
+
+bool RTIMUMPU9150::IMURead()
+{
+    unsigned char fifoCount[2];
+    unsigned int count;
+    unsigned char fifoData[12];
+    unsigned char compassData[8];
+
+    if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_FIFO_COUNT_H, 2, fifoCount))
+         return false;
+
+    count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
+
+    if (count == 1024) {
+        resetFifo();
+        m_timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
+        return false;
+    }
+
+    if (count > MPU9150_FIFO_CHUNK_SIZE * 40) {
+        // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
+        while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) {
+            if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData))
+                return false;
+            count -= MPU9150_FIFO_CHUNK_SIZE;
+            m_timestamp += m_sampleInterval;
+        }
+    }
+
+    if (count < MPU9150_FIFO_CHUNK_SIZE)
+        return false;
+
+    if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData))
+        return false;
+
+    if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData))
+        return false;
+
+    RTMath::convertToVector(fifoData, m_accel, m_accelScale, true);
+    RTMath::convertToVector(fifoData + 6, m_gyro, m_gyroScale, true);
+
+    if (m_compassIs5883)
+        RTMath::convertToVector(compassData, m_compass, 0.092f, true);
+    else
+        RTMath::convertToVector(compassData + 1, m_compass, 0.3f, false);
+
+
+    //  sort out gyro axes
+
+    m_gyro.setY(-m_gyro.y());
+    m_gyro.setZ(-m_gyro.z());
+
+    //  sort out accel data;
+
+    m_accel.setX(-m_accel.x());
+
+    if (m_compassPresent) {
+        if (m_compassIs5883) {
+            //  sort out compass axes
+
+            float temp;
+
+            temp = m_compass.y();
+            m_compass.setY(-m_compass.z());
+            m_compass.setZ(-temp);
+
+        } else {
+
+            //  use the compass fuse data adjustments
+
+            m_compass.setX(m_compass.x() * m_compassAdjust[0]);
+            m_compass.setY(m_compass.y() * m_compassAdjust[1]);
+            m_compass.setZ(m_compass.z() * m_compassAdjust[2]);
+
+            //  sort out compass axes
+
+            float temp;
+
+            temp = m_compass.x();
+            m_compass.setX(m_compass.y());
+            m_compass.setY(-temp);
+        }
+    } else {
+        m_compass.setX(0);
+        m_compass.setY(0);
+        m_compass.setZ(0);
+    }
+
+    //  now do standard processing
+
+    handleGyroBias();
+    if (m_compassPresent)
+        calibrateAverageCompass();
+
+    if (m_firstTime)
+        m_timestamp = millis();
+    else
+        m_timestamp += m_sampleInterval;
+
+    m_firstTime = false;
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMUMPU9150.h

@@ -0,0 +1,171 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMUMPU9150_H
+#define	_RTIMUMPU9150_H
+
+#include "RTIMU.h"
+
+//  MPU9150 I2C Slave Addresses
+
+#define MPU9150_ADDRESS0            0x68
+#define MPU9150_ADDRESS1            0x69
+#define MPU9150_ID                  0x68
+
+//  these magnetometers are on aux bus
+
+#define AK8975_ADDRESS              0x0c
+#define HMC5883_ADDRESS             0x1e
+
+//  Register map
+
+#define MPU9150_YG_OFFS_TC          0x01
+#define MPU9150_SMPRT_DIV           0x19
+#define MPU9150_LPF_CONFIG          0x1a
+#define MPU9150_GYRO_CONFIG         0x1b
+#define MPU9150_ACCEL_CONFIG        0x1c
+#define MPU9150_FIFO_EN             0x23
+#define MPU9150_I2C_MST_CTRL        0x24
+#define MPU9150_I2C_SLV0_ADDR       0x25
+#define MPU9150_I2C_SLV0_REG        0x26
+#define MPU9150_I2C_SLV0_CTRL       0x27
+#define MPU9150_I2C_SLV1_ADDR       0x28
+#define MPU9150_I2C_SLV1_REG        0x29
+#define MPU9150_I2C_SLV1_CTRL       0x2a
+#define MPU9150_I2C_SLV4_CTRL       0x34
+#define MPU9150_INT_PIN_CFG         0x37
+#define MPU9150_INT_ENABLE          0x38
+#define MPU9150_INT_STATUS          0x3a
+#define MPU9150_ACCEL_XOUT_H        0x3b
+#define MPU9150_GYRO_XOUT_H         0x43
+#define MPU9150_EXT_SENS_DATA_00    0x49
+#define MPU9150_I2C_SLV1_DO         0x64
+#define MPU9150_I2C_MST_DELAY_CTRL  0x67
+#define MPU9150_USER_CTRL           0x6a
+#define MPU9150_PWR_MGMT_1          0x6b
+#define MPU9150_PWR_MGMT_2          0x6c
+#define MPU9150_FIFO_COUNT_H        0x72
+#define MPU9150_FIFO_R_W            0x74
+#define MPU9150_WHO_AM_I            0x75
+
+//  sample rate defines (applies to gyros and accels, not mags)
+
+#define MPU9150_SAMPLERATE_MIN      5                      // 5 samples per second is the lowest
+#define MPU9150_SAMPLERATE_MAX      1000                   // 1000 samples per second is the absolute maximum
+
+//  compass rate defines
+
+#define MPU9150_COMPASSRATE_MIN     1                      // 1 samples per second is the lowest
+#define MPU9150_COMPASSRATE_MAX     100                    // 100 samples per second is maximum
+
+//  LPF options (gyros and accels)
+
+#define MPU9150_LPF_256             0                       // gyro: 256Hz, accel: 260Hz
+#define MPU9150_LPF_188             1                       // gyro: 188Hz, accel: 184Hz
+#define MPU9150_LPF_98              2                       // gyro: 98Hz, accel: 98Hz
+#define MPU9150_LPF_42              3                       // gyro: 42Hz, accel: 44Hz
+#define MPU9150_LPF_20              4                       // gyro: 20Hz, accel: 21Hz
+#define MPU9150_LPF_10              5                       // gyro: 10Hz, accel: 10Hz
+#define MPU9150_LPF_5               6                       // gyro: 5Hz, accel: 5Hz
+
+//  Gyro FSR options
+
+#define MPU9150_GYROFSR_250         0                       // +/- 250 degrees per second
+#define MPU9150_GYROFSR_500         8                       // +/- 500 degrees per second
+#define MPU9150_GYROFSR_1000        0x10                    // +/- 1000 degrees per second
+#define MPU9150_GYROFSR_2000        0x18                    // +/- 2000 degrees per second
+
+//  Accel FSR options
+
+#define MPU9150_ACCELFSR_2          0                       // +/- 2g
+#define MPU9150_ACCELFSR_4          8                       // +/- 4g
+#define MPU9150_ACCELFSR_8          0x10                    // +/- 8g
+#define MPU9150_ACCELFSR_16         0x18                    // +/- 16g
+
+
+//  AK8975 compass registers
+
+#define AK8975_DEVICEID             0x0                     // the device ID
+#define AK8975_ST1                  0x02                    // status 1
+#define AK8975_CNTL                 0x0a                    // control reg
+#define AK8975_ASAX                 0x10                    // start of the fuse ROM data
+
+//  HMC5883 compass registers
+
+#define HMC5883_CONFIG_A            0x00                    // configuration A
+#define HMC5883_CONFIG_B            0x01                    // configuration B
+#define HMC5883_MODE                0x02                    // mode
+#define HMC5883_DATA_X_HI           0x03                    // data x msb
+#define HMC5883_STATUS              0x09                    // status
+#define HMC5883_ID                  0x0a                    // id
+
+//  FIFO transfer size
+
+#define MPU9150_FIFO_CHUNK_SIZE     12                      // gyro and accels take 12 bytes
+
+class RTIMUMPU9150 : public RTIMU
+{
+public:
+    RTIMUMPU9150(RTIMUSettings *settings);
+    ~RTIMUMPU9150();
+
+    bool setLpf(unsigned char lpf);
+    bool setSampleRate(int rate);
+    bool setCompassRate(int rate);
+    bool setGyroFsr(unsigned char fsr);
+    bool setAccelFsr(unsigned char fsr);
+
+    virtual const char *IMUName() { return "MPU-9150"; }
+    virtual int IMUType() { return RTIMU_TYPE_MPU9150; }
+    virtual int IMUInit();
+    virtual bool IMURead();
+    virtual int IMUGetPollInterval();
+
+private:
+    bool configureCompass();                                // configure the compass
+    bool bypassOn();                                        // talk to compass
+    bool bypassOff();                                       // talk to MPU9150
+    bool setSampleRate();
+    bool setCompassRate();
+    bool resetFifo();
+
+    bool m_firstTime;                                       // if first sample
+
+    unsigned char m_slaveAddr;                              // I2C address of MPU9150
+    unsigned char m_bus;                                    // I2C bus (usually 1 for Raspberry Pi for example)
+
+    unsigned char m_lpf;                                    // low pass filter setting
+    int m_compassRate;                                      // compass sample rate in Hz
+    unsigned char m_gyroFsr;
+    unsigned char m_accelFsr;
+
+    RTFLOAT m_gyroScale;
+    RTFLOAT m_accelScale;
+
+    RTFLOAT m_compassAdjust[3];                             // the compass fuse ROM values converted for use
+    bool m_compassPresent;                                  // false for MPU-6050
+    bool m_compassIs5883;                                   // if it is an MPU-6050/HMC5883 combo
+    int m_compassDataLength;                                // 8 for MPU-9150, 6 for HMC5883
+};
+
+#endif // _RTIMUMPU9150_H

osc32_9255/RTIMULib/RTIMUMPU9250.cpp

@@ -0,0 +1,493 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMUMPU9250.h"
+#include "RTIMUSettings.h"
+
+#if defined(MPU9250_68) || defined(MPU9250_69)
+
+RTIMUMPU9250::RTIMUMPU9250(RTIMUSettings *settings) : RTIMU(settings)
+{
+
+}
+
+RTIMUMPU9250::~RTIMUMPU9250()
+{
+}
+
+
+bool RTIMUMPU9250::setSampleRate(int rate)
+{
+    if ((rate < MPU9250_SAMPLERATE_MIN) || (rate > MPU9250_SAMPLERATE_MAX)) {
+        return false;
+    }
+    m_sampleRate = rate;
+    m_sampleInterval = (unsigned long)1000 / m_sampleRate;
+    if (m_sampleInterval == 0)
+        m_sampleInterval = 1;
+    return true;
+}
+
+bool RTIMUMPU9250::setGyroLpf(unsigned char lpf)
+{
+    switch (lpf) {
+    case MPU9250_GYRO_LPF_8800:
+    case MPU9250_GYRO_LPF_3600:
+    case MPU9250_GYRO_LPF_250:
+    case MPU9250_GYRO_LPF_184:
+    case MPU9250_GYRO_LPF_92:
+    case MPU9250_GYRO_LPF_41:
+    case MPU9250_GYRO_LPF_20:
+    case MPU9250_GYRO_LPF_10:
+    case MPU9250_GYRO_LPF_5:
+        m_gyroLpf = lpf;
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+bool RTIMUMPU9250::setAccelLpf(unsigned char lpf)
+{
+    switch (lpf) {
+    case MPU9250_ACCEL_LPF_1130:
+    case MPU9250_ACCEL_LPF_460:
+    case MPU9250_ACCEL_LPF_184:
+    case MPU9250_ACCEL_LPF_92:
+    case MPU9250_ACCEL_LPF_41:
+    case MPU9250_ACCEL_LPF_20:
+    case MPU9250_ACCEL_LPF_10:
+    case MPU9250_ACCEL_LPF_5:
+        m_accelLpf = lpf;
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+bool RTIMUMPU9250::setCompassRate(int rate)
+{
+    if ((rate < MPU9250_COMPASSRATE_MIN) || (rate > MPU9250_COMPASSRATE_MAX)) {
+        return false;
+    }
+    m_compassRate = rate;
+    return true;
+}
+
+bool RTIMUMPU9250::setGyroFsr(unsigned char fsr)
+{
+    switch (fsr) {
+    case MPU9250_GYROFSR_250:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (131.0 * 180.0);
+        return true;
+
+    case MPU9250_GYROFSR_500:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (62.5 * 180.0);
+        return true;
+
+    case MPU9250_GYROFSR_1000:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (32.8 * 180.0);
+        return true;
+
+    case MPU9250_GYROFSR_2000:
+        m_gyroFsr = fsr;
+        m_gyroScale = RTMATH_PI / (16.4 * 180.0);
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+bool RTIMUMPU9250::setAccelFsr(unsigned char fsr)
+{
+    switch (fsr) {
+    case MPU9250_ACCELFSR_2:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/16384.0;
+        return true;
+
+    case MPU9250_ACCELFSR_4:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/8192.0;
+        return true;
+
+    case MPU9250_ACCELFSR_8:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/4096.0;
+        return true;
+
+    case MPU9250_ACCELFSR_16:
+        m_accelFsr = fsr;
+        m_accelScale = 1.0/2048.0;
+        return true;
+
+    default:
+        return false;
+    }
+}
+
+
+int RTIMUMPU9250::IMUInit()
+{
+    unsigned char result;
+    unsigned char asa[3];
+
+    m_firstTime = true;
+
+#ifdef MPU9250_CACHE_MODE
+    m_cacheIn = m_cacheOut = m_cacheCount = 0;
+#endif
+    //  configure IMU
+
+    m_slaveAddr = m_settings->m_I2CSlaveAddress;
+
+    setSampleRate(m_settings->m_MPU9250GyroAccelSampleRate);
+    setCompassRate(m_settings->m_MPU9250CompassSampleRate);
+    setGyroLpf(m_settings->m_MPU9250GyroLpf);
+    setAccelLpf(m_settings->m_MPU9250AccelLpf);
+    setGyroFsr(m_settings->m_MPU9250GyroFsr);
+    setAccelFsr(m_settings->m_MPU9250AccelFsr);
+
+    setCalibrationData();
+
+    //  reset the MPU9250
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x80))
+        return -1;
+
+    delay(100);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x00))
+        return -4;
+
+    if (!I2Cdev::readByte(m_slaveAddr, MPU9250_WHO_AM_I, &result))
+        return -5;
+
+    if (result != MPU9250_ID) {
+         return -6;
+    }
+
+    //  now configure the various components
+
+    if (!setGyroConfig())
+        return -7;
+
+    if (!setAccelConfig())
+        return -8;
+
+    if (!setSampleRate())
+        return -9;
+
+    //  now configure compass
+
+    if (!bypassOn())
+        return -11;
+
+    // get fuse ROM data
+
+    if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) {
+        bypassOff();
+        return -12;
+    }
+
+    if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0f)) {
+        bypassOff();
+        return -13;
+    }
+
+    if (!I2Cdev::readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, asa)) {
+        bypassOff();
+        return -14;
+    }
+
+    //  convert asa to usable scale factor
+
+    m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
+    m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
+    m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
+
+    if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) {
+        bypassOff();
+        return -15;
+    }
+
+    if (!bypassOff())
+        return -16;
+
+    //  now set up MPU9250 to talk to the compass chip
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40))
+        return -17;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS))
+        return -18;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ST1))
+        return -19;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x88))
+        return -20;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS))
+        return -21;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL))
+        return -22;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81))
+        return -23;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x1))
+        return -24;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_DELAY_CTRL, 0x3))
+        return -25;
+
+    if (!setCompassRate())
+        return -27;
+
+    //  enable the sensors
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 1))
+        return -28;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_2, 0))
+         return -29;
+
+    //  select the data to go into the FIFO and enable
+
+    if (!resetFifo())
+        return -30;
+
+    gyroBiasInit();
+    return 1;
+}
+
+bool RTIMUMPU9250::resetFifo()
+{
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_ENABLE, 0))
+        return false;
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_FIFO_EN, 0))
+        return false;
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, 0))
+        return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, 0x04))
+        return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, 0x60))
+        return false;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_ENABLE, 1))
+        return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_FIFO_EN, 0x78))
+        return false;
+
+    return true;
+}
+
+bool RTIMUMPU9250::bypassOn()
+{
+    unsigned char userControl;
+
+    if (!I2Cdev::readByte(m_slaveAddr, MPU9250_USER_CTRL, &userControl))
+        return false;
+
+    userControl &= ~0x20;
+    userControl |= 2;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, userControl))
+        return false;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x82))
+        return false;
+
+    delay(50);
+    return true;
+}
+
+
+bool RTIMUMPU9250::bypassOff()
+{
+    unsigned char userControl;
+
+    if (!I2Cdev::readByte(m_slaveAddr, MPU9250_USER_CTRL, &userControl))
+        return false;
+
+    userControl |= 0x20;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, userControl))
+        return false;
+
+    delay(50);
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x80))
+         return false;
+
+    delay(50);
+    return true;
+}
+
+bool RTIMUMPU9250::setGyroConfig()
+{
+    unsigned char gyroConfig = m_gyroFsr + ((m_gyroLpf >> 3) & 3);
+    unsigned char gyroLpf = m_gyroLpf & 7;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_GYRO_CONFIG, gyroConfig))
+         return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_GYRO_LPF, gyroLpf))
+         return false;
+    return true;
+}
+
+bool RTIMUMPU9250::setAccelConfig()
+{
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_ACCEL_CONFIG, m_accelFsr))
+         return false;
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_ACCEL_LPF, m_accelLpf))
+         return false;
+    return true;
+}
+
+bool RTIMUMPU9250::setSampleRate()
+{
+    if (m_sampleRate > 1000)
+        return true;                                        // SMPRT not used above 1000Hz
+
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_SMPRT_DIV, (unsigned char) (1000 / m_sampleRate - 1)))
+        return false;
+
+    return true;
+}
+
+
+bool RTIMUMPU9250::setCompassRate()
+{
+    int rate;
+
+    rate = m_sampleRate / m_compassRate - 1;
+
+    if (rate > 31)
+        rate = 31;
+    if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV4_CTRL, rate))
+         return false;
+    return true;
+}
+
+int RTIMUMPU9250::IMUGetPollInterval()
+{
+    return (400 / m_sampleRate);
+}
+
+bool RTIMUMPU9250::IMURead()
+{
+    unsigned char fifoCount[2];
+    unsigned int count;
+    unsigned char fifoData[12];
+    unsigned char compassData[8];
+
+    if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount))
+         return false;
+
+    count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
+
+    if (count == 1024) {
+        resetFifo();
+        m_timestamp += m_sampleInterval * (1024 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
+        return false;
+    }
+
+    if (count > MPU9250_FIFO_CHUNK_SIZE * 40) {
+        // more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
+        while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) {
+            if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData))
+                return false;
+            count -= MPU9250_FIFO_CHUNK_SIZE;
+            m_timestamp += m_sampleInterval;
+        }
+    }
+
+    if (count < MPU9250_FIFO_CHUNK_SIZE)
+        return false;
+
+    if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData))
+        return false;
+
+    if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData))
+        return false;
+
+    RTMath::convertToVector(fifoData, m_accel, m_accelScale, true);
+    RTMath::convertToVector(fifoData + 6, m_gyro, m_gyroScale, true);
+    RTMath::convertToVector(compassData + 1, m_compass, 0.6f, false);
+
+    //  sort out gyro axes
+
+    m_gyro.setY(-m_gyro.y());
+    m_gyro.setZ(-m_gyro.z());
+
+    //  sort out accel data;
+
+    m_accel.setX(-m_accel.x());
+
+    //  use the fuse data adjustments for compass
+
+    m_compass.setX(m_compass.x() * m_compassAdjust[0]);
+    m_compass.setY(m_compass.y() * m_compassAdjust[1]);
+    m_compass.setZ(m_compass.z() * m_compassAdjust[2]);
+
+    //  sort out compass axes
+
+    float temp;
+
+    temp = m_compass.x();
+    m_compass.setX(m_compass.y());
+    m_compass.setY(-temp);
+
+    //  now do standard processing
+
+    handleGyroBias();
+    calibrateAverageCompass();
+
+    if (m_firstTime)
+        m_timestamp = millis();
+    else
+        m_timestamp += m_sampleInterval;
+
+    m_firstTime = false;
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTIMUMPU9250.h

@@ -0,0 +1,175 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMUMPU9250_H
+#define	_RTIMUMPU9250_H
+
+#include "RTIMU.h"
+
+//  MPU9250 I2C Slave Addresses
+
+#define MPU9250_ADDRESS0            0x68
+#define MPU9250_ADDRESS1            0x69
+#define MPU9250_ID                  0x71
+
+#define AK8963_ADDRESS              0x0c
+
+//  Register map
+
+#define MPU9250_SMPRT_DIV           0x19
+#define MPU9250_GYRO_LPF            0x1a
+#define MPU9250_GYRO_CONFIG         0x1b
+#define MPU9250_ACCEL_CONFIG        0x1c
+#define MPU9250_ACCEL_LPF           0x1d
+#define MPU9250_FIFO_EN             0x23
+#define MPU9250_I2C_MST_CTRL        0x24
+#define MPU9250_I2C_SLV0_ADDR       0x25
+#define MPU9250_I2C_SLV0_REG        0x26
+#define MPU9250_I2C_SLV0_CTRL       0x27
+#define MPU9250_I2C_SLV1_ADDR       0x28
+#define MPU9250_I2C_SLV1_REG        0x29
+#define MPU9250_I2C_SLV1_CTRL       0x2a
+#define MPU9250_I2C_SLV2_ADDR       0x2b
+#define MPU9250_I2C_SLV2_REG        0x2c
+#define MPU9250_I2C_SLV2_CTRL       0x2d
+#define MPU9250_I2C_SLV4_CTRL       0x34
+#define MPU9250_INT_PIN_CFG         0x37
+#define MPU9250_INT_ENABLE          0x38
+#define MPU9250_INT_STATUS          0x3a
+#define MPU9250_ACCEL_XOUT_H        0x3b
+#define MPU9250_GYRO_XOUT_H         0x43
+#define MPU9250_EXT_SENS_DATA_00    0x49
+#define MPU9250_I2C_SLV1_DO         0x64
+#define MPU9250_I2C_MST_DELAY_CTRL  0x67
+#define MPU9250_USER_CTRL           0x6a
+#define MPU9250_PWR_MGMT_1          0x6b
+#define MPU9250_PWR_MGMT_2          0x6c
+#define MPU9250_FIFO_COUNT_H        0x72
+#define MPU9250_FIFO_R_W            0x74
+#define MPU9250_WHO_AM_I            0x75
+
+//  sample rate defines (applies to gyros and accels, not mags)
+
+#define MPU9250_SAMPLERATE_MIN      5                       // 5 samples per second is the lowest
+#define MPU9250_SAMPLERATE_MAX      1000                    // 1000 samples per second is the absolute maximum
+
+//  compass rate defines
+
+#define MPU9250_COMPASSRATE_MIN     1                       // 1 samples per second is the lowest
+#define MPU9250_COMPASSRATE_MAX     100                     // 100 samples per second is maximum
+
+//  Gyro LPF options
+
+#define MPU9250_GYRO_LPF_8800       0x11                    // 8800Hz, 0.64mS delay
+#define MPU9250_GYRO_LPF_3600       0x10                    // 3600Hz, 0.11mS delay
+#define MPU9250_GYRO_LPF_250        0x00                    // 250Hz, 0.97mS delay
+#define MPU9250_GYRO_LPF_184        0x01                    // 184Hz, 2.9mS delay
+#define MPU9250_GYRO_LPF_92         0x02                    // 92Hz, 3.9mS delay
+#define MPU9250_GYRO_LPF_41         0x03                    // 41Hz, 5.9mS delay
+#define MPU9250_GYRO_LPF_20         0x04                    // 20Hz, 9.9mS delay
+#define MPU9250_GYRO_LPF_10         0x05                    // 10Hz, 17.85mS delay
+#define MPU9250_GYRO_LPF_5          0x06                    // 5Hz, 33.48mS delay
+
+//  Gyro FSR options
+
+#define MPU9250_GYROFSR_250         0                       // +/- 250 degrees per second
+#define MPU9250_GYROFSR_500         8                       // +/- 500 degrees per second
+#define MPU9250_GYROFSR_1000        0x10                    // +/- 1000 degrees per second
+#define MPU9250_GYROFSR_2000        0x18                    // +/- 2000 degrees per second
+
+//  Accel FSR options
+
+#define MPU9250_ACCELFSR_2          0                       // +/- 2g
+#define MPU9250_ACCELFSR_4          8                       // +/- 4g
+#define MPU9250_ACCELFSR_8          0x10                    // +/- 8g
+#define MPU9250_ACCELFSR_16         0x18                    // +/- 16g
+
+//  Accel LPF options
+
+#define MPU9250_ACCEL_LPF_1130      0x08                    // 1130Hz, 0.75mS delay
+#define MPU9250_ACCEL_LPF_460       0x00                    // 460Hz, 1.94mS delay
+#define MPU9250_ACCEL_LPF_184       0x01                    // 184Hz, 5.80mS delay
+#define MPU9250_ACCEL_LPF_92        0x02                    // 92Hz, 7.80mS delay
+#define MPU9250_ACCEL_LPF_41        0x03                    // 41Hz, 11.80mS delay
+#define MPU9250_ACCEL_LPF_20        0x04                    // 20Hz, 19.80mS delay
+#define MPU9250_ACCEL_LPF_10        0x05                    // 10Hz, 35.70mS delay
+#define MPU9250_ACCEL_LPF_5         0x06                    // 5Hz, 66.96mS delay
+
+//  AK8963 compass registers
+
+#define AK8963_DEVICEID             0x48                    // the device ID
+#define AK8963_ST1                  0x02                    // status 1
+#define AK8963_CNTL                 0x0a                    // control reg
+#define AK8963_ASAX                 0x10                    // start of the fuse ROM data
+
+//  FIFO transfer size
+
+#define MPU9250_FIFO_CHUNK_SIZE     12                      // gyro and accels take 12 bytes
+
+class RTIMUMPU9250 : public RTIMU
+{
+public:
+    RTIMUMPU9250(RTIMUSettings *settings);
+    ~RTIMUMPU9250();
+
+    bool setGyroLpf(unsigned char lpf);
+    bool setAccelLpf(unsigned char lpf);
+    bool setSampleRate(int rate);
+    bool setCompassRate(int rate);
+    bool setGyroFsr(unsigned char fsr);
+    bool setAccelFsr(unsigned char fsr);
+
+    virtual const char *IMUName() { return "MPU-9250"; }
+    virtual int IMUType() { return RTIMU_TYPE_MPU9250; }
+    virtual int IMUInit();
+    virtual bool IMURead();
+    virtual int IMUGetPollInterval();
+
+private:
+    bool setGyroConfig();
+    bool setAccelConfig();
+    bool setSampleRate();
+    bool compassSetup();
+    bool setCompassRate();
+    bool resetFifo();
+    bool bypassOn();
+    bool bypassOff();
+
+    bool m_firstTime;                                       // if first sample
+
+    unsigned char m_slaveAddr;                              // I2C address of MPU9250
+    unsigned char m_bus;                                    // I2C bus (usually 1 for Raspberry Pi for example)
+
+    unsigned char m_gyroLpf;                                // gyro low pass filter setting
+    unsigned char m_accelLpf;                               // accel low pass filter setting
+    int m_compassRate;                                      // compass sample rate in Hz
+    unsigned char m_gyroFsr;
+    unsigned char m_accelFsr;
+
+    RTFLOAT m_gyroScale;
+    RTFLOAT m_accelScale;
+
+    RTFLOAT m_compassAdjust[3];                             // the compass fuse ROM values converted for use
+};
+
+#endif // _RTIMUMPU9250_H

osc32_9255/RTIMULib/RTIMUSettings.cpp

@@ -0,0 +1,308 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTIMUSettings.h"
+
+#if defined(MPU9150_68) || defined(MPU9150_69)
+#include "RTIMUMPU9150.h"
+#endif
+
+#if defined(MPU9250_68) || defined(MPU9250_69)
+#include "RTIMUMPU9250.h"
+#endif
+
+#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
+#include "RTIMULSM9DS0.h"
+#endif
+
+#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
+#include "RTIMUGD20HM303D.h"
+#endif
+
+#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
+#include "RTIMUGD20M303DLHC.h"
+#endif
+
+#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
+#include "RTIMUGD20HM303DLHC.h"
+#endif
+
+#if defined(BNO055_28) || defined(BNO055_29)
+#include "RTIMUBNO055.h"
+#endif
+
+#if defined(BMP180)
+#include "RTPressureBMP180.h"
+#endif
+
+#if defined(LPS25H_5c) || defined (LPS25H_5d)
+#include "RTPressureLPS25H.h"
+#endif
+
+#if defined(MS5611_76) || defined (MS5611_77)
+#include "RTPressureMS5611.h"
+#endif
+
+#define RATE_TIMER_INTERVAL 2
+
+RTIMUSettings::RTIMUSettings()
+{
+    //  preset general defaults
+
+    m_imuType = -1;
+    m_I2CSlaveAddress = 0;
+
+#ifdef MPU9150_68
+    //  MPU9150 defaults
+
+    m_MPU9150GyroAccelSampleRate = 50;
+    m_MPU9150CompassSampleRate = 25;
+    m_MPU9150GyroAccelLpf = MPU9150_LPF_20;
+    m_MPU9150GyroFsr = MPU9150_GYROFSR_1000;
+    m_MPU9150AccelFsr = MPU9150_ACCELFSR_8;
+    m_imuType = RTIMU_TYPE_MPU9150;
+    m_I2CSlaveAddress = MPU9150_ADDRESS0;
+#endif
+
+#ifdef MPU9150_69
+    //  MPU9150 defaults
+
+    m_MPU9150GyroAccelSampleRate = 50;
+    m_MPU9150CompassSampleRate = 25;
+    m_MPU9150GyroAccelLpf = MPU9150_LPF_20;
+    m_MPU9150GyroFsr = MPU9150_GYROFSR_1000;
+    m_MPU9150AccelFsr = MPU9150_ACCELFSR_8;
+    m_imuType = RTIMU_TYPE_MPU9150;
+    m_I2CSlaveAddress = MPU9150_ADDRESS1;
+#endif
+
+#ifdef MPU9250_68
+    //  MPU9250 defaults
+
+    m_MPU9250GyroAccelSampleRate = 80;
+    m_MPU9250CompassSampleRate = 40;
+    m_MPU9250GyroLpf = MPU9250_GYRO_LPF_41;
+    m_MPU9250AccelLpf = MPU9250_ACCEL_LPF_41;
+    m_MPU9250GyroFsr = MPU9250_GYROFSR_1000;
+    m_MPU9250AccelFsr = MPU9250_ACCELFSR_8;
+    m_I2CSlaveAddress = MPU9250_ADDRESS0;
+#endif
+
+#ifdef MPU9250_69
+    //  MPU9250 defaults
+
+    m_MPU9250GyroAccelSampleRate = 80;
+    m_MPU9250CompassSampleRate = 40;
+    m_MPU9250GyroLpf = MPU9250_GYRO_LPF_41;
+    m_MPU9250AccelLpf = MPU9250_ACCEL_LPF_41;
+    m_MPU9250GyroFsr = MPU9250_GYROFSR_1000;
+    m_MPU9250AccelFsr = MPU9250_ACCELFSR_8;
+    m_I2CSlaveAddress = MPU9250_ADDRESS1;
+#endif
+
+#ifdef LSM9DS0_6a
+    //  LSM9DS0 defaults
+
+    m_LSM9DS0GyroSampleRate = LSM9DS0_GYRO_SAMPLERATE_95;
+    m_LSM9DS0GyroBW = LSM9DS0_GYRO_BANDWIDTH_1;
+    m_LSM9DS0GyroHpf = LSM9DS0_GYRO_HPF_4;
+    m_LSM9DS0GyroFsr = LSM9DS0_GYRO_FSR_500;
+
+    m_LSM9DS0AccelSampleRate = LSM9DS0_ACCEL_SAMPLERATE_50;
+    m_LSM9DS0AccelFsr = LSM9DS0_ACCEL_FSR_8;
+    m_LSM9DS0AccelLpf = LSM9DS0_ACCEL_LPF_50;
+
+    m_LSM9DS0CompassSampleRate = LSM9DS0_COMPASS_SAMPLERATE_50;
+    m_LSM9DS0CompassFsr = LSM9DS0_COMPASS_FSR_2;
+
+    m_imuType = RTIMU_TYPE_LSM9DS0;
+    m_I2CSlaveAddress = LSM9DS0_GYRO_ADDRESS0;
+
+#endif
+
+#ifdef LSM9DS0_6b
+    //  LSM9DS0 defaults
+
+    m_LSM9DS0GyroSampleRate = LSM9DS0_GYRO_SAMPLERATE_95;
+    m_LSM9DS0GyroBW = LSM9DS0_GYRO_BANDWIDTH_1;
+    m_LSM9DS0GyroHpf = LSM9DS0_GYRO_HPF_4;
+    m_LSM9DS0GyroFsr = LSM9DS0_GYRO_FSR_500;
+
+    m_LSM9DS0AccelSampleRate = LSM9DS0_ACCEL_SAMPLERATE_50;
+    m_LSM9DS0AccelFsr = LSM9DS0_ACCEL_FSR_8;
+    m_LSM9DS0AccelLpf = LSM9DS0_ACCEL_LPF_50;
+
+    m_LSM9DS0CompassSampleRate = LSM9DS0_COMPASS_SAMPLERATE_50;
+    m_LSM9DS0CompassFsr = LSM9DS0_COMPASS_FSR_2;
+
+    m_imuType = RTIMU_TYPE_LSM9DS0;
+    m_I2CSlaveAddress = LSM9DS0_GYRO_ADDRESS1;
+
+#endif
+
+#ifdef GD20HM303D_6a
+    //  GD20HM303D defaults
+
+    m_GD20HM303DGyroSampleRate = L3GD20H_SAMPLERATE_50;
+    m_GD20HM303DGyroBW = L3GD20H_BANDWIDTH_1;
+    m_GD20HM303DGyroHpf = L3GD20H_HPF_4;
+    m_GD20HM303DGyroFsr = L3GD20H_FSR_500;
+
+    m_GD20HM303DAccelSampleRate = LSM303D_ACCEL_SAMPLERATE_50;
+    m_GD20HM303DAccelFsr = LSM303D_ACCEL_FSR_8;
+    m_GD20HM303DAccelLpf = LSM303D_ACCEL_LPF_50;
+
+    m_GD20HM303DCompassSampleRate = LSM303D_COMPASS_SAMPLERATE_50;
+    m_GD20HM303DCompassFsr = LSM303D_COMPASS_FSR_2;
+
+    m_imuType = RTIMU_TYPE_GD20HM303D;
+    m_I2CSlaveAddress = L3GD20H_ADDRESS0;
+#endif
+
+#ifdef GD20HM303D_6b
+    //  GD20HM303D defaults
+
+    m_GD20HM303DGyroSampleRate = L3GD20H_SAMPLERATE_50;
+    m_GD20HM303DGyroBW = L3GD20H_BANDWIDTH_1;
+    m_GD20HM303DGyroHpf = L3GD20H_HPF_4;
+    m_GD20HM303DGyroFsr = L3GD20H_FSR_500;
+
+    m_GD20HM303DAccelSampleRate = LSM303D_ACCEL_SAMPLERATE_50;
+    m_GD20HM303DAccelFsr = LSM303D_ACCEL_FSR_8;
+    m_GD20HM303DAccelLpf = LSM303D_ACCEL_LPF_50;
+
+    m_GD20HM303DCompassSampleRate = LSM303D_COMPASS_SAMPLERATE_50;
+    m_GD20HM303DCompassFsr = LSM303D_COMPASS_FSR_2;
+
+    m_imuType = RTIMU_TYPE_GD20HM303D;
+    m_I2CSlaveAddress = L3GD20H_ADDRESS1;
+#endif
+
+#ifdef GD20M303DLHC_6a
+    //  GD20M303DLHC defaults
+
+    m_GD20M303DLHCGyroSampleRate = L3GD20_SAMPLERATE_95;
+    m_GD20M303DLHCGyroBW = L3GD20_BANDWIDTH_1;
+    m_GD20M303DLHCGyroHpf = L3GD20_HPF_4;
+    m_GD20M303DLHCGyroFsr = L3GD20_FSR_500;
+
+    m_GD20M303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50;
+    m_GD20M303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8;
+
+    m_GD20M303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30;
+    m_GD20M303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3;
+
+    m_imuType = RTIMU_TYPE_GD20M303DLHC;
+    m_I2CSlaveAddress = L3GD20_ADDRESS0;
+#endif
+
+#ifdef GD20M303DLHC_6b
+    //  GD20M303DLHC defaults
+
+    m_GD20M303DLHCGyroSampleRate = L3GD20_SAMPLERATE_95;
+    m_GD20M303DLHCGyroBW = L3GD20_BANDWIDTH_1;
+    m_GD20M303DLHCGyroHpf = L3GD20_HPF_4;
+    m_GD20M303DLHCGyroFsr = L3GD20_FSR_500;
+
+    m_GD20M303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50;
+    m_GD20M303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8;
+
+    m_GD20M303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30;
+    m_GD20M303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3;
+
+    m_imuType = RTIMU_TYPE_GD20M303DLHC;
+    m_I2CSlaveAddress = L3GD20_ADDRESS1;
+#endif
+
+#ifdef GD20HM303DLHC_6a
+    //  GD20HM303DLHC defaults
+
+    m_GD20HM303DLHCGyroSampleRate = L3GD20H_SAMPLERATE_50;
+    m_GD20HM303DLHCGyroBW = L3GD20H_BANDWIDTH_1;
+    m_GD20HM303DLHCGyroHpf = L3GD20H_HPF_4;
+    m_GD20HM303DLHCGyroFsr = L3GD20H_FSR_500;
+
+    m_GD20HM303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50;
+    m_GD20HM303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8;
+
+    m_GD20HM303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30;
+    m_GD20HM303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3;
+
+    m_imuType = RTIMU_TYPE_GD20HM303DLHC;
+    m_I2CSlaveAddress = L3GD20H_ADDRESS0;
+#endif
+
+#ifdef GD20HM303DLHC_6b
+    //  GD20M303DLHC defaults
+
+    m_GD20HM303DLHCGyroSampleRate = L3GD20H_SAMPLERATE_50;
+    m_GD20HM303DLHCGyroBW = L3GD20H_BANDWIDTH_1;
+    m_GD20HM303DLHCGyroHpf = L3GD20H_HPF_4;
+    m_GD20HM303DLHCGyroFsr = L3GD20H_FSR_500;
+
+    m_GD20HM303DLHCAccelSampleRate = LSM303DLHC_ACCEL_SAMPLERATE_50;
+    m_GD20HM303DLHCAccelFsr = LSM303DLHC_ACCEL_FSR_8;
+
+    m_GD20HM303DLHCCompassSampleRate = LSM303DLHC_COMPASS_SAMPLERATE_30;
+    m_GD20HM303DLHCCompassFsr = LSM303DLHC_COMPASS_FSR_1_3;
+
+    m_imuType = RTIMU_TYPE_GD20HM303DLHC;
+    m_I2CSlaveAddress = L3GD20H_ADDRESS1;
+#endif
+
+#ifdef BNO055_28
+    m_imuType = RTIMU_TYPE_BNO055;
+    m_I2CSlaveAddress = BNO055_ADDRESS0;
+#endif
+
+#ifdef BNO055_29
+    m_imuType = RTIMU_TYPE_BNO055;
+    m_I2CSlaveAddress = BNO055_ADDRESS1;
+#endif
+
+#ifdef BMP180
+    m_pressureType = RTPRESSURE_TYPE_BMP180;
+    m_I2CPressureAddress = BMP180_ADDRESS;
+#endif
+
+#ifdef LPS25H_5c
+    m_pressureType = RTPRESSURE_TYPE_LPS25H;
+    m_I2CPressureAddress = LPS25H_ADDRESS0;
+#endif
+
+#ifdef LPS25H_5d
+    m_pressureType = RTPRESSURE_TYPE_LPS25H;
+    m_I2CPressureAddress = LPS25H_ADDRESS1;
+#endif
+
+#ifdef MS5611_76
+    m_pressureType = RTPRESSURE_TYPE_MS5611;
+    m_I2CPressureAddress = MS5611_ADDRESS0;
+#endif
+
+#ifdef MS5611_77
+    m_pressureType = RTPRESSURE_TYPE_MS5611;
+    m_I2CPressureAddress = MS5611_ADDRESS1;
+#endif
+
+}

osc32_9255/RTIMULib/RTIMUSettings.h

@@ -0,0 +1,124 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTIMUSETTINGS_H
+#define _RTIMUSETTINGS_H
+
+#include "RTMath.h"
+#include "RTIMULibDefs.h"
+
+class RTIMUSettings
+{
+public:
+    RTIMUSettings();
+
+    //  These are the local variables
+
+    int m_imuType;                                          // type code of imu in use
+    unsigned char m_I2CSlaveAddress;                        // I2C slave address of the imu
+    int m_pressureType;                                     // type code of pressure sensor in use
+    unsigned char m_I2CPressureAddress;                     // I2C slave address of the pressure sensor
+
+    //  IMU-specific vars
+
+#if defined(MPU9150_68) || defined(MPU9150_69)
+    //  MPU9150
+
+    int m_MPU9150GyroAccelSampleRate;                       // the sample rate (samples per second) for gyro and accel
+    int m_MPU9150CompassSampleRate;                         // same for the compass
+    int m_MPU9150GyroAccelLpf;                              // low pass filter code for the gyro and accel
+    int m_MPU9150GyroFsr;                                   // FSR code for the gyro
+    int m_MPU9150AccelFsr;                                  // FSR code for the accel
+#endif
+
+#if defined(MPU9250_68) || defined(MPU9250_69)
+    //  MPU9250
+
+    int m_MPU9250GyroAccelSampleRate;                       // the sample rate (samples per second) for gyro and accel
+    int m_MPU9250CompassSampleRate;                         // same for the compass
+    int m_MPU9250GyroLpf;                                   // low pass filter code for the gyro
+    int m_MPU9250AccelLpf;                                  // low pass filter code for the accel
+    int m_MPU9250GyroFsr;                                   // FSR code for the gyro
+    int m_MPU9250AccelFsr;                                  // FSR code for the accel
+#endif
+
+#if defined(LSM9DS0_6a) || defined(LSM9DS0_6b)
+    //  LSM9DS0
+
+    int m_LSM9DS0GyroSampleRate;                            // the gyro sample rate
+    int m_LSM9DS0GyroBW;                                    // the gyro bandwidth code
+    int m_LSM9DS0GyroHpf;                                   // the gyro high pass filter cutoff code
+    int m_LSM9DS0GyroFsr;                                   // the gyro full scale range
+
+    int m_LSM9DS0AccelSampleRate;                           // the accel sample rate
+    int m_LSM9DS0AccelFsr;                                  // the accel full scale range
+    int m_LSM9DS0AccelLpf;                                  // the accel low pass filter
+
+    int m_LSM9DS0CompassSampleRate;                         // the compass sample rate
+    int m_LSM9DS0CompassFsr;                                // the compass full scale range
+#endif
+
+#if defined(GD20HM303D_6a) || defined(GD20HM303D_6b)
+    int m_GD20HM303DGyroSampleRate;                         // the gyro sample rate
+    int m_GD20HM303DGyroBW;                                 // the gyro bandwidth code
+    int m_GD20HM303DGyroHpf;                                // the gyro high pass filter cutoff code
+    int m_GD20HM303DGyroFsr;                                // the gyro full scale range
+
+    int m_GD20HM303DAccelSampleRate;                        // the accel sample rate
+    int m_GD20HM303DAccelFsr;                               // the accel full scale range
+    int m_GD20HM303DAccelLpf;                               // the accel low pass filter
+
+    int m_GD20HM303DCompassSampleRate;                      // the compass sample rate
+    int m_GD20HM303DCompassFsr;                             // the compass full scale range
+#endif
+
+#if defined(GD20M303DLHC_6a) || defined(GD20M303DLHC_6b)
+    int m_GD20M303DLHCGyroSampleRate;                       // the gyro sample rate
+    int m_GD20M303DLHCGyroBW;                               // the gyro bandwidth code
+    int m_GD20M303DLHCGyroHpf;                              // the gyro high pass filter cutoff code
+    int m_GD20M303DLHCGyroFsr;                              // the gyro full scale range
+
+    int m_GD20M303DLHCAccelSampleRate;                      // the accel sample rate
+    int m_GD20M303DLHCAccelFsr;                             // the accel full scale range
+
+    int m_GD20M303DLHCCompassSampleRate;                    // the compass sample rate
+    int m_GD20M303DLHCCompassFsr;                           // the compass full scale range
+#endif
+
+#if defined(GD20HM303DLHC_6a) || defined(GD20HM303DLHC_6b)
+    int m_GD20HM303DLHCGyroSampleRate;                      // the gyro sample rate
+    int m_GD20HM303DLHCGyroBW;                              // the gyro bandwidth code
+    int m_GD20HM303DLHCGyroHpf;                             // the gyro high pass filter cutoff code
+    int m_GD20HM303DLHCGyroFsr;                             // the gyro full scale range
+
+    int m_GD20HM303DLHCAccelSampleRate;                     // the accel sample rate
+    int m_GD20HM303DLHCAccelFsr;                            // the accel full scale range
+
+    int m_GD20HM303DLHCCompassSampleRate;                   // the compass sample rate
+    int m_GD20HM303DLHCCompassFsr;                          // the compass full scale range
+#endif
+
+};
+
+#endif // _RTIMUSETTINGS_H
+

osc32_9255/RTIMULib/RTMath.cpp

@@ -0,0 +1,419 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTMath.h"
+#include <Arduino.h>
+
+#ifndef RTARDULINK_MODE
+void RTMath::display(const char *label, RTVector3& vec)
+{
+    Serial.print(label);
+    Serial.print(" x:"); Serial.print(vec.x());
+    Serial.print(" y:"); Serial.print(vec.y());
+    Serial.print(" z:"); Serial.print(vec.z());
+}
+
+void RTMath::displayDegrees(const char *label, RTVector3& vec)
+{
+    Serial.print(label);
+    Serial.print(" x:"); Serial.print(vec.x() * RTMATH_RAD_TO_DEGREE);
+    Serial.print(" y:"); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE);
+    Serial.print(" z:"); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE);
+}
+
+void RTMath::displayRollPitchYaw(const char *label, RTVector3& vec)
+{
+    Serial.print(label);
+    Serial.print(" roll:"); Serial.print(vec.x() * RTMATH_RAD_TO_DEGREE);
+    Serial.print(" pitch:"); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE);
+    Serial.print(" yaw:"); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE);
+}
+
+void RTMath::display(const char *label, RTQuaternion& quat)
+{
+    Serial.print(label);
+    Serial.print(" scalar:"); Serial.print(quat.scalar());
+    Serial.print(" x:"); Serial.print(quat.x());
+    Serial.print(" y:"); Serial.print(quat.y());
+    Serial.print(" z:"); Serial.print(quat.z());
+}
+
+RTVector3 RTMath::poseFromAccelMag(const RTVector3& accel, const RTVector3& mag)
+{
+    RTVector3 result;
+    RTQuaternion m;
+    RTQuaternion q;
+
+    accel.accelToEuler(result);
+
+//  q.fromEuler(result);
+//  since result.z() is always 0, this can be optimized a little
+
+    RTFLOAT cosX2 = cos(result.x() / 2.0f);
+    RTFLOAT sinX2 = sin(result.x() / 2.0f);
+    RTFLOAT cosY2 = cos(result.y() / 2.0f);
+    RTFLOAT sinY2 = sin(result.y() / 2.0f);
+
+    q.setScalar(cosX2 * cosY2);
+    q.setX(sinX2 * cosY2);
+    q.setY(cosX2 * sinY2);
+    q.setZ(-sinX2 * sinY2);
+//    q.normalize();
+
+    m.setScalar(0);
+    m.setX(mag.x());
+    m.setY(mag.y());
+    m.setZ(mag.z());
+
+    m = q * m * q.conjugate();
+    result.setZ(-atan2(m.y(), m.x()));
+    return result;
+}
+
+void RTMath::convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian)
+{
+    if (bigEndian) {
+        vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[0] << 8) | (uint16_t)rawData[1])) * scale);
+        vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[2] << 8) | (uint16_t)rawData[3])) * scale);
+        vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[4] << 8) | (uint16_t)rawData[5])) * scale);
+    } else {
+        vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[1] << 8) | (uint16_t)rawData[0])) * scale);
+        vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[3] << 8) | (uint16_t)rawData[2])) * scale);
+        vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[5] << 8) | (uint16_t)rawData[4])) * scale);
+     }
+}
+
+#endif // #ifndef RTARDULINK_MODE
+
+//----------------------------------------------------------
+//
+//  The RTVector3 class
+
+RTVector3::RTVector3()
+{
+    zero();
+}
+
+RTVector3::RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z)
+{
+    m_data[0] = x;
+    m_data[1] = y;
+    m_data[2] = z;
+}
+
+RTVector3& RTVector3::operator =(const RTVector3& vec)
+{
+    if (this == &vec)
+        return *this;
+
+    m_data[0] = vec.m_data[0];
+    m_data[1] = vec.m_data[1];
+    m_data[2] = vec.m_data[2];
+
+    return *this;
+}
+
+
+const RTVector3& RTVector3::operator +=(RTVector3& vec)
+{
+    for (int i = 0; i < 3; i++)
+        m_data[i] += vec.m_data[i];
+    return *this;
+}
+
+const RTVector3& RTVector3::operator -=(RTVector3& vec)
+{
+    for (int i = 0; i < 3; i++)
+        m_data[i] -= vec.m_data[i];
+    return *this;
+}
+
+void RTVector3::zero()
+{
+    for (int i = 0; i < 3; i++)
+        m_data[i] = 0;
+}
+
+#ifndef RTARDULINK_MODE
+RTFLOAT RTVector3::dotProduct(const RTVector3& a, const RTVector3& b)
+{
+    return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
+}
+
+void RTVector3::crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d)
+{
+    d.setX(a.y() * b.z() - a.z() * b.y());
+    d.setY(a.z() * b.x() - a.x() * b.z());
+    d.setZ(a.x() * b.y() - a.y() * b.x());
+}
+
+
+void RTVector3::accelToEuler(RTVector3& rollPitchYaw) const
+{
+    RTVector3 normAccel = *this;
+
+    normAccel.normalize();
+
+    rollPitchYaw.setX(atan2(normAccel.y(), normAccel.z()));
+    rollPitchYaw.setY(-atan2(normAccel.x(), sqrt(normAccel.y() * normAccel.y() + normAccel.z() * normAccel.z())));
+    rollPitchYaw.setZ(0);
+}
+
+
+void RTVector3::accelToQuaternion(RTQuaternion& qPose) const
+{
+    RTVector3 normAccel = *this;
+    RTVector3 vec;
+    RTVector3 z(0, 0, 1.0);
+
+    normAccel.normalize();
+
+    RTFLOAT angle = acos(RTVector3::dotProduct(z, normAccel));
+    RTVector3::crossProduct(normAccel, z, vec);
+    vec.normalize();
+
+    qPose.fromAngleVector(angle, vec);
+}
+
+
+void RTVector3::normalize()
+{
+    RTFLOAT length = (RTFLOAT)sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+        m_data[2] * m_data[2]);
+
+    if ((length == 0) || (length == 1))
+        return;
+
+    m_data[0] /= length;
+    m_data[1] /= length;
+    m_data[2] /= length;
+}
+
+RTFLOAT RTVector3::length()
+{
+    return (RTFLOAT)sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+            m_data[2] * m_data[2]);
+}
+
+#endif // #ifndef RTARDULINK_MODE
+
+RTFLOAT RTVector3::squareLength()
+{
+   return m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+            m_data[2] * m_data[2];
+}
+
+#ifndef RTARDULINK_MODE
+//----------------------------------------------------------
+//
+//  The RTQuaternion class
+
+RTQuaternion::RTQuaternion()
+{
+    zero();
+}
+
+RTQuaternion::RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z)
+{
+    m_data[0] = scalar;
+    m_data[1] = x;
+    m_data[2] = y;
+    m_data[3] = z;
+}
+
+RTQuaternion& RTQuaternion::operator =(const RTQuaternion& quat)
+{
+    if (this == &quat)
+        return *this;
+
+    m_data[0] = quat.m_data[0];
+    m_data[1] = quat.m_data[1];
+    m_data[2] = quat.m_data[2];
+    m_data[3] = quat.m_data[3];
+
+    return *this;
+}
+
+RTQuaternion& RTQuaternion::operator +=(const RTQuaternion& quat)
+{
+    for (int i = 0; i < 4; i++)
+        m_data[i] += quat.m_data[i];
+    return *this;
+}
+
+RTQuaternion& RTQuaternion::operator -=(const RTQuaternion& quat)
+{
+    for (int i = 0; i < 4; i++)
+        m_data[i] -= quat.m_data[i];
+    return *this;
+}
+
+RTQuaternion& RTQuaternion::operator -=(const RTFLOAT val)
+{
+    for (int i = 0; i < 4; i++)
+        m_data[i] -= val;
+    return *this;
+}
+
+RTQuaternion& RTQuaternion::operator *=(const RTQuaternion& qb)
+{
+    RTQuaternion qa;
+
+    qa = *this;
+
+    m_data[0] = qa.scalar() * qb.scalar() - qa.x() * qb.x() - qa.y() * qb.y() - qa.z() * qb.z();
+    m_data[1] = qa.scalar() * qb.x() + qa.x() * qb.scalar() + qa.y() * qb.z() - qa.z() * qb.y();
+    m_data[2] = qa.scalar() * qb.y() - qa.x() * qb.z() + qa.y() * qb.scalar() + qa.z() * qb.x();
+    m_data[3] = qa.scalar() * qb.z() + qa.x() * qb.y() - qa.y() * qb.x() + qa.z() * qb.scalar();
+
+    return *this;
+}
+
+
+RTQuaternion& RTQuaternion::operator *=(const RTFLOAT val)
+{
+    m_data[0] *= val;
+    m_data[1] *= val;
+    m_data[2] *= val;
+    m_data[3] *= val;
+
+    return *this;
+}
+
+
+const RTQuaternion RTQuaternion::operator *(const RTQuaternion& qb) const
+{
+    RTQuaternion result = *this;
+    result *= qb;
+    return result;
+}
+
+const RTQuaternion RTQuaternion::operator *(const RTFLOAT val) const
+{
+    RTQuaternion result = *this;
+    result *= val;
+    return result;
+}
+
+
+const RTQuaternion RTQuaternion::operator -(const RTQuaternion& qb) const
+{
+    RTQuaternion result = *this;
+    result -= qb;
+    return result;
+}
+
+const RTQuaternion RTQuaternion::operator -(const RTFLOAT val) const
+{
+    RTQuaternion result = *this;
+    result -= val;
+    return result;
+}
+
+
+void RTQuaternion::zero()
+{
+    for (int i = 0; i < 4; i++)
+        m_data[i] = 0;
+}
+
+void RTQuaternion::normalize()
+{
+    RTFLOAT length = sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
+            m_data[2] * m_data[2] + m_data[3] * m_data[3]);
+
+    if ((length == 0) || (length == 1))
+        return;
+
+    m_data[0] /= length;
+    m_data[1] /= length;
+    m_data[2] /= length;
+    m_data[3] /= length;
+}
+
+void RTQuaternion::toEuler(RTVector3& vec)
+{
+    vec.setX(atan2(2.0 * (m_data[2] * m_data[3] + m_data[0] * m_data[1]),
+            1 - 2.0 * (m_data[1] * m_data[1] + m_data[2] * m_data[2])));
+
+    vec.setY(asin(2.0 * (m_data[0] * m_data[2] - m_data[1] * m_data[3])));
+
+    vec.setZ(atan2(2.0 * (m_data[1] * m_data[2] + m_data[0] * m_data[3]),
+            1 - 2.0 * (m_data[2] * m_data[2] + m_data[3] * m_data[3])));
+}
+
+void RTQuaternion::fromEuler(RTVector3& vec)
+{
+    RTFLOAT cosX2 = cos(vec.x() / 2.0f);
+    RTFLOAT sinX2 = sin(vec.x() / 2.0f);
+    RTFLOAT cosY2 = cos(vec.y() / 2.0f);
+    RTFLOAT sinY2 = sin(vec.y() / 2.0f);
+    RTFLOAT cosZ2 = cos(vec.z() / 2.0f);
+    RTFLOAT sinZ2 = sin(vec.z() / 2.0f);
+
+    m_data[0] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
+    m_data[1] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
+    m_data[2] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
+    m_data[3] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
+    normalize();
+}
+
+RTQuaternion RTQuaternion::conjugate() const
+{
+    RTQuaternion q;
+    q.setScalar(m_data[0]);
+    q.setX(-m_data[1]);
+    q.setY(-m_data[2]);
+    q.setZ(-m_data[3]);
+    return q;
+}
+
+void RTQuaternion::toAngleVector(RTFLOAT& angle, RTVector3& vec)
+{
+    RTFLOAT halfTheta;
+    RTFLOAT sinHalfTheta;
+
+    halfTheta = acos(m_data[0]);
+    sinHalfTheta = sin(halfTheta);
+
+    if (sinHalfTheta == 0) {
+        vec.setX(1.0);
+        vec.setY(0);
+        vec.setZ(0);
+    } else {
+        vec.setX(m_data[1] / sinHalfTheta);
+        vec.setY(m_data[1] / sinHalfTheta);
+        vec.setZ(m_data[1] / sinHalfTheta);
+    }
+    angle = 2.0 * halfTheta;
+}
+
+void RTQuaternion::fromAngleVector(const RTFLOAT& angle, const RTVector3& vec)
+{
+    RTFLOAT sinHalfTheta = sin(angle / 2.0);
+    m_data[0] = cos(angle / 2.0);
+    m_data[1] = vec.x() * sinHalfTheta;
+    m_data[2] = vec.y() * sinHalfTheta;
+    m_data[3] = vec.z() * sinHalfTheta;
+}
+#endif // #ifndef RTARDULINK_MODE

osc32_9255/RTIMULib/RTMath.h

@@ -0,0 +1,161 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTMATH_H_
+#define _RTMATH_H_
+
+#include <math.h>
+#include <stdint.h>
+
+//  The fundamental float type
+
+#ifdef RTMATH_USE_DOUBLE
+typedef double RTFLOAT;
+#else
+typedef float RTFLOAT;
+#endif
+
+//  Useful constants
+
+#define	RTMATH_PI                   3.1415926535
+#define	RTMATH_DEGREE_TO_RAD        (M_PI / 180.0)
+#define	RTMATH_RAD_TO_DEGREE        (180.0 / M_PI)
+
+class RTVector3;
+
+#ifndef RTARDULINK_MODE
+class RTQuaternion;
+#endif
+
+class RTMath
+{
+public:
+#ifndef RTARDULINK_MODE
+    // convenient display routines
+
+    static void display(const char *label, RTVector3& vec);
+    static void displayDegrees(const char *label, RTVector3& vec);
+    static void displayRollPitchYaw(const char *label, RTVector3& vec);
+    static void display(const char *label, RTQuaternion& quat);
+
+    //  poseFromAccelMag generates pose Euler angles from measured settings
+
+    static RTVector3 poseFromAccelMag(const RTVector3& accel, const RTVector3& mag);
+
+    //  Takes signed 16 bit data from a char array and converts it to a vector of scaled RTFLOATs
+
+    static void convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian);
+
+#endif // #ifndef RTARDULINK_MODE
+};
+
+
+class RTVector3
+{
+public:
+    RTVector3();
+    RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z);
+
+    const RTVector3&  operator +=(RTVector3& vec);
+    const RTVector3&  operator -=(RTVector3& vec);
+
+    RTVector3& operator =(const RTVector3& vec);
+
+    RTFLOAT squareLength();
+    void zero();
+
+    inline RTFLOAT x() const { return m_data[0]; }
+    inline RTFLOAT y() const { return m_data[1]; }
+    inline RTFLOAT z() const { return m_data[2]; }
+    inline RTFLOAT data(const int i) const { return m_data[i]; }
+
+    inline void setX(const RTFLOAT val) { m_data[0] = val; }
+    inline void setY(const RTFLOAT val) { m_data[1] = val; }
+    inline void setZ(const RTFLOAT val) { m_data[2] = val; }
+    inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
+
+    #ifndef RTARDULINK_MODE
+    RTFLOAT length();
+    void normalize();
+
+    const char *display();
+    const char *displayDegrees();
+
+    static RTFLOAT dotProduct(const RTVector3& a, const RTVector3& b);
+    static void crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d);
+
+    void accelToEuler(RTVector3& rollPitchYaw) const;
+    void accelToQuaternion(RTQuaternion& qPose) const;
+#endif // #ifndef RTARDULINK_MODE
+
+private:
+    RTFLOAT m_data[3];
+};
+
+#ifndef RTARDULINK_MODE
+class RTQuaternion
+{
+public:
+    RTQuaternion();
+    RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z);
+
+    RTQuaternion& operator +=(const RTQuaternion& quat);
+    RTQuaternion& operator -=(const RTQuaternion& quat);
+    RTQuaternion& operator *=(const RTQuaternion& qb);
+    RTQuaternion& operator *=(const RTFLOAT val);
+    RTQuaternion& operator -=(const RTFLOAT val);
+
+    RTQuaternion& operator =(const RTQuaternion& quat);
+    const RTQuaternion operator *(const RTQuaternion& qb) const;
+    const RTQuaternion operator *(const RTFLOAT val) const;
+    const RTQuaternion operator -(const RTQuaternion& qb) const;
+    const RTQuaternion operator -(const RTFLOAT val) const;
+
+    void normalize();
+    void toEuler(RTVector3& vec);
+    void fromEuler(RTVector3& vec);
+    RTQuaternion conjugate() const;
+    void toAngleVector(RTFLOAT& angle, RTVector3& vec);
+    void fromAngleVector(const RTFLOAT& angle, const RTVector3& vec);
+
+    void zero();
+    const char *display();
+
+    inline RTFLOAT scalar() const { return m_data[0]; }
+    inline RTFLOAT x() const { return m_data[1]; }
+    inline RTFLOAT y() const { return m_data[2]; }
+    inline RTFLOAT z() const { return m_data[3]; }
+    inline RTFLOAT data(const int i) const { return m_data[i]; }
+
+    inline void setScalar(const RTFLOAT val) { m_data[0] = val; }
+    inline void setX(const RTFLOAT val) { m_data[1] = val; }
+    inline void setY(const RTFLOAT val) { m_data[2] = val; }
+    inline void setZ(const RTFLOAT val) { m_data[3] = val; }
+    inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
+
+private:
+    RTFLOAT m_data[4];
+};
+#endif // #ifndef RTARDULINK_MODE
+
+#endif /* _RTMATH_H_ */

osc32_9255/RTIMULib/RTPressure.cpp

@@ -0,0 +1,59 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTPressure.h"
+
+#if defined(BMP180)
+#include "RTPressureBMP180.h"
+#endif
+#if defined(LPS25H_5c) || defined(LPS25H_5d)
+#include "RTPressureLPS25H.h"
+#endif
+#if defined(MS5611_76) || defined(MS5611_77)
+#include "RTPressureMS5611.h"
+#endif
+
+RTPressure *RTPressure::createPressure(RTIMUSettings *settings)
+{
+#if defined(BMP180)
+    return new RTPressureBMP180(settings);
+#endif
+#if defined(LPS25H_5c) || defined(LPS25H_5d)
+    return new RTPressureLPS25H(settings);
+#endif
+#if defined(MS5611_76) || defined(MS5611_77)
+    return new RTPressureMS5611(settings);
+#endif
+    return 0;
+}
+
+
+RTPressure::RTPressure(RTIMUSettings *settings)
+{
+    m_settings = settings;
+}
+
+RTPressure::~RTPressure()
+{
+}

osc32_9255/RTIMULib/RTPressure.h

@@ -0,0 +1,56 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTPRESSURE_H
+#define	_RTPRESSURE_H
+
+#include "RTIMUSettings.h"
+#include "RTIMULibDefs.h"
+#include "RTPressureDefs.h"
+#include "I2Cdev.h"
+
+class RTPressure
+{
+public:
+    //  Pressure sensor objects should always be created with the following call
+
+    static RTPressure *createPressure(RTIMUSettings *settings);
+
+    //  Constructor/destructor
+
+    RTPressure(RTIMUSettings *settings);
+    virtual ~RTPressure();
+
+    //  These functions must be provided by sub classes
+
+    virtual const char *pressureName() = 0;                 // the name of the pressure sensor
+    virtual int pressureType() = 0;                         // the type code of the pressure sensor
+    virtual bool pressureInit() = 0;                        // set up the pressure sensor
+    virtual bool pressureRead(float &latestPressure, float &latestTemperature) = 0;   // get latest value
+
+protected:
+    RTIMUSettings *m_settings;                              // the settings object pointer
+
+};
+
+#endif // _RTPRESSURE_H

osc32_9255/RTIMULib/RTPressureBMP180.cpp

@@ -0,0 +1,227 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTPressureBMP180.h"
+
+#if defined(BMP180)
+
+RTPressureBMP180::RTPressureBMP180(RTIMUSettings *settings) : RTPressure(settings)
+{
+    m_validReadings = false;
+}
+
+RTPressureBMP180::~RTPressureBMP180()
+{
+}
+
+bool RTPressureBMP180::pressureInit()
+{
+    unsigned char result;
+    unsigned char data[22];
+
+    m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+    // check ID of chip
+
+    if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_ID, 1, &result))
+        return false;
+
+    if (result != BMP180_ID) {
+        return false;
+    }
+
+    // get calibration data
+
+    if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_AC1, 22, data))
+        return false;
+
+    m_AC1 = (int16_t)(((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+    m_AC2 = (int16_t)(((uint16_t)data[2]) << 8) + (uint16_t)data[3];
+    m_AC3 = (int16_t)(((uint16_t)data[4]) << 8) + (uint16_t)data[4];
+    m_AC4 = (((uint16_t)data[6]) << 8) + (uint16_t)data[7];
+    m_AC5 = (((uint16_t)data[8]) << 8) + (uint16_t)data[9];
+    m_AC6 = (((uint16_t)data[10]) << 8) + (uint16_t)data[11];
+    m_B1 = (int16_t)(((uint16_t)data[12]) << 8) + (uint16_t)data[13];
+    m_B2 = (int16_t)(((uint16_t)data[14]) << 8) + (uint16_t)data[15];
+    m_MB = (int16_t)(((uint16_t)data[16]) << 8) + (uint16_t)data[17];
+    m_MC = (int16_t)(((uint16_t)data[18]) << 8) + (uint16_t)data[19];
+    m_MD = (int16_t)(((uint16_t)data[20]) << 8) + (uint16_t)data[21];
+
+    m_state = BMP180_STATE_IDLE;
+    m_oss = BMP180_SCO_PRESSURECONV_UHR;
+    return true;
+}
+
+bool RTPressureBMP180::pressureRead(float &latestPressure, float &latestTemperature)
+{
+    latestPressure = 0;
+    latestTemperature = 0;
+
+    if (m_state == BMP180_STATE_IDLE) {
+        // start a temperature conversion
+        if (!I2Cdev::writeByte(m_pressureAddr, BMP180_REG_SCO, BMP180_SCO_TEMPCONV)) {
+            return false;
+        } else {
+            m_state = BMP180_STATE_TEMPERATURE;
+        }
+    }
+
+    pressureBackground();
+
+    if (m_validReadings) {
+        latestTemperature = m_temperature;
+        latestPressure = m_pressure;
+    }
+    return true;
+}
+
+
+void RTPressureBMP180::pressureBackground()
+{
+    uint8_t data[2];
+
+    switch (m_state) {
+        case BMP180_STATE_IDLE:
+        break;
+
+        case BMP180_STATE_TEMPERATURE:
+        if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_SCO, 1, data)) {
+            break;
+        }
+        if ((data[0] & 0x20) == 0x20)
+            break;                                      // conversion not finished
+        if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_RESULT, 2, data)) {
+            m_state = BMP180_STATE_IDLE;
+            break;
+        }
+        m_rawTemperature = (((uint16_t)data[0]) << 8) | (uint16_t)data[1];
+
+        data[0] = 0x34 + (m_oss << 6);
+        if (!I2Cdev::writeBytes(m_pressureAddr, BMP180_REG_SCO, 1, data)) {
+            m_state = BMP180_STATE_IDLE;
+            break;
+        }
+        m_state = BMP180_STATE_PRESSURE;
+        break;
+
+        case BMP180_STATE_PRESSURE:
+        if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_SCO, 1, data)) {
+            break;
+        }
+        if ((data[0] & 0x20) == 0x20)
+            break;                                      // conversion not finished
+        if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_RESULT, 2, data)) {
+            m_state = BMP180_STATE_IDLE;
+            break;
+        }
+        m_rawPressure = (((uint16_t)data[0]) << 8) | (uint16_t)data[1];
+
+        if (!I2Cdev::readBytes(m_pressureAddr, BMP180_REG_XLSB, 1, data)) {
+            m_state = BMP180_STATE_IDLE;
+            break;
+        }
+
+        // call this function for testing only
+        // should give T = 150 (15.0C) and pressure 6996 (699.6hPa)
+
+        // setTestData();
+
+        int32_t pressure = ((((uint32_t)(m_rawPressure)) << 8) + (uint32_t)(data[0])) >> (8 - m_oss);
+
+        m_state = BMP180_STATE_IDLE;
+
+        // calculate compensated temperature
+
+        int32_t X1 = ((m_rawTemperature - (int32_t)m_AC6) * (int32_t)m_AC5) / 32768;
+
+        if ((X1 + m_MD) == 0) {
+            break;
+        }
+
+        int32_t X2 = ((int32_t)m_MC * 2048)  / (X1 + (int32_t)m_MD);
+        int32_t B5 = X1 + X2;
+        m_temperature = (RTFLOAT)((B5 + 8) / 16) / (RTFLOAT)10;
+
+        // calculate compensated pressure
+
+        int32_t B6 = B5 - 4000;
+        //          printf("B6 = %d\n", B6);
+        X1 = (m_B2 * ((B6 * B6) / 4096)) / 2048;
+        //          printf("X1 = %d\n", X1);
+        X2 = (m_AC2 * B6) / 2048;
+        //          printf("X2 = %d\n", X2);
+        int32_t X3 = X1 + X2;
+        //          printf("X3 = %d\n", X3);
+        int32_t B3 = (((m_AC1 * 4 + X3) << m_oss) + 2) / 4;
+        //          printf("B3 = %d\n", B3);
+        X1 = (m_AC3 * B6) / 8192;
+        //          printf("X1 = %d\n", X1);
+        X2 = (m_B1 * ((B6 * B6) / 4096)) / 65536;
+        //          printf("X2 = %d\n", X2);
+        X3 = ((X1 + X2) + 2) / 4;
+        //          printf("X3 = %d\n", X3);
+        uint32_t B4 = (m_AC4 * (uint32_t)(X3 + 32768)) / 32768;
+        //          printf("B4 = %d\n", B4);
+        uint32_t B7 = (uint32_t)(pressure - B3) * (50000 >> m_oss);
+        //          printf("B7 = %d\n", B7);
+
+        int32_t p;
+        if (B7 < 0x80000000)
+        p = (B7 * 2) / B4;
+            else
+        p = (B7 / B4) * 2;
+
+        //          printf("p = %d\n", p);
+        X1 = (p / 256) * (p / 256);
+        //          printf("X1 = %d\n", X1);
+        X1 = (X1 * 3038) / 65536;
+        //          printf("X1 = %d\n", X1);
+        X2 = (-7357 * p) / 65536;
+        //          printf("X2 = %d\n", X2);
+        m_pressure = (RTFLOAT)(p + (X1 + X2 + 3791) / 16) / (RTFLOAT)100;      // the extra 100 factor is to get 1hPa units
+
+        m_validReadings = true;
+
+        // printf("UP = %d, P = %f, UT = %d, T = %f\n", m_rawPressure, m_pressure, m_rawTemperature, m_temperature);
+        break;
+    }
+}
+
+void RTPressureBMP180::setTestData()
+{
+    m_AC1 = 408;
+    m_AC2 = -72;
+    m_AC3 = -14383;
+    m_AC4 = 32741;
+    m_AC5 = 32757;
+    m_AC6 = 23153;
+    m_B1 = 6190;
+    m_B2 = 4;
+    m_MB = -32767;
+    m_MC = -8711;
+    m_MD = 2868;
+
+    m_rawTemperature = 27898;
+    m_rawPressure = 23843;
+}
+#endif

osc32_9255/RTIMULib/RTPressureBMP180.h

@@ -0,0 +1,88 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTPRESSUREBMP180_H_
+#define _RTPRESSUREBMP180_H_
+
+#include "RTPressure.h"
+
+//  State definitions
+
+#define BMP180_STATE_IDLE               0
+#define BMP180_STATE_TEMPERATURE        1
+#define BMP180_STATE_PRESSURE           2
+
+//  Conversion reg defs
+
+#define BMP180_SCO_TEMPCONV             0x2e                // temperature conversion
+#define BMP180_SCO_PRESSURECONV_ULP     0                   // ultra low power pressure conversion
+#define BMP180_SCO_PRESSURECONV_STD     1                   // standard pressure conversion
+#define BMP180_SCO_PRESSURECONV_HR      2                   // high res pressure conversion
+#define BMP180_SCO_PRESSURECONV_UHR     3                   // ultra high res pressure conversion
+
+class RTIMUSettings;
+
+class RTPressureBMP180 : public RTPressure
+{
+public:
+    RTPressureBMP180(RTIMUSettings *settings);
+    ~RTPressureBMP180();
+
+    virtual const char *pressureName() { return "BMP180"; }
+    virtual int pressureType() { return RTPRESSURE_TYPE_BMP180; }
+    virtual bool pressureInit();
+    virtual bool pressureRead(float &latestPressure, float &latestTemperature);
+
+private:
+    void pressureBackground();
+    void setTestData();
+
+    unsigned char m_pressureAddr;                           // I2C address
+    RTFLOAT m_pressure;                                     // the current pressure
+    RTFLOAT m_temperature;                                  // the current temperature
+
+    // This is the calibration data read from the sensor
+
+    int16_t m_AC1;
+    int16_t m_AC2;
+    int16_t m_AC3;
+    uint16_t m_AC4;
+    uint16_t m_AC5;
+    uint16_t m_AC6;
+    int16_t m_B1;
+    int16_t m_B2;
+    int16_t m_MB;
+    int16_t m_MC;
+    int16_t m_MD;
+
+    int m_state;
+    int m_oss;
+
+    int32_t m_rawPressure;
+    int32_t m_rawTemperature;
+
+    bool m_validReadings;
+};
+
+#endif // _RTPRESSUREBMP180_H_
+

osc32_9255/RTIMULib/RTPressureDefs.h

@@ -0,0 +1,104 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTPRESSUREDEFS_H
+#define	_RTPRESSUREDEFS_H
+
+//  Pressure sensor type codes
+
+#define RTPRESSURE_TYPE_AUTODISCOVER        0                   // audodiscover the pressure sensor
+#define RTPRESSURE_TYPE_NULL                1                   // if no physical hardware
+#define RTPRESSURE_TYPE_BMP180              2                   // BMP180
+#define RTPRESSURE_TYPE_LPS25H              3                   // LPS25H
+#define RTPRESSURE_TYPE_MS5611              4                   // MS5611
+
+//----------------------------------------------------------
+//
+//  BMP180
+
+//  BMP180 I2C Slave Addresse
+
+#define BMP180_ADDRESS              0x77
+#define BMP180_REG_ID               0xd0
+#define BMP180_ID                   0x55
+
+//	Register map
+
+#define BMP180_REG_AC1              0xaa
+#define BMP180_REG_SCO              0xf4
+#define BMP180_REG_RESULT           0xf6
+#define BMP180_REG_XLSB             0xf8
+
+//----------------------------------------------------------
+//
+//  LPS25H
+
+//  LPS25H I2C Slave Addresse
+
+#define LPS25H_ADDRESS0             0x5c
+#define LPS25H_ADDRESS1             0x5d
+#define LPS25H_REG_ID               0x0f
+#define LPS25H_ID                   0xbd
+
+//	Register map
+
+#define LPS25H_REF_P_XL             0x08
+#define LPS25H_REF_P_XH             0x09
+#define LPS25H_RES_CONF             0x10
+#define LPS25H_CTRL_REG_1           0x20
+#define LPS25H_CTRL_REG_2           0x21
+#define LPS25H_CTRL_REG_3           0x22
+#define LPS25H_CTRL_REG_4           0x23
+#define LPS25H_INT_CFG              0x24
+#define LPS25H_INT_SOURCE           0x25
+#define LPS25H_STATUS_REG           0x27
+#define LPS25H_PRESS_OUT_XL         0x28
+#define LPS25H_PRESS_OUT_L          0x29
+#define LPS25H_PRESS_OUT_H          0x2a
+#define LPS25H_TEMP_OUT_L           0x2b
+#define LPS25H_TEMP_OUT_H           0x2c
+#define LPS25H_FIFO_CTRL            0x2e
+#define LPS25H_FIFO_STATUS          0x2f
+#define LPS25H_THS_P_L              0x30
+#define LPS25H_THS_P_H              0x31
+#define LPS25H_RPDS_L               0x39
+#define LPS25H_RPDS_H               0x3a
+
+//----------------------------------------------------------
+//
+//  MS5611
+
+//  MS5611 I2C Slave Addresses
+
+#define MS5611_ADDRESS0             0x76
+#define MS5611_ADDRESS1             0x77
+
+//	commands
+
+#define MS5611_CMD_RESET            0x1e
+#define MS5611_CMD_CONV_D1          0x48
+#define MS5611_CMD_CONV_D2          0x58
+#define MS5611_CMD_PROM             0xa0
+#define MS5611_CMD_ADC              0x00
+
+#endif // _RTPRESSUREDEFS_H

osc32_9255/RTIMULib/RTPressureLPS25H.cpp

@@ -0,0 +1,90 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTPressureLPS25H.h"
+#include "RTPressureDefs.h"
+
+#if defined(LPS25H_5c) || defined(LPS25H_5d)
+
+RTPressureLPS25H::RTPressureLPS25H(RTIMUSettings *settings) : RTPressure(settings)
+{
+    m_pressureValid = false;
+    m_temperatureValid = false;
+ }
+
+RTPressureLPS25H::~RTPressureLPS25H()
+{
+}
+
+bool RTPressureLPS25H::pressureInit()
+{
+    m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+    if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_CTRL_REG_1, 0xc4))
+        return false;
+
+    if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_RES_CONF, 0x05))
+        return false;
+
+    if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_FIFO_CTRL, 0xc0))
+        return false;
+
+    if (!I2Cdev::writeByte(m_pressureAddr, LPS25H_CTRL_REG_2, 0x40))
+        return false;
+
+    return true;
+}
+
+
+bool RTPressureLPS25H::pressureRead(float &latestPressure, float &latestTemperature)
+{
+    unsigned char rawData[3];
+    unsigned char status;
+
+    latestPressure = 0;
+    latestTemperature = 0;
+
+    if (!I2Cdev::readBytes(m_pressureAddr, LPS25H_STATUS_REG, 1, &status))
+        return false;
+
+    if (status & 2) {
+        if (!I2Cdev::readBytes(m_pressureAddr, LPS25H_PRESS_OUT_XL + 0x80, 3, rawData))
+            return false;
+
+        m_pressure = (RTFLOAT)((((unsigned long)rawData[2]) << 16) | (((unsigned long)rawData[1]) << 8) | (unsigned long)rawData[0]) / (RTFLOAT)4096;
+        m_pressureValid = true;
+    }
+    if (status & 1) {
+        if (!I2Cdev::readBytes(m_pressureAddr, LPS25H_TEMP_OUT_L + 0x80, 2, rawData))
+            return false;
+
+        m_temperature = (int16_t)((((unsigned int)rawData[1]) << 8) | (unsigned int)rawData[0]) / (RTFLOAT)480 + (RTFLOAT)42.5;
+        m_temperatureValid = true;
+    }
+
+    latestPressure = m_pressure;
+    latestTemperature = m_temperature;
+
+    return true;
+}
+#endif

osc32_9255/RTIMULib/RTPressureLPS25H.h

@@ -0,0 +1,53 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTPRESSURELPS25H_H_
+#define _RTPRESSURELPS25H_H_
+
+#include "RTPressure.h"
+
+class RTIMUSettings;
+
+class RTPressureLPS25H : public RTPressure
+{
+public:
+    RTPressureLPS25H(RTIMUSettings *settings);
+    ~RTPressureLPS25H();
+
+    virtual const char *pressureName() { return "LPS25H"; }
+    virtual int pressureType() { return RTPRESSURE_TYPE_LPS25H; }
+    virtual bool pressureInit();
+    virtual bool pressureRead(float &latestPressure, float &latestTemperature);
+
+private:
+    unsigned char m_pressureAddr;                           // I2C address
+
+    RTFLOAT m_pressure;                                     // the current pressure
+    RTFLOAT m_temperature;                                  // the current temperature
+    bool m_pressureValid;
+    bool m_temperatureValid;
+
+};
+
+#endif // _RTPRESSURELPS25H_H_
+

osc32_9255/RTIMULib/RTPressureMS5611.cpp

@@ -0,0 +1,163 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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 "RTPressureMS5611.h"
+
+RTPressureMS5611::RTPressureMS5611(RTIMUSettings *settings) : RTPressure(settings)
+{
+    m_validReadings = false;
+}
+
+RTPressureMS5611::~RTPressureMS5611()
+{
+}
+
+bool RTPressureMS5611::pressureInit()
+{
+    unsigned char cmd = MS5611_CMD_PROM + 2;
+    unsigned char data[2];
+
+    m_pressureAddr = m_settings->m_I2CPressureAddress;
+
+    // get calibration data
+
+    for (int i = 0; i < 6; i++) {
+        if (!I2Cdev::readBytes(m_pressureAddr, cmd, 2, data))
+            return false;
+        m_calData[i] = (((uint16_t)data[0]) << 8) + (uint16_t)data[1];
+        // printf("Cal index: %d, data: %d\n", i, m_calData[i]);
+        cmd += 2;
+    }
+
+    m_state = MS5611_STATE_IDLE;
+    return true;
+}
+
+bool RTPressureMS5611::pressureRead(float &latestPressure, float &latestTemperature)
+{
+    if (m_state == MS5611_STATE_IDLE) {
+        // start pressure conversion
+        if (!I2Cdev::writeBytes(m_pressureAddr, MS5611_CMD_CONV_D1, 0, 0)) {
+            return false;
+        } else {
+            m_state = MS5611_STATE_PRESSURE;
+            m_timer = millis();
+        }
+    }
+
+    pressureBackground();
+
+    if (m_validReadings) {
+        latestTemperature = m_temperature;
+        latestPressure = m_pressure;
+    }
+    return true;
+}
+
+
+void RTPressureMS5611::pressureBackground()
+{
+    uint8_t data[3];
+
+    switch (m_state) {
+        case MS5611_STATE_IDLE:
+        break;
+
+        case MS5611_STATE_PRESSURE:
+        if ((millis() - m_timer) < 10)
+            break;                                          // not time yet
+        if (!I2Cdev::readBytes(m_pressureAddr, MS5611_CMD_ADC, 3, data)) {
+            break;
+        }
+        m_D1 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+
+        // start temperature conversion
+
+        if (!I2Cdev::writeBytes(m_pressureAddr, MS5611_CMD_CONV_D2, 0, 0)) {
+            break;
+        } else {
+            m_state = MS5611_STATE_TEMPERATURE;
+            m_timer = millis();
+        }
+        break;
+
+        case MS5611_STATE_TEMPERATURE:
+        if ((millis() - m_timer) < 10)
+            break;                                          // not time yet
+        if (!I2Cdev::readBytes(m_pressureAddr, MS5611_CMD_ADC, 3, data)) {
+            break;
+        }
+        m_D2 = (((uint32_t)data[0]) << 16) + (((uint32_t)data[1]) << 8) + (uint32_t)data[2];
+
+        //  call this function for testing only
+        //  should give T = 2007 (20.07C) and pressure 100009 (1000.09hPa)
+
+        // setTestData();
+
+        //  now calculate the real values
+
+        int64_t deltaT = (int32_t)m_D2 - (((int32_t)m_calData[4]) << 8);
+
+        int32_t temperature = 2000 + ((deltaT * (int64_t)m_calData[5]) >> 23); // note - this needs to be divided by 100
+
+        int64_t offset = ((int64_t)m_calData[1] << 16) + (((int64_t)m_calData[3] * deltaT) >> 7);
+        int64_t sens = ((int64_t)m_calData[0] << 15) + (((int64_t)m_calData[2] * deltaT) >> 8);
+
+        //  do second order temperature compensation
+
+        if (temperature < 2000) {
+            int64_t T2 = (deltaT * deltaT) >> 31;
+            int64_t offset2 = 5 * ((temperature - 2000) * (temperature - 2000)) / 2;
+            int64_t sens2 = offset2 / 2;
+            if (temperature < -1500) {
+                offset2 += 7 * (temperature + 1500) * (temperature + 1500);
+                sens2 += 11 * ((temperature + 1500) * (temperature + 1500)) / 2;
+            }
+            temperature -= T2;
+            offset -= offset2;
+            sens -= sens2;
+        }
+
+        m_pressure = (RTFLOAT)(((((int64_t)m_D1 * sens) >> 21) - offset) >> 15) / (RTFLOAT)100.0;
+        m_temperature = (RTFLOAT)temperature / (RTFLOAT)100;
+
+        // printf("Temp: %f, pressure: %f\n", m_temperature, m_pressure);
+
+        m_validReadings = true;
+        m_state = MS5611_STATE_IDLE;
+        break;
+    }
+}
+
+void RTPressureMS5611::setTestData()
+{
+    m_calData[0] = 40127;
+    m_calData[1] = 36924;
+    m_calData[2] = 23317;
+    m_calData[3] = 23282;
+    m_calData[4] = 33464;
+    m_calData[5] = 28312;
+
+    m_D1 = 9085466;
+    m_D2 = 8569150;
+}

osc32_9255/RTIMULib/RTPressureMS5611.h

@@ -0,0 +1,69 @@
+////////////////////////////////////////////////////////////////////////////
+//
+//  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.
+
+#ifndef _RTPRESSUREMS5611_H_
+#define _RTPRESSUREMS5611_H_
+
+#include "RTPressure.h"
+
+//  State definitions
+
+#define MS5611_STATE_IDLE               0
+#define MS5611_STATE_TEMPERATURE        1
+#define MS5611_STATE_PRESSURE           2
+
+class RTIMUSettings;
+
+class RTPressureMS5611 : public RTPressure
+{
+public:
+    RTPressureMS5611(RTIMUSettings *settings);
+    ~RTPressureMS5611();
+
+    virtual const char *pressureName() { return "MS5611"; }
+    virtual int pressureType() { return RTPRESSURE_TYPE_MS5611; }
+    virtual bool pressureInit();
+    virtual bool pressureRead(float &latestPressure, float &latestTemperature);
+
+private:
+    void pressureBackground();
+    void setTestData();
+
+    unsigned char m_pressureAddr;                           // I2C address
+    RTFLOAT m_pressure;                                     // the current pressure
+    RTFLOAT m_temperature;                                  // the current temperature
+
+    int m_state;
+
+    uint16_t m_calData[6];                                  // calibration data
+
+    uint32_t m_D1;
+    uint32_t m_D2;
+
+    long m_timer;                                           // used to time conversions
+
+    bool m_validReadings;
+};
+
+#endif // _RTPRESSUREMS5611_H_
+

osc32_9255/SLIPEncodedBluetoothSerial.cpp

@@ -0,0 +1,181 @@
+#include "SLIPEncodedBluetoothSerial.h"
+#include "BluetoothSerial.h"
+
+/*
+ CONSTRUCTOR
+ */
+//instantiate with the tranmission layer
+//use BluetoothSerial
+SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
+	serial = &s;
+	rstate = CHAR;
+}
+
+static const uint8_t eot = 0300;
+static const uint8_t slipesc = 0333;
+static const uint8_t slipescend = 0334;
+static const uint8_t slipescesc = 0335;
+/*
+ SERIAL METHODS
+ */
+bool SLIPEncodedBluetoothSerial::endofPacket()
+{
+	if(rstate == SECONDEOT)
+	{
+		rstate = CHAR; 
+		return true;
+	}
+	if (rstate==FIRSTEOT)
+	{
+        if(serial->available())
+        {
+            uint8_t c =serial->peek();
+            if(c==eot)
+            {
+                serial->read(); // throw it on the floor
+            }
+        }
+		rstate = CHAR;
+		return true;
+	}
+	return false;
+}
+int SLIPEncodedBluetoothSerial::available(){
+back:
+	int cnt = serial->available();
+	
+	if(cnt==0)
+		return 0;
+	if(rstate==CHAR)
+	{
+		uint8_t c =serial->peek();
+		if(c==slipesc)
+		{
+			rstate = SLIPESC;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		else if( c==eot)
+		{
+			rstate = FIRSTEOT;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		return 1; // we may have more but this is the only sure bet
+	}
+	else if(rstate==SLIPESC)	
+		return 1;
+	else if(rstate==FIRSTEOT)
+	{
+		if(serial->peek()==eot)
+		{
+			rstate = SECONDEOT;
+			serial->read(); // throw it on the floor
+			return 0;
+		}		
+		rstate = CHAR;
+	}else if (rstate==SECONDEOT) {
+		rstate = CHAR;
+	}
+	
+	return 0;
+		
+}
+
+//reads a byte from the buffer
+int SLIPEncodedBluetoothSerial::read(){
+back:
+	uint8_t c = serial->read();
+	if(rstate==CHAR)
+	{
+		if(c==slipesc)
+		{
+			rstate=SLIPESC;
+			goto back;
+		}	
+		else if(c==eot){
+		
+			return -1; // xxx this is an error
+		}
+		return c;
+	}
+	else
+	if(rstate==SLIPESC)
+	{
+		rstate=CHAR;
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+			else {
+				// insert some error code here
+				return -1;
+			}
+
+	}
+	else
+		return -1;
+}
+
+// as close as we can get to correct behavior
+int SLIPEncodedBluetoothSerial::peek(){
+	uint8_t c = serial->peek();
+	if(rstate==SLIPESC)
+	{
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+	}
+	return c; 
+}
+
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+
+//encode SLIP
+ void SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) {  while(size--) write(*buffer++); }
+#else
+//encode SLIP
+size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
+
+#endif
+
+void SLIPEncodedBluetoothSerial::begin(String name){
+	serial->begin(name);
+}
+//SLIP specific method which begins a transmitted packet
+void SLIPEncodedBluetoothSerial::beginPacket() { 	serial->write(eot); }
+
+//signify the end of the packet with an EOT
+void SLIPEncodedBluetoothSerial::endPacket(){
+	serial->write(eot);
+
+}
+
+void SLIPEncodedBluetoothSerial::flush(){
+	serial->flush();
+}
+

osc32_9255/SLIPEncodedBluetoothSerial.h

@@ -0,0 +1,62 @@
+/*
+Extends the Serial class to encode SLIP over serial
+*/
+
+#ifndef SLIPEncodedBluetoothSerial_h
+#define SLIPEncodedBluetoothSerial_h
+
+
+#include "Arduino.h"
+#include <Stream.h>
+#include "BluetoothSerial.h"
+
+
+class SLIPEncodedBluetoothSerial: public Stream{
+	
+private:
+	enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
+	
+	//the serial port used
+	BluetoothSerial * serial;
+
+	
+public:
+	
+	//the serial port used
+	SLIPEncodedBluetoothSerial(BluetoothSerial & );
+
+	
+	int available();
+	int read();
+	int peek();
+	void flush();
+	
+	//same as Serial.begin
+	void begin(String);
+    
+    //SLIP specific method which begins a transmitted packet
+	void beginPacket();
+	
+	//SLIP specific method which ends a transmittedpacket
+	void endPacket();
+	// SLIP specific method which indicates that an EOT was received 
+	bool endofPacket();
+	
+	
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+	void write(uint8_t b);
+    void write(const uint8_t *buffer, size_t size);
+
+#else
+	//overrides the Stream's write function to encode SLIP
+	size_t write(uint8_t b);
+    size_t write(const uint8_t *buffer, size_t size);
+
+	//using Print::write;
+#endif
+
+};
+
+
+#endif

osc32_9255/osc32_9255.ino

@@ -0,0 +1,310 @@
+// ESP32 Dev Module
+
+#include "Wire.h"
+#include "mpu9250.h"
+
+#include <OSCBundle.h>
+#include <OSCBoards.h>
+
+// POSITION CALCULATION
+#include <BasicLinearAlgebra.h>
+#include "math.h"
+
+using namespace BLA;
+
+#define SERIAL_OSC
+//#define WIFI_OSC
+#define BT_OSC
+
+#define OUTPUT_READABLE_WORLDACCEL
+
+// SERIAL
+#ifdef BOARD_HAS_USB_SERIAL
+#include <SLIPEncodedUSBSerial.h>
+SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
+#else
+#include <SLIPEncodedSerial.h>
+SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
+#endif
+
+// WIFI
+#ifdef WIFI_OSC
+#include <WiFi.h>
+  const char* ssid     = "Grajski";     // your network SSID (name of wifi network)
+  const char* password = "nedeladanes"; // your network password
+  
+  // Multicast IP / port
+  const IPAddress castIp = IPAddress(224,0,1,9);
+  const int port = 6696;
+  bool connected = false;
+
+#include <WiFiUdp.h>
+  WiFiUDP udp;
+
+  void connectToWiFi(const char * ssid, const char * pwd){
+    Serial.println("Connecting to WiFi network: " + String(ssid));
+  
+    // delete old config
+    WiFi.disconnect(true);
+    //register event handler
+    WiFi.onEvent(WiFiEvent);
+    
+    //Initiate connection
+    WiFi.begin(ssid, pwd);
+  
+    Serial.println("Waiting for WIFI connection...");
+  }
+  
+  //wifi event handler
+  void WiFiEvent(WiFiEvent_t event){
+    switch(event) {
+      case ARDUINO_EVENT_WIFI_STA_GOT_IP:
+        //When connected set 
+        Serial.print("WiFi connected! IP address: ");
+        Serial.println(WiFi.localIP());  
+        //initializes the UDP state
+        //This initializes the transfer buffer
+        udp.begin(WiFi.localIP(), port);
+        connected = true;
+        break;
+      case ARDUINO_EVENT_WIFI_STA_DISCONNECTED:
+        connected = false;
+        Serial.println("\n\n\n================\nLOST WIFI CONNECTION!\n\n\nTrying again soon...\n\n\n");
+        delay(1000);
+        connectToWiFi(ssid, password);
+        break;
+      default: break;
+    }
+  }
+#endif
+
+// Bluetooth
+#ifdef BT_OSC
+#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+  
+#include <SLIPEncodedSerial.h>
+#include "BluetoothSerial.h"
+#include "SLIPEncodedBluetoothSerial.h"
+  
+BluetoothSerial SerialBT;
+SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
+#endif
+
+// Motion sensor object
+bfs::Mpu9250 mpu(&Wire, bfs::Mpu9250::I2C_ADDR_PRIM);
+
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+bool devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+uint32_t timeOn = 0;    //                      Uptime counter for movement calculation
+Matrix<3> position;     // [x,y,z]              tracks position of device
+Matrix<3> speed;        // [x,y,z]              tracks speed of device
+Matrix<3> eulerVector;
+Matrix<3> eulerDiffVector;
+bool reset; // For quaternion calibration
+
+
+// Sem dobimo vrednosti pospeskomerja in ziroskopa
+int16_t AcX,AcY,AcZ;
+float GyX, GyY, GyZ;
+
+// Keys
+byte keys[] = {16, 17, 5, 18};
+byte pressed[] = {0, 0, 0, 0};
+byte KEYLEN = 4;
+
+
+float clamp(float value,float min,float max) {
+  return fmaxf( min, fminf(max, value));
+}
+
+
+/* OSC MSG channels */
+OSCBundle bundle;
+
+void setup() {
+  // Basic(debug) serial init
+  //  Serial.begin(115200);   // set this as high as you can reliably run on your platform
+  Serial.println("Starting up...");
+  
+  // I2C init
+  Wire.begin();
+  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
+
+#ifdef SERIAL_OSC
+  SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
+#endif
+  
+  // Keys
+  for(int i = 0; i < KEYLEN; i++) {
+    pinMode(keys[i], INPUT_PULLUP);
+  }
+   
+  // Position and speed tracking
+
+  timeOn = 0;
+  position.Fill(0);
+  speed.Fill(0);
+
+  // Start MPU
+  if (!mpu.Begin()) {
+    Serial.println("IMU initialization failed");
+    while(1) {}
+  }
+  
+  /* Set the sample rate divider */
+  if (!mpu.ConfigSrd(19)) {
+    Serial.println("Error configured SRD");
+    while(1) {}
+  }
+  
+#ifdef WIFI_OSC
+  // WIFI init
+  Serial.print("Attempting to connect to SSID: ");
+  Serial.println(ssid);
+  connectToWiFi(ssid, password);
+
+  // attempt to connect to Wifi network:
+  while (WiFi.status() != WL_CONNECTED) {
+    Serial.print(".");
+    // wait 1 second for re-trying
+    delay(1000);
+  }
+#endif
+
+#ifdef BT_OSC
+  //SerialBT.begin("wavey wind");
+  SerialBT.begin("wavey wind 2");
+#endif
+}
+
+void loop() {
+   if (mpu.Read()) {
+      Serial.print(mpu.new_imu_data());
+      Serial.print("\t");
+      Serial.print(mpu.new_mag_data());
+      Serial.print("\t");
+      Serial.print(mpu.accel_x_mps2());
+      Serial.print("\t");
+      Serial.print(mpu.accel_y_mps2());
+      Serial.print("\t");
+      Serial.print(mpu.accel_z_mps2());
+      Serial.print("\t");
+      Serial.print(mpu.gyro_x_radps());
+      Serial.print("\t");
+      Serial.print(mpu.gyro_y_radps());
+      Serial.print("\t");
+      Serial.print(mpu.gyro_z_radps());
+      Serial.print("\t");
+      Serial.print(mpu.mag_x_ut());
+      Serial.print("\t");
+      Serial.print(mpu.mag_y_ut());
+      Serial.print("\t");
+      Serial.print(mpu.mag_z_ut());
+      Serial.print("\t");
+      Serial.print(mpu.die_temp_c());
+      Serial.print("\n");
+    }
+    return;
+  
+    // Euler - rotacija
+    //eulerVector = eulerFromQuaternion(q);
+    //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
+    
+    // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
+    //bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
+
+    // Rotation diff value in euler angle
+    //eulerDiffVector = eulerFromQuaternion(diff);
+    //bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
+    
+#ifdef OUTPUT_READABLE_REALACCEL
+    // display real acceleration, adjusted to remove gravity
+
+    //AcX = aaReal.x;
+    //AcY = aaReal.y;
+    //AcZ = aaReal.z;
+#endif
+
+#ifdef OUTPUT_READABLE_WORLDACCEL
+    // display initial world-frame acceleration, adjusted to remove gravity
+    // and rotated based on known orientation from quaternion
+
+    //AcX = aaWorld.x;
+    //AcY = aaWorld.y;
+    //AcZ = aaWorld.z;
+#endif
+
+   // Calculate speed and position from accelerometer data
+   /*
+    int prevTime = timeOn;
+    timeOn = millis();
+    int elapsedTime = timeOn - prevTime;
+    Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
+    
+    //Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
+    position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
+    speed += speedGain;
+
+    bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
+    bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
+*/
+    // Accelerometer
+    //bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
+
+    // Keys held down
+    //bundle.add("/keys"); // A B C D E
+    
+    // Send keys
+    for(int i = 0; i < KEYLEN; i++) {
+      pressed[i] = !digitalRead(keys[i]);
+      bundle.getOSCMessage("/keys")->add(pressed[i]);
+    }
+
+    // Reset calibration euler?
+    if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
+      if (!reset) {
+        //cq = q.getConjugate();
+        reset = true;
+        Serial.println("Quaternion calibrate");
+      }
+    } else {
+      if (reset) {
+        reset = false;
+      }
+    }
+    
+#ifdef SERIAL_OSC
+    SLIPSerial.beginPacket();
+    bundle.send(SLIPSerial);
+    SLIPSerial.endPacket();
+#endif
+    
+  
+#ifdef WIFI_OSC
+    udp.beginPacket(castIp, port);
+    bundle.send(udp);
+    udp.endPacket();
+#endif
+
+// Some bug below, it seems
+#ifdef BT_OSC
+    SLIPBTSerial.beginPacket();
+    bundle.send(SLIPBTSerial);
+    SLIPBTSerial.endPacket();
+#endif
+
+    bundle.empty();
+  
+}

osc32_9255/quaternionFilters.ino

@@ -0,0 +1,194 @@
+
+// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
+// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
+// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
+// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
+// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
+// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
+        void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
+        {
+            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
+            float norm;
+            float hx, hy, _2bx, _2bz;
+            float s1, s2, s3, s4;
+            float qDot1, qDot2, qDot3, qDot4;
+
+            // Auxiliary variables to avoid repeated arithmetic
+            float _2q1mx;
+            float _2q1my;
+            float _2q1mz;
+            float _2q2mx;
+            float _4bx;
+            float _4bz;
+            float _2q1 = 2.0f * q1;
+            float _2q2 = 2.0f * q2;
+            float _2q3 = 2.0f * q3;
+            float _2q4 = 2.0f * q4;
+            float _2q1q3 = 2.0f * q1 * q3;
+            float _2q3q4 = 2.0f * q3 * q4;
+            float q1q1 = q1 * q1;
+            float q1q2 = q1 * q2;
+            float q1q3 = q1 * q3;
+            float q1q4 = q1 * q4;
+            float q2q2 = q2 * q2;
+            float q2q3 = q2 * q3;
+            float q2q4 = q2 * q4;
+            float q3q3 = q3 * q3;
+            float q3q4 = q3 * q4;
+            float q4q4 = q4 * q4;
+
+            // Normalise accelerometer measurement
+            norm = sqrtf(ax * ax + ay * ay + az * az);
+            if (norm == 0.0f) return; // handle NaN
+            norm = 1.0f/norm;
+            ax *= norm;
+            ay *= norm;
+            az *= norm;
+
+            // Normalise magnetometer measurement
+            norm = sqrtf(mx * mx + my * my + mz * mz);
+            if (norm == 0.0f) return; // handle NaN
+            norm = 1.0f/norm;
+            mx *= norm;
+            my *= norm;
+            mz *= norm;
+
+            // Reference direction of Earth's magnetic field
+            _2q1mx = 2.0f * q1 * mx;
+            _2q1my = 2.0f * q1 * my;
+            _2q1mz = 2.0f * q1 * mz;
+            _2q2mx = 2.0f * q2 * mx;
+            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+            _2bx = sqrtf(hx * hx + hy * hy);
+            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+            _4bx = 2.0f * _2bx;
+            _4bz = 2.0f * _2bz;
+
+            // Gradient decent algorithm corrective step
+            s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+            s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+            s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+            s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+            norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
+            norm = 1.0f/norm;
+            s1 *= norm;
+            s2 *= norm;
+            s3 *= norm;
+            s4 *= norm;
+
+            // Compute rate of change of quaternion
+            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
+            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
+            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
+            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
+
+            // Integrate to yield quaternion
+            q1 += qDot1 * deltat;
+            q2 += qDot2 * deltat;
+            q3 += qDot3 * deltat;
+            q4 += qDot4 * deltat;
+            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+            norm = 1.0f/norm;
+            q[0] = q1 * norm;
+            q[1] = q2 * norm;
+            q[2] = q3 * norm;
+            q[3] = q4 * norm;
+
+        }
+  
+  
+  
+ // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
+ // measured ones. 
+            void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
+        {
+            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
+            float norm;
+            float hx, hy, bx, bz;
+            float vx, vy, vz, wx, wy, wz;
+            float ex, ey, ez;
+            float pa, pb, pc;
+
+            // Auxiliary variables to avoid repeated arithmetic
+            float q1q1 = q1 * q1;
+            float q1q2 = q1 * q2;
+            float q1q3 = q1 * q3;
+            float q1q4 = q1 * q4;
+            float q2q2 = q2 * q2;
+            float q2q3 = q2 * q3;
+            float q2q4 = q2 * q4;
+            float q3q3 = q3 * q3;
+            float q3q4 = q3 * q4;
+            float q4q4 = q4 * q4;   
+
+            // Normalise accelerometer measurement
+            norm = sqrtf(ax * ax + ay * ay + az * az);
+            if (norm == 0.0f) return; // handle NaN
+            norm = 1.0f / norm;        // use reciprocal for division
+            ax *= norm;
+            ay *= norm;
+            az *= norm;
+
+            // Normalise magnetometer measurement
+            norm = sqrtf(mx * mx + my * my + mz * mz);
+            if (norm == 0.0f) return; // handle NaN
+            norm = 1.0f / norm;        // use reciprocal for division
+            mx *= norm;
+            my *= norm;
+            mz *= norm;
+
+            // Reference direction of Earth's magnetic field
+            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
+            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
+            bx = sqrtf((hx * hx) + (hy * hy));
+            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
+
+            // Estimated direction of gravity and magnetic field
+            vx = 2.0f * (q2q4 - q1q3);
+            vy = 2.0f * (q1q2 + q3q4);
+            vz = q1q1 - q2q2 - q3q3 + q4q4;
+            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
+            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
+            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  
+
+            // Error is cross product between estimated direction and measured direction of gravity
+            ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+            if (Ki > 0.0f)
+            {
+                eInt[0] += ex;      // accumulate integral error
+                eInt[1] += ey;
+                eInt[2] += ez;
+            }
+            else
+            {
+                eInt[0] = 0.0f;     // prevent integral wind up
+                eInt[1] = 0.0f;
+                eInt[2] = 0.0f;
+            }
+
+            // Apply feedback terms
+            gx = gx + Kp * ex + Ki * eInt[0];
+            gy = gy + Kp * ey + Ki * eInt[1];
+            gz = gz + Kp * ez + Ki * eInt[2];
+
+            // Integrate rate of change of quaternion
+            pa = q2;
+            pb = q3;
+            pc = q4;
+            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
+            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
+            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
+            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
+
+            // Normalise quaternion
+            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+            norm = 1.0f / norm;
+            q[0] = q1 * norm;
+            q[1] = q2 * norm;
+            q[2] = q3 * norm;
+            q[3] = q4 * norm;
+ 
+        }

osc32_9255_rtimulib/I2CDev/I2Cdev.h

@@ -0,0 +1,269 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
+//
+// Changelog:
+//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+//      2011-10-03 - added automatic Arduino version detection for ease of use
+//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+//      2011-08-02 - added support for 16-bit registers
+//                 - fixed incorrect Doxygen comments on some methods
+//                 - added timeout value for read operations (thanks mem @ Arduino forums)
+//      2011-07-30 - changed read/write function structures to return success or byte counts
+//                 - made all methods static for multi-device memory savings
+//      2011-07-28 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+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.
+===============================================
+*/
+
+#ifndef _I2CDEV_H_
+#define _I2CDEV_H_
+
+// -----------------------------------------------------------------------------
+// I2C interface implementation setting
+// -----------------------------------------------------------------------------
+#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
+//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_FASTWIRE
+
+// comment this out if you are using a non-optimal IDE/implementation setting
+// but want the compiler to shut up about it
+#define I2CDEV_IMPLEMENTATION_WARNINGS
+
+// -----------------------------------------------------------------------------
+// I2C interface implementation options
+// -----------------------------------------------------------------------------
+#define I2CDEV_ARDUINO_WIRE         1 // Wire object from Arduino
+#define I2CDEV_BUILTIN_NBWIRE       2 // Tweaked Wire object from Gene Knight's NBWire project
+                                      // ^^^ NBWire implementation is still buggy w/some interrupts!
+#define I2CDEV_BUILTIN_FASTWIRE     3 // FastWire object from Francesco Ferrara's project
+#define I2CDEV_I2CMASTER_LIBRARY    4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
+
+// -----------------------------------------------------------------------------
+// Arduino-style "Serial.print" debug constant (uncomment to enable)
+// -----------------------------------------------------------------------------
+//#define I2CDEV_SERIAL_DEBUG
+
+#ifdef ARDUINO
+    #if ARDUINO < 100
+        #include "WProgram.h"
+    #else
+        #include "Arduino.h"
+    #endif
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+        #include <Wire.h>
+    #endif
+    #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
+        #include <I2C.h>
+    #endif
+#endif
+
+// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
+#define I2CDEV_DEFAULT_READ_TIMEOUT     1000
+
+class I2Cdev {
+    public:
+        I2Cdev();
+        
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+};
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+    //////////////////////
+    // FastWire 0.24
+    // This is a library to help faster programs to read I2C devices.
+    // Copyright(C) 2012
+    // Francesco Ferrara
+    //////////////////////
+    
+    /* Master */
+    #define TW_START                0x08
+    #define TW_REP_START            0x10
+
+    /* Master Transmitter */
+    #define TW_MT_SLA_ACK           0x18
+    #define TW_MT_SLA_NACK          0x20
+    #define TW_MT_DATA_ACK          0x28
+    #define TW_MT_DATA_NACK         0x30
+    #define TW_MT_ARB_LOST          0x38
+
+    /* Master Receiver */
+    #define TW_MR_ARB_LOST          0x38
+    #define TW_MR_SLA_ACK           0x40
+    #define TW_MR_SLA_NACK          0x48
+    #define TW_MR_DATA_ACK          0x50
+    #define TW_MR_DATA_NACK         0x58
+
+    #define TW_OK                   0
+    #define TW_ERROR                1
+
+    class Fastwire {
+        private:
+            static boolean waitInt();
+
+        public:
+            static void setup(int khz, boolean pullup);
+            static byte beginTransmission(byte device);
+            static byte write(byte value);
+            static byte writeBuf(byte device, byte address, byte *data, byte num);
+            static byte readBuf(byte device, byte address, byte *data, byte num);
+            static void reset();
+            static byte stop();
+    };
+#endif
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
+    // NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
+    // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
+    // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
+
+    #define NBWIRE_BUFFER_LENGTH 32
+    
+    class TwoWire {
+        private:
+            static uint8_t rxBuffer[];
+            static uint8_t rxBufferIndex;
+            static uint8_t rxBufferLength;
+        
+            static uint8_t txAddress;
+            static uint8_t txBuffer[];
+            static uint8_t txBufferIndex;
+            static uint8_t txBufferLength;
+        
+            // static uint8_t transmitting;
+            static void (*user_onRequest)(void);
+            static void (*user_onReceive)(int);
+            static void onRequestService(void);
+            static void onReceiveService(uint8_t*, int);
+    
+        public:
+            TwoWire();
+            void begin();
+            void begin(uint8_t);
+            void begin(int);
+            void beginTransmission(uint8_t);
+            //void beginTransmission(int);
+            uint8_t endTransmission(uint16_t timeout=0);
+            void nbendTransmission(void (*function)(int)) ;
+            uint8_t requestFrom(uint8_t, int, uint16_t timeout=0);
+            //uint8_t requestFrom(int, int);
+            void nbrequestFrom(uint8_t, int, void (*function)(int));
+            void send(uint8_t);
+            void send(uint8_t*, uint8_t);
+            //void send(int);
+            void send(char*);
+            uint8_t available(void);
+            uint8_t receive(void);
+            void onReceive(void (*)(int));
+            void onRequest(void (*)(void));
+    };
+    
+    #define TWI_READY   0
+    #define TWI_MRX     1
+    #define TWI_MTX     2
+    #define TWI_SRX     3
+    #define TWI_STX     4
+    
+    #define TW_WRITE    0
+    #define TW_READ     1
+    
+    #define TW_MT_SLA_NACK      0x20
+    #define TW_MT_DATA_NACK     0x30
+    
+    #define CPU_FREQ            16000000L
+    #define TWI_FREQ            100000L
+    #define TWI_BUFFER_LENGTH   32
+    
+    /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
+    
+    #define TW_STATUS_MASK              (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
+    #define TW_STATUS                   (TWSR & TW_STATUS_MASK)
+    #define TW_START                    0x08
+    #define TW_REP_START                0x10
+    #define TW_MT_SLA_ACK               0x18
+    #define TW_MT_SLA_NACK              0x20
+    #define TW_MT_DATA_ACK              0x28
+    #define TW_MT_DATA_NACK             0x30
+    #define TW_MT_ARB_LOST              0x38
+    #define TW_MR_ARB_LOST              0x38
+    #define TW_MR_SLA_ACK               0x40
+    #define TW_MR_SLA_NACK              0x48
+    #define TW_MR_DATA_ACK              0x50
+    #define TW_MR_DATA_NACK             0x58
+    #define TW_ST_SLA_ACK               0xA8
+    #define TW_ST_ARB_LOST_SLA_ACK      0xB0
+    #define TW_ST_DATA_ACK              0xB8
+    #define TW_ST_DATA_NACK             0xC0
+    #define TW_ST_LAST_DATA             0xC8
+    #define TW_SR_SLA_ACK               0x60
+    #define TW_SR_ARB_LOST_SLA_ACK      0x68
+    #define TW_SR_GCALL_ACK             0x70
+    #define TW_SR_ARB_LOST_GCALL_ACK    0x78
+    #define TW_SR_DATA_ACK              0x80
+    #define TW_SR_DATA_NACK             0x88
+    #define TW_SR_GCALL_DATA_ACK        0x90
+    #define TW_SR_GCALL_DATA_NACK       0x98
+    #define TW_SR_STOP                  0xA0
+    #define TW_NO_INFO                  0xF8
+    #define TW_BUS_ERROR                0x00
+    
+    //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr))
+    //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
+    
+    #ifndef sbi // set bit
+        #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+    #endif // sbi
+    
+    #ifndef cbi // clear bit
+        #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+    #endif // cbi
+    
+    extern TwoWire Wire;
+
+#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
+
+#endif /* _I2CDEV_H_ */

osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.cpp

@@ -0,0 +1,181 @@
+#include "SLIPEncodedBluetoothSerial.h"
+#include "BluetoothSerial.h"
+
+/*
+ CONSTRUCTOR
+ */
+//instantiate with the tranmission layer
+//use BluetoothSerial
+SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
+	serial = &s;
+	rstate = CHAR;
+}
+
+static const uint8_t eot = 0300;
+static const uint8_t slipesc = 0333;
+static const uint8_t slipescend = 0334;
+static const uint8_t slipescesc = 0335;
+/*
+ SERIAL METHODS
+ */
+bool SLIPEncodedBluetoothSerial::endofPacket()
+{
+	if(rstate == SECONDEOT)
+	{
+		rstate = CHAR; 
+		return true;
+	}
+	if (rstate==FIRSTEOT)
+	{
+        if(serial->available())
+        {
+            uint8_t c =serial->peek();
+            if(c==eot)
+            {
+                serial->read(); // throw it on the floor
+            }
+        }
+		rstate = CHAR;
+		return true;
+	}
+	return false;
+}
+int SLIPEncodedBluetoothSerial::available(){
+back:
+	int cnt = serial->available();
+	
+	if(cnt==0)
+		return 0;
+	if(rstate==CHAR)
+	{
+		uint8_t c =serial->peek();
+		if(c==slipesc)
+		{
+			rstate = SLIPESC;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		else if( c==eot)
+		{
+			rstate = FIRSTEOT;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		return 1; // we may have more but this is the only sure bet
+	}
+	else if(rstate==SLIPESC)	
+		return 1;
+	else if(rstate==FIRSTEOT)
+	{
+		if(serial->peek()==eot)
+		{
+			rstate = SECONDEOT;
+			serial->read(); // throw it on the floor
+			return 0;
+		}		
+		rstate = CHAR;
+	}else if (rstate==SECONDEOT) {
+		rstate = CHAR;
+	}
+	
+	return 0;
+		
+}
+
+//reads a byte from the buffer
+int SLIPEncodedBluetoothSerial::read(){
+back:
+	uint8_t c = serial->read();
+	if(rstate==CHAR)
+	{
+		if(c==slipesc)
+		{
+			rstate=SLIPESC;
+			goto back;
+		}	
+		else if(c==eot){
+		
+			return -1; // xxx this is an error
+		}
+		return c;
+	}
+	else
+	if(rstate==SLIPESC)
+	{
+		rstate=CHAR;
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+			else {
+				// insert some error code here
+				return -1;
+			}
+
+	}
+	else
+		return -1;
+}
+
+// as close as we can get to correct behavior
+int SLIPEncodedBluetoothSerial::peek(){
+	uint8_t c = serial->peek();
+	if(rstate==SLIPESC)
+	{
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+	}
+	return c; 
+}
+
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+
+//encode SLIP
+ void SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) {  while(size--) write(*buffer++); }
+#else
+//encode SLIP
+size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
+
+#endif
+
+void SLIPEncodedBluetoothSerial::begin(String name){
+	serial->begin(name);
+}
+//SLIP specific method which begins a transmitted packet
+void SLIPEncodedBluetoothSerial::beginPacket() { 	serial->write(eot); }
+
+//signify the end of the packet with an EOT
+void SLIPEncodedBluetoothSerial::endPacket(){
+	serial->write(eot);
+
+}
+
+void SLIPEncodedBluetoothSerial::flush(){
+	serial->flush();
+}
+

osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.h

@@ -0,0 +1,62 @@
+/*
+Extends the Serial class to encode SLIP over serial
+*/
+
+#ifndef SLIPEncodedBluetoothSerial_h
+#define SLIPEncodedBluetoothSerial_h
+
+
+#include "Arduino.h"
+#include <Stream.h>
+#include "BluetoothSerial.h"
+
+
+class SLIPEncodedBluetoothSerial: public Stream{
+	
+private:
+	enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
+	
+	//the serial port used
+	BluetoothSerial * serial;
+
+	
+public:
+	
+	//the serial port used
+	SLIPEncodedBluetoothSerial(BluetoothSerial & );
+
+	
+	int available();
+	int read();
+	int peek();
+	void flush();
+	
+	//same as Serial.begin
+	void begin(String);
+    
+    //SLIP specific method which begins a transmitted packet
+	void beginPacket();
+	
+	//SLIP specific method which ends a transmittedpacket
+	void endPacket();
+	// SLIP specific method which indicates that an EOT was received 
+	bool endofPacket();
+	
+	
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+	void write(uint8_t b);
+    void write(const uint8_t *buffer, size_t size);
+
+#else
+	//overrides the Stream's write function to encode SLIP
+	size_t write(uint8_t b);
+    size_t write(const uint8_t *buffer, size_t size);
+
+	//using Print::write;
+#endif
+
+};
+
+
+#endif

osc32_9255_rtimulib/osc32_9255_rtimulib.ino

@@ -0,0 +1,191 @@
+// ESP32 Dev Module
+
+#include <Wire.h>
+
+// Parameters for device
+#define BTNAME "kegel 2"
+
+// IMU libraries
+#include "I2Cdev.h"
+#include "RTIMUSettings.h"
+#include "RTIMU.h"
+#include "RTFusionRTQF.h" 
+#include "CalLib.h"
+#include <EEPROM.h>
+
+#include <OSCBundle.h>
+#include <OSCBoards.h>
+
+#define BT_OSC
+
+#define DISPLAY_INTERVAL  5                         // interval between pose displays
+
+
+// Bluetooth
+#ifdef BT_OSC
+#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+  
+#include <SLIPEncodedSerial.h>
+#include "BluetoothSerial.h"
+#include "SLIPEncodedBluetoothSerial.h"
+  
+BluetoothSerial SerialBT;
+SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
+#endif
+
+
+// SERIAL
+#ifdef BOARD_HAS_USB_SERIAL
+#include <SLIPEncodedUSBSerial.h>
+SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
+#else
+#include <SLIPEncodedSerial.h>
+SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
+#endif
+
+// Motion sensor objects
+RTIMU *imu;                                           // the IMU object
+RTFusionRTQF fusion;                                  // the fusion object
+RTIMUSettings settings;                               // the settings object
+
+unsigned long lastDisplay;
+unsigned long lastRate;
+int sampleCount;
+RTQuaternion gravity;
+
+bool reset; // For quaternion calibration
+
+/* OSC MSG channels */
+OSCBundle bundle;
+
+void setup() {
+  int errcode;
+  // Basic(debug) serial init
+  SLIPSerial.begin(115200);
+  //Serial.begin(115200);   // set this as high as you can reliably run on your platform
+  Serial.println("Starting up...");
+  
+  // Init EEPROM based on magnet calibration size requirement
+  EEPROM.begin(512);
+  
+  // I2C init
+  Wire.begin();
+  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
+
+  // create the imu object
+  imu = RTIMU::createIMU(&settings);
+
+  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");
+  
+  // Gravity obj
+  gravity.setScalar(0);
+  gravity.setX(0);
+  gravity.setY(0);
+  gravity.setZ(1);
+/*
+  fusion.setSlerpPower(0.02);
+  fusion.setGyroEnable(true);
+  fusion.setAccelEnable(true);
+  fusion.setCompassEnable(true);
+  */
+
+  lastDisplay = lastRate = millis();
+  sampleCount = 0;
+
+#ifdef BT_OSC
+  SerialBT.begin(BTNAME);
+#endif
+}
+
+void loop() {
+  unsigned long now = millis();
+  unsigned long delta;
+  RTVector3 realAccel;
+  RTQuaternion rotatedGravity;
+  RTQuaternion fusedConjugate;
+  RTQuaternion qTemp;
+  int loopCount = 0;
+
+  // get the latest data if ready yet
+  while (imu->IMURead()) {          
+    // this flushes remaining data in case we are falling behind
+    if (++loopCount >= 10)
+      continue;
+
+    fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
+    
+    //  do gravity rotation and subtraction
+    
+    // create the conjugate of the pose
+    fusedConjugate = fusion.getFusionQPose().conjugate();
+  
+    // now do the rotation - takes two steps with qTemp as the intermediate variable
+    qTemp = gravity * fusion.getFusionQPose();
+    rotatedGravity = fusedConjugate * qTemp;
+  
+    // now adjust the measured accel and change the signs to make sense
+    realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
+    realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
+    realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
+  
+    sampleCount++;
+    if ((delta = now - lastRate) >= 1000) {
+    
+      //Serial.print("Sample rate: "); Serial.print(sampleCount);
+      if (!imu->IMUGyroBiasValid()) {
+      //  Serial.println(", calculating gyro bias");
+      } else {
+      //  Serial.println();
+      }
+      
+      sampleCount = 0;
+      lastRate = now;
+    }
+  
+    //RTMath::display("Accel:", realAccel);
+    //Serial.println();
+  
+    // Euler - rotacija
+    //eulerVector = eulerFromQuaternion(q);
+    //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
+    //bundle.add("/euler").add(fusion.getFusionPose().x()).add(fusion.getFusionPose().y()).add(fusion.getFusionPose().z());
+    // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
+    //bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
+  
+    // Rotation diff value in euler angle
+    //eulerDiffVector = eulerFromQuaternion(diff);
+    //bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
+
+    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
+      lastDisplay = now;
+      // Accelerometer
+      bundle.add("/accel").add(realAccel.x()).add(realAccel.y()).add(realAccel.z());
+      
+      // Quaternion - rotacija
+      bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z
+
+      SLIPSerial.beginPacket();
+      bundle.send(SLIPSerial);
+      SLIPSerial.endPacket();
+  
+      #ifdef BT_OSC
+        SLIPBTSerial.beginPacket();
+        bundle.send(SLIPBTSerial);
+        SLIPBTSerial.endPacket();
+      #endif
+    
+      bundle.empty();
+    }
+  }
+}

osc32final/osc32final.ino

@@ -16,7 +16,8 @@ using namespace BLA;
 //#define WIFI_OSC
 #define BT_OSC
 
-#define OUTPUT_READABLE_WORLDACCEL
+//#define OUTPUT_READABLE_WORLDACCEL
+#define OUTPUT_READABLE_REALACCEL
 
 // SERIAL
 #ifdef BOARD_HAS_USB_SERIAL
@@ -204,13 +205,13 @@ void setup() {
   // supply your own gyro offsets here, scaled for min sensitivity
   // !!! Run Zero IMU to get readings (read comments for instructions)
 
-  /* First proto (right hand, black&blue)*/
-  mpu.setXGyroOffset(76);
-  mpu.setYGyroOffset(68);
-  mpu.setZGyroOffset(10);
-  mpu.setXAccelOffset(-3527);
-  mpu.setYAccelOffset(-913);
-  mpu.setZAccelOffset(1027);
+  /* Kegel */
+  mpu.setXAccelOffset(-4011);
+  mpu.setYAccelOffset(1059);
+  mpu.setZAccelOffset(4738);
+  mpu.setXGyroOffset(86);
+  mpu.setYGyroOffset(69);
+  mpu.setZGyroOffset(-1);
 
   /* Second proto, translucent / white 
   mpu.setXGyroOffset(-3650);
@@ -220,6 +221,15 @@ void setup() {
   mpu.setYAccelOffset(-16);
   mpu.setZAccelOffset(-12);
   */
+
+  /* Third proto, D1 mini
+  mpu.setXAccelOffset(-1272);
+  mpu.setYAccelOffset(-700);
+  mpu.setZAccelOffset(1486);
+  mpu.setXGyroOffset(60);
+  mpu.setYGyroOffset(11);
+  mpu.setZGyroOffset(28);
+  */
   
   // make sure it worked (returns 0 if so)
   if (devStatus == 0) {
@@ -261,7 +271,8 @@ void setup() {
 #endif
 
 #ifdef BT_OSC
-  SerialBT.begin("wavey wind");
+  //SerialBT.begin("wavey wind");
+  SerialBT.begin("wavey wind 2");
 #endif
 }