From 9e0a0c5c8964a49bf1310f50bf38a1a97b1ad68d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Fri, 27 May 2022 14:57:30 +0200 Subject: [PATCH] MPU 9255 sketcha --- osc32_9255/ArduinoAccel.ino | 127 ++ osc32_9255/CalLib/CalLib.cpp | 95 ++ osc32_9255/CalLib/CalLib.h | 58 + osc32_9255/I2CDev/I2Cdev.cpp | 1457 +++++++++++++++++ osc32_9255/I2CDev/I2Cdev.h | 269 +++ osc32_9255/RTArduLink/RTArduLink.cpp | 231 +++ osc32_9255/RTArduLink/RTArduLink.h | 81 + osc32_9255/RTArduLink/RTArduLinkDefs.h | 199 +++ osc32_9255/RTArduLink/RTArduLinkDemoDefs.h | 66 + osc32_9255/RTArduLink/RTArduLinkHAL.cpp | 194 +++ osc32_9255/RTArduLink/RTArduLinkHAL.h | 141 ++ osc32_9255/RTArduLink/RTArduLinkUtils.cpp | 181 ++ osc32_9255/RTArduLink/RTArduLinkUtils.h | 55 + osc32_9255/RTIMULib/RTFusionRTQF.cpp | 235 +++ osc32_9255/RTIMULib/RTFusionRTQF.h | 101 ++ osc32_9255/RTIMULib/RTIMU.cpp | 357 ++++ osc32_9255/RTIMULib/RTIMU.h | 114 ++ osc32_9255/RTIMULib/RTIMUBNO055.cpp | 180 ++ osc32_9255/RTIMULib/RTIMUBNO055.h | 85 + osc32_9255/RTIMULib/RTIMUGD20HM303D.cpp | 402 +++++ osc32_9255/RTIMULib/RTIMUGD20HM303D.h | 238 +++ osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.cpp | 409 +++++ osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.h | 208 +++ osc32_9255/RTIMULib/RTIMUGD20M303DLHC.cpp | 382 +++++ osc32_9255/RTIMULib/RTIMUGD20M303DLHC.h | 204 +++ osc32_9255/RTIMULib/RTIMULSM9DS0.cpp | 363 ++++ osc32_9255/RTIMULib/RTIMULSM9DS0.h | 246 +++ osc32_9255/RTIMULib/RTIMULibDefs.h | 103 ++ osc32_9255/RTIMULib/RTIMUMPU9150.cpp | 545 ++++++ osc32_9255/RTIMULib/RTIMUMPU9150.h | 171 ++ osc32_9255/RTIMULib/RTIMUMPU9250.cpp | 493 ++++++ osc32_9255/RTIMULib/RTIMUMPU9250.h | 175 ++ osc32_9255/RTIMULib/RTIMUSettings.cpp | 308 ++++ osc32_9255/RTIMULib/RTIMUSettings.h | 124 ++ osc32_9255/RTIMULib/RTMath.cpp | 419 +++++ osc32_9255/RTIMULib/RTMath.h | 161 ++ osc32_9255/RTIMULib/RTPressure.cpp | 59 + osc32_9255/RTIMULib/RTPressure.h | 56 + osc32_9255/RTIMULib/RTPressureBMP180.cpp | 227 +++ osc32_9255/RTIMULib/RTPressureBMP180.h | 88 + osc32_9255/RTIMULib/RTPressureDefs.h | 104 ++ osc32_9255/RTIMULib/RTPressureLPS25H.cpp | 90 + osc32_9255/RTIMULib/RTPressureLPS25H.h | 53 + osc32_9255/RTIMULib/RTPressureMS5611.cpp | 163 ++ osc32_9255/RTIMULib/RTPressureMS5611.h | 69 + osc32_9255/SLIPEncodedBluetoothSerial.cpp | 181 ++ osc32_9255/SLIPEncodedBluetoothSerial.h | 62 + osc32_9255/osc32_9255.ino | 310 ++++ osc32_9255/quaternionFilters.ino | 194 +++ osc32_9255_rtimulib/I2CDev/I2Cdev.cpp | 1457 +++++++++++++++++ osc32_9255_rtimulib/I2CDev/I2Cdev.h | 269 +++ .../SLIPEncodedBluetoothSerial.cpp | 181 ++ .../SLIPEncodedBluetoothSerial.h | 62 + osc32_9255_rtimulib/osc32_9255_rtimulib.ino | 175 ++ 54 files changed, 12977 insertions(+) create mode 100644 osc32_9255/ArduinoAccel.ino create mode 100644 osc32_9255/CalLib/CalLib.cpp create mode 100644 osc32_9255/CalLib/CalLib.h create mode 100644 osc32_9255/I2CDev/I2Cdev.cpp create mode 100644 osc32_9255/I2CDev/I2Cdev.h create mode 100644 osc32_9255/RTArduLink/RTArduLink.cpp create mode 100644 osc32_9255/RTArduLink/RTArduLink.h create mode 100644 osc32_9255/RTArduLink/RTArduLinkDefs.h create mode 100644 osc32_9255/RTArduLink/RTArduLinkDemoDefs.h create mode 100644 osc32_9255/RTArduLink/RTArduLinkHAL.cpp create mode 100644 osc32_9255/RTArduLink/RTArduLinkHAL.h create mode 100644 osc32_9255/RTArduLink/RTArduLinkUtils.cpp create mode 100644 osc32_9255/RTArduLink/RTArduLinkUtils.h create mode 100644 osc32_9255/RTIMULib/RTFusionRTQF.cpp create mode 100644 osc32_9255/RTIMULib/RTFusionRTQF.h create mode 100644 osc32_9255/RTIMULib/RTIMU.cpp create mode 100644 osc32_9255/RTIMULib/RTIMU.h create mode 100644 osc32_9255/RTIMULib/RTIMUBNO055.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUBNO055.h create mode 100644 osc32_9255/RTIMULib/RTIMUGD20HM303D.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUGD20HM303D.h create mode 100644 osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.h create mode 100644 osc32_9255/RTIMULib/RTIMUGD20M303DLHC.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUGD20M303DLHC.h create mode 100644 osc32_9255/RTIMULib/RTIMULSM9DS0.cpp create mode 100644 osc32_9255/RTIMULib/RTIMULSM9DS0.h create mode 100644 osc32_9255/RTIMULib/RTIMULibDefs.h create mode 100644 osc32_9255/RTIMULib/RTIMUMPU9150.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUMPU9150.h create mode 100644 osc32_9255/RTIMULib/RTIMUMPU9250.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUMPU9250.h create mode 100644 osc32_9255/RTIMULib/RTIMUSettings.cpp create mode 100644 osc32_9255/RTIMULib/RTIMUSettings.h create mode 100644 osc32_9255/RTIMULib/RTMath.cpp create mode 100644 osc32_9255/RTIMULib/RTMath.h create mode 100644 osc32_9255/RTIMULib/RTPressure.cpp create mode 100644 osc32_9255/RTIMULib/RTPressure.h create mode 100644 osc32_9255/RTIMULib/RTPressureBMP180.cpp create mode 100644 osc32_9255/RTIMULib/RTPressureBMP180.h create mode 100644 osc32_9255/RTIMULib/RTPressureDefs.h create mode 100644 osc32_9255/RTIMULib/RTPressureLPS25H.cpp create mode 100644 osc32_9255/RTIMULib/RTPressureLPS25H.h create mode 100644 osc32_9255/RTIMULib/RTPressureMS5611.cpp create mode 100644 osc32_9255/RTIMULib/RTPressureMS5611.h create mode 100644 osc32_9255/SLIPEncodedBluetoothSerial.cpp create mode 100644 osc32_9255/SLIPEncodedBluetoothSerial.h create mode 100644 osc32_9255/osc32_9255.ino create mode 100644 osc32_9255/quaternionFilters.ino create mode 100644 osc32_9255_rtimulib/I2CDev/I2Cdev.cpp create mode 100644 osc32_9255_rtimulib/I2CDev/I2Cdev.h create mode 100644 osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.cpp create mode 100644 osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.h create mode 100644 osc32_9255_rtimulib/osc32_9255_rtimulib.ino diff --git a/osc32_9255/ArduinoAccel.ino b/osc32_9255/ArduinoAccel.ino new file mode 100644 index 0000000..1907916 --- /dev/null +++ b/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 +#include "I2Cdev.h" +#include "RTIMULib/RTIMUSettings.h" +#include "RTIMULib/RTIMU.h" +#include "RTIMULib/RTFusionRTQF.h" +#include "CalLib/CalLib.h" +#include + +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(); + } + } +} diff --git a/osc32_9255/CalLib/CalLib.cpp b/osc32_9255/CalLib/CalLib.cpp new file mode 100644 index 0000000..c958136 --- /dev/null +++ b/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 + +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 diff --git a/osc32_9255/CalLib/CalLib.h b/osc32_9255/CalLib/CalLib.h new file mode 100644 index 0000000..f8cbda0 --- /dev/null +++ b/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 + +#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_ diff --git a/osc32_9255/I2CDev/I2Cdev.cpp b/osc32_9255/I2CDev/I2Cdev.cpp new file mode 100644 index 0000000..c576c08 --- /dev/null +++ b/osc32_9255/I2CDev/I2Cdev.cpp @@ -0,0 +1,1457 @@ +// I2Cdev library collection - Main I2C device class +// Abstracts bit and byte I2C R/W functions into a convenient class +// 6/9/2012 by Jeff Rowberg +// +// 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. +=============================================== +*/ + +#include "I2Cdev.h" + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #if ARDUINO < 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO == 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO > 100 + #warning Using current Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Timeout detection (some Wire requests block forever) + #endif + #endif + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + + //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #endif + + // NBWire implementation based heavily on code by Gene Knight + // 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 + TwoWire Wire; + +#endif + +/** Default constructor. + */ +I2Cdev::I2Cdev() { +} + +/** Read a single bit from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-7) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { + uint8_t b; + uint8_t count = readByte(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read a single bit from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-15) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { + uint16_t b; + uint8_t count = readWord(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read multiple bits from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-7) + * @param length Number of bits to read (not more than 8) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { + // 01101001 read byte + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 010 masked + // -> 010 shifted + uint8_t count, b; + if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + b &= mask; + b >>= (bitStart - length + 1); + *data = b; + } + return count; +} + +/** Read multiple bits from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-15) + * @param length Number of bits to read (not more than 16) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) + */ +int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { + // 1101011001101001 read byte + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 010 masked + // -> 010 shifted + uint8_t count; + uint16_t w; + if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + w &= mask; + w >>= (bitStart - length + 1); + *data = w; + } + return count; +} + +/** Read single byte from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for byte value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { + return readBytes(devAddr, regAddr, 1, data, timeout); +} + +/** Read single word from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for word value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { + return readWords(devAddr, regAddr, 1, data, timeout); +} + +/** Read multiple bytes from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of bytes to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of bytes read (-1 indicates failure) + */ +int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" bytes from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); + if (status == 0) { + count = length; // success + } else { + count = -1; // error + } + + #endif + + // check for timeout + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** Read multiple words from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of words to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of words read (-1 indicates failure) + */ +int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" words from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.receive() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint16_t intermediate[(uint8_t)length]; + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); + if (status == 0) { + count = length; // success + for (uint8_t i = 0; i < length; i++) { + data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; + } + } else { + count = -1; // error + } + + #endif + + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** write a single bit in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-7) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { + uint8_t b; + readByte(devAddr, regAddr, &b); + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return writeByte(devAddr, regAddr, b); +} + +/** write a single bit in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-15) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { + uint16_t w; + readWord(devAddr, regAddr, &w); + w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); + return writeWord(devAddr, regAddr, w); +} + +/** Write multiple bits in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-7) + * @param length Number of bits to write (not more than 8) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { + // 010 value to write + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 00011100 mask byte + // 10101111 original value (sample) + // 10100011 original & ~mask + // 10101011 masked | value + uint8_t b; + if (readByte(devAddr, regAddr, &b) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= data; // combine data with existing byte + return writeByte(devAddr, regAddr, b); + } else { + return false; + } +} + +/** Write multiple bits in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-15) + * @param length Number of bits to write (not more than 16) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { + // 010 value to write + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 0001110000000000 mask word + // 1010111110010110 original value (sample) + // 1010001110010110 original & ~mask + // 1010101110010110 masked | value + uint16_t w; + if (readWord(devAddr, regAddr, &w) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + w &= ~(mask); // zero all important bits in existing word + w |= data; // combine data with existing word + return writeWord(devAddr, regAddr, w); + } else { + return false; + } +} + +/** Write single byte to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New byte value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { + return writeBytes(devAddr, regAddr, 1, &data); +} + +/** Write single word to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New word value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { + return writeWords(devAddr, regAddr, 1, &data); +} + +/** Write multiple bytes to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of bytes to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" bytes to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.write((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t) data[i]); + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Write multiple words to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of words to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" words to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length * 2; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t)(data[i] >> 8)); // send MSB + Wire.send((uint8_t)data[i++]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.write((uint8_t)(data[i] >> 8)); // send MSB + Wire.write((uint8_t)data[i++]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB + status = Fastwire::write((uint8_t)data[i++]); // send LSB + if (status != 0) break; + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Default timeout value for read operations. + * Set this to 0 to disable timeout detection. + */ +uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + // I2C library + ////////////////////// + // Copyright(C) 2012 + // Francesco Ferrara + // ferrara[at]libero[point]it + ////////////////////// + + /* + FastWire + - 0.24 added stop + - 0.23 added reset + + This is a library to help faster programs to read I2C devices. + Copyright(C) 2012 Francesco Ferrara + occhiobello at gmail dot com + [used by Jeff Rowberg for I2Cdevlib with permission] + */ + + boolean Fastwire::waitInt() { + int l = 250; + while (!(TWCR & (1 << TWINT)) && l-- > 0); + return l > 0; + } + + void Fastwire::setup(int khz, boolean pullup) { + TWCR = 0; + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi (PORTC bits 4 & 5) + // as per note from atmega8 manual pg167 + if (pullup) PORTC |= ((1 << 4) | (1 << 5)); + else PORTC &= ~((1 << 4) | (1 << 5)); + #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) + // activate internal pull-ups for twi (PORTC bits 0 & 1) + if (pullup) PORTC |= ((1 << 0) | (1 << 1)); + else PORTC &= ~((1 << 0) | (1 << 1)); + #else + // activate internal pull-ups for twi (PORTD bits 0 & 1) + // as per note from atmega128 manual pg204 + if (pullup) PORTD |= ((1 << 0) | (1 << 1)); + else PORTD &= ~((1 << 0) | (1 << 1)); + #endif + + TWSR = 0; // no prescaler => prescaler = 1 + TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate + TWCR = 1 << TWEN; // enable twi module, no interrupt + } + + // added by Jeff Rowberg 2013-05-07: + // Arduino Wire-style "beginTransmission" function + // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) + byte Fastwire::beginTransmission(byte device) { + byte twst, retry; + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device << 1; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + return 0; + } + + byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xFE; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 5; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 6; + + for (byte i = 0; i < num; i++) { + //Serial.print(data[i], HEX); + //Serial.print(" "); + TWDR = data[i]; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 7; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 8; + } + //Serial.print("\n"); + + return 0; + } + + byte Fastwire::write(byte value) { + byte twst; + //Serial.println(value, HEX); + TWDR = value; // send data + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 2; + return 0; + } + + byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 16; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 17; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xfe; // send device address to write + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 18; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 19; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 20; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 21; + + /***/ + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 22; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 23; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device | 0x01; // send device address with the read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 24; + twst = TWSR & 0xF8; + } while (twst == TW_MR_SLA_NACK && retry-- > 0); + if (twst != TW_MR_SLA_ACK) return 25; + + for (uint8_t i = 0; i < num; i++) { + if (i == num - 1) + TWCR = (1 << TWINT) | (1 << TWEN); + else + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); + if (!waitInt()) return 26; + twst = TWSR & 0xF8; + if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; + data[i] = TWDR; + //Serial.print(data[i], HEX); + //Serial.print(" "); + } + //Serial.print("\n"); + stop(); + + return 0; + } + + void Fastwire::reset() { + TWCR = 0; + } + + byte Fastwire::stop() { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); + if (!waitInt()) return 1; + return 0; + } +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // 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 + + /* + call this version 1.0 + + Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer + length and index are set *before* the data is actually read. The problem is that these + are variables local to the TwoWire object, and by the time we actually have read the + data, and know what the length actually is, we have no simple access to the object's + variables. The actual bytes read *is* given to the callback function, though. + + The ISR code for a slave receiver is commented out. I don't have that setup, and can't + verify it at this time. Save it for 2.0! + + The handling of the read and write processes here is much like in the demo sketch code: + the process is broken down into sequential functions, where each registers the next as a + callback, essentially. + + For example, for the Read process, twi_read00 just returns if TWI is not yet in a + ready state. When there's another interrupt, and the interface *is* ready, then it + sets up the read, starts it, and registers twi_read01 as the function to call after + the *next* interrupt. twi_read01, then, just returns if the interface is still in a + "reading" state. When the reading is done, it copies the information to the buffer, + cleans up, and calls the user-requested callback function with the actual number of + bytes read. + + The writing is similar. + + Questions, comments and problems can go to Gene@Telobot.com. + + Thumbs Up! + Gene Knight + + */ + + uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::rxBufferIndex = 0; + uint8_t TwoWire::rxBufferLength = 0; + + uint8_t TwoWire::txAddress = 0; + uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::txBufferIndex = 0; + uint8_t TwoWire::txBufferLength = 0; + + //uint8_t TwoWire::transmitting = 0; + void (*TwoWire::user_onRequest)(void); + void (*TwoWire::user_onReceive)(int); + + static volatile uint8_t twi_transmitting; + static volatile uint8_t twi_state; + static uint8_t twi_slarw; + static volatile uint8_t twi_error; + static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_masterBufferIndex; + static uint8_t twi_masterBufferLength; + static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_rxBufferIndex; + //static volatile uint8_t twi_Interrupt_Continue_Command; + static volatile uint8_t twi_Return_Value; + static volatile uint8_t twi_Done; + void (*twi_cbendTransmissionDone)(int); + void (*twi_cbreadFromDone)(int); + + void twi_init() { + // initialize state + twi_state = TWI_READY; + + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits + cbi(TWSR, TWPS1); + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register + // enable twi module, acks, and twi interrupt + + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); + + /* TWEN - TWI Enable Bit + TWIE - TWI Interrupt Enable + TWEA - TWI Enable Acknowledge Bit + TWINT - TWI Interrupt Flag + TWSTA - TWI Start Condition + */ + } + + typedef struct { + uint8_t address; + uint8_t* data; + uint8_t length; + uint8_t wait; + uint8_t i; + } twi_Write_Vars; + + twi_Write_Vars *ptwv = 0; + static void (*fNextInterruptFunction)(void) = 0; + + void twi_Finish(byte bRetVal) { + if (ptwv) { + free(ptwv); + ptwv = 0; + } + twi_Done = 0xFF; + twi_Return_Value = bRetVal; + fNextInterruptFunction = 0; + } + + uint8_t twii_WaitForDone(uint16_t timeout) { + uint32_t endMillis = millis() + timeout; + while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; + return twi_Return_Value; + } + + void twii_SetState(uint8_t ucState) { + twi_state = ucState; + } + + void twii_SetError(uint8_t ucError) { + twi_error = ucError ; + } + + void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { + twi_masterBufferIndex = 0; + twi_masterBufferLength = ucLength; + } + + void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + twi_masterBuffer[i] = pData[i]; + } + } + + void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + pData[i] = twi_masterBuffer[i]; + } + } + + void twii_SetSlaRW(uint8_t ucSlaRW) { + twi_slarw = ucSlaRW; + } + + void twii_SetStart() { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + } + + void twi_write01() { + if (TWI_MTX == twi_state) return; // blocking test + twi_transmitting = 0 ; + if (twi_error == 0xFF) + twi_Finish (0); // success + else if (twi_error == TW_MT_SLA_NACK) + twi_Finish (2); // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + twi_Finish (3); // error: data send, nack received + else + twi_Finish (4); // other twi error + if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); + return; + } + + + void twi_write00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) { + twi_Finish(1); // end write with error 1 + return; + } + twi_Done = 0x00; // show as working + twii_SetState(TWI_MTX); // to transmitting + twii_SetError(0xFF); // to No Error + twii_InitBuffer(0, ptwv -> length); // pointer and length + twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data + twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command + twii_SetStart(); // start the cycle + fNextInterruptFunction = twi_write01; // next routine + return twi_write01(); + } + + void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { + uint8_t i; + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + ptwv -> wait = wait; + fNextInterruptFunction = twi_write00; + return twi_write00(); + } + + void twi_read01() { + if (TWI_MRX == twi_state) return; // blocking test + if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; + twii_CopyFromBuf(ptwv -> data, ptwv -> length); + twi_Finish(ptwv -> length); + if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); + return; + } + + void twi_read00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return + twi_Done = 0x00; // show as working + twii_SetState(TWI_MRX); // reading + twii_SetError(0xFF); // reset error + twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length + twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command + twii_SetStart(); // start cycle + fNextInterruptFunction = twi_read01; + return twi_read01(); + } + + void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { + uint8_t i; + + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + fNextInterruptFunction = twi_read00; + return twi_read00(); + } + + void twi_reply(uint8_t ack) { + // transmit master read ready signal, with or without ack + if (ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + } else { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } + } + + void twi_stop(void) { + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while (TWCR & _BV(TWSTO)) { + continue; + } + + // update twi state + twi_state = TWI_READY; + } + + void twi_releaseBus(void) { + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; + } + + SIGNAL(TWI_vect) { + switch (TW_STATUS) { + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if (twi_masterBufferIndex < twi_masterBufferLength) { + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + } else { + twi_stop(); + } + break; + + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if (twi_masterBufferIndex < twi_masterBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver (NOT IMPLEMENTED YET) + /* + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + } else { + // otherwise nack + twi_reply(0); + } + break; + + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + twi_rxBuffer[twi_rxBufferIndex] = 0; + } + + // sends ack and stops interface for clock stretching + twi_stop(); + + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + + // if they didn't change buffer & length, initialize it + if (0 == twi_txBufferLength) { + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + + // transmit first byte from buffer, fall through + + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + + // if there is more to send, ack, otherwise nack + if (twi_txBufferIndex < twi_txBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + */ + + // all + case TW_NO_INFO: // no state information + break; + + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } + + if (fNextInterruptFunction) return fNextInterruptFunction(); + } + + TwoWire::TwoWire() { } + + void TwoWire::begin(void) { + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); + } + + void TwoWire::beginTransmission(uint8_t address) { + //beginTransmission((uint8_t)address); + + // indicate that we are transmitting + twi_transmitting = 1; + + // set address of targeted slave + txAddress = address; + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + } + + uint8_t TwoWire::endTransmission(uint16_t timeout) { + // transmit buffer (blocking) + //int8_t ret = + twi_cbendTransmissionDone = NULL; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + int8_t ret = twii_WaitForDone(timeout); + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + + // indicate that we are done transmitting + // twi_transmitting = 0; + return ret; + } + + void TwoWire::nbendTransmission(void (*function)(int)) { + twi_cbendTransmissionDone = function; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + return; + } + + void TwoWire::send(uint8_t data) { + if (twi_transmitting) { + // in master transmitter mode + // don't bother if buffer is full + if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { + return; + } + + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + + // update amount in buffer + txBufferLength = txBufferIndex; + } else { + // in slave send mode + // reply to master + //twi_transmit(&data, 1); + } + } + + uint8_t TwoWire::receive(void) { + // default to returning null char + // for people using with char strings + uint8_t value = 0; + + // get each successive byte on each call + if (rxBufferIndex < rxBufferLength) { + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; + } + + uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = NULL; + twi_readFrom(address, rxBuffer, quantity); + uint8_t read = twii_WaitForDone(timeout); + + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; + } + + void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = function; + twi_readFrom(address, rxBuffer, quantity); + //uint8_t read = twii_WaitForDone(); + + // set rx buffer iterator vars + //rxBufferIndex = 0; + //rxBufferLength = read; + + rxBufferIndex = 0; + rxBufferLength = quantity; // this is a hack + + return; //read; + } + + uint8_t TwoWire::available(void) { + return rxBufferLength - rxBufferIndex; + } + +#endif diff --git a/osc32_9255/I2CDev/I2Cdev.h b/osc32_9255/I2CDev/I2Cdev.h new file mode 100644 index 0000000..f6f8797 --- /dev/null +++ b/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 +// +// 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 + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY + #include + #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 + // 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_ */ diff --git a/osc32_9255/RTArduLink/RTArduLink.cpp b/osc32_9255/RTArduLink/RTArduLink.cpp new file mode 100644 index 0000000..cd71b2e --- /dev/null +++ b/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 + +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); +} + diff --git a/osc32_9255/RTArduLink/RTArduLink.h b/osc32_9255/RTArduLink/RTArduLink.h new file mode 100644 index 0000000..f4f90ce --- /dev/null +++ b/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 + diff --git a/osc32_9255/RTArduLink/RTArduLinkDefs.h b/osc32_9255/RTArduLink/RTArduLinkDefs.h new file mode 100644 index 0000000..0e2f654 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTArduLink/RTArduLinkDemoDefs.h b/osc32_9255/RTArduLink/RTArduLinkDemoDefs.h new file mode 100644 index 0000000..f1b4c64 --- /dev/null +++ b/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_ */ diff --git a/osc32_9255/RTArduLink/RTArduLinkHAL.cpp b/osc32_9255/RTArduLink/RTArduLinkHAL.cpp new file mode 100644 index 0000000..9a0c686 --- /dev/null +++ b/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 +#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++); +} diff --git a/osc32_9255/RTArduLink/RTArduLinkHAL.h b/osc32_9255/RTArduLink/RTArduLinkHAL.h new file mode 100644 index 0000000..99c5206 --- /dev/null +++ b/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 +#include +#include + +#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 + diff --git a/osc32_9255/RTArduLink/RTArduLinkUtils.cpp b/osc32_9255/RTArduLink/RTArduLinkUtils.cpp new file mode 100644 index 0000000..55b979a --- /dev/null +++ b/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]; +} diff --git a/osc32_9255/RTArduLink/RTArduLinkUtils.h b/osc32_9255/RTArduLink/RTArduLinkUtils.h new file mode 100644 index 0000000..41dd210 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTFusionRTQF.cpp b/osc32_9255/RTIMULib/RTFusionRTQF.cpp new file mode 100644 index 0000000..d26e262 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTFusionRTQF.h b/osc32_9255/RTIMULib/RTFusionRTQF.h new file mode 100644 index 0000000..bc65213 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMU.cpp b/osc32_9255/RTIMULib/RTIMU.cpp new file mode 100644 index 0000000..afdaf57 --- /dev/null +++ b/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; +} + + diff --git a/osc32_9255/RTIMULib/RTIMU.h b/osc32_9255/RTIMULib/RTIMU.h new file mode 100644 index 0000000..468b96a --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUBNO055.cpp b/osc32_9255/RTIMULib/RTIMUBNO055.cpp new file mode 100644 index 0000000..32f7a59 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTIMUBNO055.h b/osc32_9255/RTIMULib/RTIMUBNO055.h new file mode 100644 index 0000000..c2072ac --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUGD20HM303D.cpp b/osc32_9255/RTIMULib/RTIMUGD20HM303D.cpp new file mode 100644 index 0000000..343309a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTIMUGD20HM303D.h b/osc32_9255/RTIMULib/RTIMUGD20HM303D.h new file mode 100644 index 0000000..a1a5096 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.cpp b/osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.cpp new file mode 100644 index 0000000..e84e7c6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.h b/osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.h new file mode 100644 index 0000000..26ee591 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUGD20M303DLHC.cpp b/osc32_9255/RTIMULib/RTIMUGD20M303DLHC.cpp new file mode 100644 index 0000000..ffbe58f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTIMUGD20M303DLHC.h b/osc32_9255/RTIMULib/RTIMUGD20M303DLHC.h new file mode 100644 index 0000000..69d47ca --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMULSM9DS0.cpp b/osc32_9255/RTIMULib/RTIMULSM9DS0.cpp new file mode 100644 index 0000000..faba6f8 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMULSM9DS0.h b/osc32_9255/RTIMULib/RTIMULSM9DS0.h new file mode 100644 index 0000000..aecea8c --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMULibDefs.h b/osc32_9255/RTIMULib/RTIMULibDefs.h new file mode 100644 index 0000000..1673b01 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUMPU9150.cpp b/osc32_9255/RTIMULib/RTIMUMPU9150.cpp new file mode 100644 index 0000000..eea6d0f --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUMPU9150.h b/osc32_9255/RTIMULib/RTIMUMPU9150.h new file mode 100644 index 0000000..80cb538 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUMPU9250.cpp b/osc32_9255/RTIMULib/RTIMUMPU9250.cpp new file mode 100644 index 0000000..09bd39c --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUMPU9250.h b/osc32_9255/RTIMULib/RTIMUMPU9250.h new file mode 100644 index 0000000..9a96a47 --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTIMUSettings.cpp b/osc32_9255/RTIMULib/RTIMUSettings.cpp new file mode 100644 index 0000000..4c6cfb9 --- /dev/null +++ b/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 + +} diff --git a/osc32_9255/RTIMULib/RTIMUSettings.h b/osc32_9255/RTIMULib/RTIMUSettings.h new file mode 100644 index 0000000..01b41f3 --- /dev/null +++ b/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 + diff --git a/osc32_9255/RTIMULib/RTMath.cpp b/osc32_9255/RTIMULib/RTMath.cpp new file mode 100644 index 0000000..f418c08 --- /dev/null +++ b/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 + +#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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTMath.h b/osc32_9255/RTIMULib/RTMath.h new file mode 100644 index 0000000..808b75b --- /dev/null +++ b/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 +#include + +// 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_ */ diff --git a/osc32_9255/RTIMULib/RTPressure.cpp b/osc32_9255/RTIMULib/RTPressure.cpp new file mode 100644 index 0000000..5c49870 --- /dev/null +++ b/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() +{ +} diff --git a/osc32_9255/RTIMULib/RTPressure.h b/osc32_9255/RTIMULib/RTPressure.h new file mode 100644 index 0000000..c4897ac --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTPressureBMP180.cpp b/osc32_9255/RTIMULib/RTPressureBMP180.cpp new file mode 100644 index 0000000..e4e8c7c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTPressureBMP180.h b/osc32_9255/RTIMULib/RTPressureBMP180.h new file mode 100644 index 0000000..8588fec --- /dev/null +++ b/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_ + diff --git a/osc32_9255/RTIMULib/RTPressureDefs.h b/osc32_9255/RTIMULib/RTPressureDefs.h new file mode 100644 index 0000000..b1287db --- /dev/null +++ b/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 diff --git a/osc32_9255/RTIMULib/RTPressureLPS25H.cpp b/osc32_9255/RTIMULib/RTPressureLPS25H.cpp new file mode 100644 index 0000000..de620a0 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/osc32_9255/RTIMULib/RTPressureLPS25H.h b/osc32_9255/RTIMULib/RTPressureLPS25H.h new file mode 100644 index 0000000..fef2601 --- /dev/null +++ b/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_ + diff --git a/osc32_9255/RTIMULib/RTPressureMS5611.cpp b/osc32_9255/RTIMULib/RTPressureMS5611.cpp new file mode 100644 index 0000000..caa3549 --- /dev/null +++ b/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; +} diff --git a/osc32_9255/RTIMULib/RTPressureMS5611.h b/osc32_9255/RTIMULib/RTPressureMS5611.h new file mode 100644 index 0000000..857f2f5 --- /dev/null +++ b/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_ + diff --git a/osc32_9255/SLIPEncodedBluetoothSerial.cpp b/osc32_9255/SLIPEncodedBluetoothSerial.cpp new file mode 100644 index 0000000..36e5520 --- /dev/null +++ b/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(); +} + diff --git a/osc32_9255/SLIPEncodedBluetoothSerial.h b/osc32_9255/SLIPEncodedBluetoothSerial.h new file mode 100644 index 0000000..c7404ae --- /dev/null +++ b/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 +#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 diff --git a/osc32_9255/osc32_9255.ino b/osc32_9255/osc32_9255.ino new file mode 100644 index 0000000..c1f379a --- /dev/null +++ b/osc32_9255/osc32_9255.ino @@ -0,0 +1,310 @@ +// ESP32 Dev Module + +#include "Wire.h" +#include "mpu9250.h" + +#include +#include + +// POSITION CALCULATION +#include +#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 SLIPSerial( thisBoardsSerialUSB ); +#else +#include +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 + 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 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 +#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(); + +} diff --git a/osc32_9255/quaternionFilters.ino b/osc32_9255/quaternionFilters.ino new file mode 100644 index 0000000..2cca17a --- /dev/null +++ b/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; + + } diff --git a/osc32_9255_rtimulib/I2CDev/I2Cdev.cpp b/osc32_9255_rtimulib/I2CDev/I2Cdev.cpp new file mode 100644 index 0000000..c576c08 --- /dev/null +++ b/osc32_9255_rtimulib/I2CDev/I2Cdev.cpp @@ -0,0 +1,1457 @@ +// I2Cdev library collection - Main I2C device class +// Abstracts bit and byte I2C R/W functions into a convenient class +// 6/9/2012 by Jeff Rowberg +// +// 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. +=============================================== +*/ + +#include "I2Cdev.h" + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #if ARDUINO < 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO == 100 + #warning Using outdated Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #warning - Timeout detection (some Wire requests block forever) + #elif ARDUINO > 100 + #warning Using current Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Timeout detection (some Wire requests block forever) + #endif + #endif + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + + //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #endif + + // NBWire implementation based heavily on code by Gene Knight + // 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 + TwoWire Wire; + +#endif + +/** Default constructor. + */ +I2Cdev::I2Cdev() { +} + +/** Read a single bit from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-7) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { + uint8_t b; + uint8_t count = readByte(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read a single bit from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitNum Bit position to read (0-15) + * @param data Container for single bit value + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { + uint16_t b; + uint8_t count = readWord(devAddr, regAddr, &b, timeout); + *data = b & (1 << bitNum); + return count; +} + +/** Read multiple bits from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-7) + * @param length Number of bits to read (not more than 8) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { + // 01101001 read byte + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 010 masked + // -> 010 shifted + uint8_t count, b; + if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + b &= mask; + b >>= (bitStart - length + 1); + *data = b; + } + return count; +} + +/** Read multiple bits from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param bitStart First bit position to read (0-15) + * @param length Number of bits to read (not more than 16) + * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) + */ +int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { + // 1101011001101001 read byte + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 010 masked + // -> 010 shifted + uint8_t count; + uint16_t w; + if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + w &= mask; + w >>= (bitStart - length + 1); + *data = w; + } + return count; +} + +/** Read single byte from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for byte value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { + return readBytes(devAddr, regAddr, 1, data, timeout); +} + +/** Read single word from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to read from + * @param data Container for word value read from device + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Status of read operation (true = success) + */ +int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { + return readWords(devAddr, regAddr, 1, data, timeout); +} + +/** Read multiple bytes from an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of bytes to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of bytes read (-1 indicates failure) + */ +int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" bytes from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + + for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); + if (status == 0) { + count = length; // success + } else { + count = -1; // error + } + + #endif + + // check for timeout + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** Read multiple words from a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register regAddr to read from + * @param length Number of words to read + * @param data Buffer to store read data in + * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) + * @return Number of words read (-1 indicates failure) + */ +int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" words from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.send(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.receive() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { + Wire.beginTransmission(devAddr); + Wire.write(regAddr); + Wire.endTransmission(); + Wire.beginTransmission(devAddr); + Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = Wire.read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= Wire.read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + Wire.endTransmission(); + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint16_t intermediate[(uint8_t)length]; + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); + if (status == 0) { + count = length; // success + for (uint8_t i = 0; i < length; i++) { + data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; + } + } else { + count = -1; // error + } + + #endif + + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** write a single bit in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-7) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { + uint8_t b; + readByte(devAddr, regAddr, &b); + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return writeByte(devAddr, regAddr, b); +} + +/** write a single bit in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitNum Bit position to write (0-15) + * @param value New bit value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { + uint16_t w; + readWord(devAddr, regAddr, &w); + w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); + return writeWord(devAddr, regAddr, w); +} + +/** Write multiple bits in an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-7) + * @param length Number of bits to write (not more than 8) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { + // 010 value to write + // 76543210 bit numbers + // xxx args: bitStart=4, length=3 + // 00011100 mask byte + // 10101111 original value (sample) + // 10100011 original & ~mask + // 10101011 masked | value + uint8_t b; + if (readByte(devAddr, regAddr, &b) != 0) { + uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + b &= ~(mask); // zero all important bits in existing byte + b |= data; // combine data with existing byte + return writeByte(devAddr, regAddr, b); + } else { + return false; + } +} + +/** Write multiple bits in a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register regAddr to write to + * @param bitStart First bit position to write (0-15) + * @param length Number of bits to write (not more than 16) + * @param data Right-aligned value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { + // 010 value to write + // fedcba9876543210 bit numbers + // xxx args: bitStart=12, length=3 + // 0001110000000000 mask word + // 1010111110010110 original value (sample) + // 1010001110010110 original & ~mask + // 1010101110010110 masked | value + uint16_t w; + if (readWord(devAddr, regAddr, &w) != 0) { + uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); + data <<= (bitStart - length + 1); // shift data into correct position + data &= mask; // zero all non-important bits in data + w &= ~(mask); // zero all important bits in existing word + w |= data; // combine data with existing word + return writeWord(devAddr, regAddr, w); + } else { + return false; + } +} + +/** Write single byte to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New byte value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { + return writeBytes(devAddr, regAddr, 1, &data); +} + +/** Write single word to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr Register address to write to + * @param data New word value to write + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { + return writeWords(devAddr, regAddr, 1, &data); +} + +/** Write multiple bytes to an 8-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of bytes to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" bytes to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.write((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t) data[i]); + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Write multiple words to a 16-bit device register. + * @param devAddr I2C slave device address + * @param regAddr First register address to write to + * @param length Number of words to write + * @param data Buffer to copy new data from + * @return Status of operation (true = success) + */ +bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" words to 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + uint8_t status = 0; + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.beginTransmission(devAddr); + Wire.send(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.beginTransmission(devAddr); + Wire.write(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length * 2; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.send((uint8_t)(data[i] >> 8)); // send MSB + Wire.send((uint8_t)data[i++]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + Wire.write((uint8_t)(data[i] >> 8)); // send MSB + Wire.write((uint8_t)data[i++]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB + status = Fastwire::write((uint8_t)data[i++]); // send LSB + if (status != 0) break; + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) + status = Wire.endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Default timeout value for read operations. + * Set this to 0 to disable timeout detection. + */ +uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + // I2C library + ////////////////////// + // Copyright(C) 2012 + // Francesco Ferrara + // ferrara[at]libero[point]it + ////////////////////// + + /* + FastWire + - 0.24 added stop + - 0.23 added reset + + This is a library to help faster programs to read I2C devices. + Copyright(C) 2012 Francesco Ferrara + occhiobello at gmail dot com + [used by Jeff Rowberg for I2Cdevlib with permission] + */ + + boolean Fastwire::waitInt() { + int l = 250; + while (!(TWCR & (1 << TWINT)) && l-- > 0); + return l > 0; + } + + void Fastwire::setup(int khz, boolean pullup) { + TWCR = 0; + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi (PORTC bits 4 & 5) + // as per note from atmega8 manual pg167 + if (pullup) PORTC |= ((1 << 4) | (1 << 5)); + else PORTC &= ~((1 << 4) | (1 << 5)); + #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) + // activate internal pull-ups for twi (PORTC bits 0 & 1) + if (pullup) PORTC |= ((1 << 0) | (1 << 1)); + else PORTC &= ~((1 << 0) | (1 << 1)); + #else + // activate internal pull-ups for twi (PORTD bits 0 & 1) + // as per note from atmega128 manual pg204 + if (pullup) PORTD |= ((1 << 0) | (1 << 1)); + else PORTD &= ~((1 << 0) | (1 << 1)); + #endif + + TWSR = 0; // no prescaler => prescaler = 1 + TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate + TWCR = 1 << TWEN; // enable twi module, no interrupt + } + + // added by Jeff Rowberg 2013-05-07: + // Arduino Wire-style "beginTransmission" function + // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) + byte Fastwire::beginTransmission(byte device) { + byte twst, retry; + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device << 1; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + return 0; + } + + byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xFE; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 5; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 6; + + for (byte i = 0; i < num; i++) { + //Serial.print(data[i], HEX); + //Serial.print(" "); + TWDR = data[i]; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 7; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 8; + } + //Serial.print("\n"); + + return 0; + } + + byte Fastwire::write(byte value) { + byte twst; + //Serial.println(value, HEX); + TWDR = value; // send data + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 2; + return 0; + } + + byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 16; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 17; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xfe; // send device address to write + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 18; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 19; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 20; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 21; + + /***/ + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 22; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 23; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device | 0x01; // send device address with the read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 24; + twst = TWSR & 0xF8; + } while (twst == TW_MR_SLA_NACK && retry-- > 0); + if (twst != TW_MR_SLA_ACK) return 25; + + for (uint8_t i = 0; i < num; i++) { + if (i == num - 1) + TWCR = (1 << TWINT) | (1 << TWEN); + else + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); + if (!waitInt()) return 26; + twst = TWSR & 0xF8; + if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; + data[i] = TWDR; + //Serial.print(data[i], HEX); + //Serial.print(" "); + } + //Serial.print("\n"); + stop(); + + return 0; + } + + void Fastwire::reset() { + TWCR = 0; + } + + byte Fastwire::stop() { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); + if (!waitInt()) return 1; + return 0; + } +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // 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 + + /* + call this version 1.0 + + Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer + length and index are set *before* the data is actually read. The problem is that these + are variables local to the TwoWire object, and by the time we actually have read the + data, and know what the length actually is, we have no simple access to the object's + variables. The actual bytes read *is* given to the callback function, though. + + The ISR code for a slave receiver is commented out. I don't have that setup, and can't + verify it at this time. Save it for 2.0! + + The handling of the read and write processes here is much like in the demo sketch code: + the process is broken down into sequential functions, where each registers the next as a + callback, essentially. + + For example, for the Read process, twi_read00 just returns if TWI is not yet in a + ready state. When there's another interrupt, and the interface *is* ready, then it + sets up the read, starts it, and registers twi_read01 as the function to call after + the *next* interrupt. twi_read01, then, just returns if the interface is still in a + "reading" state. When the reading is done, it copies the information to the buffer, + cleans up, and calls the user-requested callback function with the actual number of + bytes read. + + The writing is similar. + + Questions, comments and problems can go to Gene@Telobot.com. + + Thumbs Up! + Gene Knight + + */ + + uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::rxBufferIndex = 0; + uint8_t TwoWire::rxBufferLength = 0; + + uint8_t TwoWire::txAddress = 0; + uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::txBufferIndex = 0; + uint8_t TwoWire::txBufferLength = 0; + + //uint8_t TwoWire::transmitting = 0; + void (*TwoWire::user_onRequest)(void); + void (*TwoWire::user_onReceive)(int); + + static volatile uint8_t twi_transmitting; + static volatile uint8_t twi_state; + static uint8_t twi_slarw; + static volatile uint8_t twi_error; + static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_masterBufferIndex; + static uint8_t twi_masterBufferLength; + static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_rxBufferIndex; + //static volatile uint8_t twi_Interrupt_Continue_Command; + static volatile uint8_t twi_Return_Value; + static volatile uint8_t twi_Done; + void (*twi_cbendTransmissionDone)(int); + void (*twi_cbreadFromDone)(int); + + void twi_init() { + // initialize state + twi_state = TWI_READY; + + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits + cbi(TWSR, TWPS1); + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register + // enable twi module, acks, and twi interrupt + + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); + + /* TWEN - TWI Enable Bit + TWIE - TWI Interrupt Enable + TWEA - TWI Enable Acknowledge Bit + TWINT - TWI Interrupt Flag + TWSTA - TWI Start Condition + */ + } + + typedef struct { + uint8_t address; + uint8_t* data; + uint8_t length; + uint8_t wait; + uint8_t i; + } twi_Write_Vars; + + twi_Write_Vars *ptwv = 0; + static void (*fNextInterruptFunction)(void) = 0; + + void twi_Finish(byte bRetVal) { + if (ptwv) { + free(ptwv); + ptwv = 0; + } + twi_Done = 0xFF; + twi_Return_Value = bRetVal; + fNextInterruptFunction = 0; + } + + uint8_t twii_WaitForDone(uint16_t timeout) { + uint32_t endMillis = millis() + timeout; + while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; + return twi_Return_Value; + } + + void twii_SetState(uint8_t ucState) { + twi_state = ucState; + } + + void twii_SetError(uint8_t ucError) { + twi_error = ucError ; + } + + void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { + twi_masterBufferIndex = 0; + twi_masterBufferLength = ucLength; + } + + void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + twi_masterBuffer[i] = pData[i]; + } + } + + void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + pData[i] = twi_masterBuffer[i]; + } + } + + void twii_SetSlaRW(uint8_t ucSlaRW) { + twi_slarw = ucSlaRW; + } + + void twii_SetStart() { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + } + + void twi_write01() { + if (TWI_MTX == twi_state) return; // blocking test + twi_transmitting = 0 ; + if (twi_error == 0xFF) + twi_Finish (0); // success + else if (twi_error == TW_MT_SLA_NACK) + twi_Finish (2); // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + twi_Finish (3); // error: data send, nack received + else + twi_Finish (4); // other twi error + if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); + return; + } + + + void twi_write00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) { + twi_Finish(1); // end write with error 1 + return; + } + twi_Done = 0x00; // show as working + twii_SetState(TWI_MTX); // to transmitting + twii_SetError(0xFF); // to No Error + twii_InitBuffer(0, ptwv -> length); // pointer and length + twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data + twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command + twii_SetStart(); // start the cycle + fNextInterruptFunction = twi_write01; // next routine + return twi_write01(); + } + + void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { + uint8_t i; + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + ptwv -> wait = wait; + fNextInterruptFunction = twi_write00; + return twi_write00(); + } + + void twi_read01() { + if (TWI_MRX == twi_state) return; // blocking test + if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; + twii_CopyFromBuf(ptwv -> data, ptwv -> length); + twi_Finish(ptwv -> length); + if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); + return; + } + + void twi_read00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return + twi_Done = 0x00; // show as working + twii_SetState(TWI_MRX); // reading + twii_SetError(0xFF); // reset error + twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length + twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command + twii_SetStart(); // start cycle + fNextInterruptFunction = twi_read01; + return twi_read01(); + } + + void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { + uint8_t i; + + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + fNextInterruptFunction = twi_read00; + return twi_read00(); + } + + void twi_reply(uint8_t ack) { + // transmit master read ready signal, with or without ack + if (ack){ + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + } else { + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + } + } + + void twi_stop(void) { + // send stop condition + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + + // wait for stop condition to be exectued on bus + // TWINT is not set after a stop condition! + while (TWCR & _BV(TWSTO)) { + continue; + } + + // update twi state + twi_state = TWI_READY; + } + + void twi_releaseBus(void) { + // release bus + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + + // update twi state + twi_state = TWI_READY; + } + + SIGNAL(TWI_vect) { + switch (TW_STATUS) { + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if (twi_masterBufferIndex < twi_masterBufferLength) { + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + } else { + twi_stop(); + } + break; + + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if (twi_masterBufferIndex < twi_masterBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver (NOT IMPLEMENTED YET) + /* + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + } else { + // otherwise nack + twi_reply(0); + } + break; + + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + twi_rxBuffer[twi_rxBufferIndex] = 0; + } + + // sends ack and stops interface for clock stretching + twi_stop(); + + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + + // if they didn't change buffer & length, initialize it + if (0 == twi_txBufferLength) { + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + + // transmit first byte from buffer, fall through + + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + + // if there is more to send, ack, otherwise nack + if (twi_txBufferIndex < twi_txBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + */ + + // all + case TW_NO_INFO: // no state information + break; + + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } + + if (fNextInterruptFunction) return fNextInterruptFunction(); + } + + TwoWire::TwoWire() { } + + void TwoWire::begin(void) { + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); + } + + void TwoWire::beginTransmission(uint8_t address) { + //beginTransmission((uint8_t)address); + + // indicate that we are transmitting + twi_transmitting = 1; + + // set address of targeted slave + txAddress = address; + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + } + + uint8_t TwoWire::endTransmission(uint16_t timeout) { + // transmit buffer (blocking) + //int8_t ret = + twi_cbendTransmissionDone = NULL; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + int8_t ret = twii_WaitForDone(timeout); + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + + // indicate that we are done transmitting + // twi_transmitting = 0; + return ret; + } + + void TwoWire::nbendTransmission(void (*function)(int)) { + twi_cbendTransmissionDone = function; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + return; + } + + void TwoWire::send(uint8_t data) { + if (twi_transmitting) { + // in master transmitter mode + // don't bother if buffer is full + if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { + return; + } + + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + + // update amount in buffer + txBufferLength = txBufferIndex; + } else { + // in slave send mode + // reply to master + //twi_transmit(&data, 1); + } + } + + uint8_t TwoWire::receive(void) { + // default to returning null char + // for people using with char strings + uint8_t value = 0; + + // get each successive byte on each call + if (rxBufferIndex < rxBufferLength) { + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; + } + + uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = NULL; + twi_readFrom(address, rxBuffer, quantity); + uint8_t read = twii_WaitForDone(timeout); + + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; + } + + void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = function; + twi_readFrom(address, rxBuffer, quantity); + //uint8_t read = twii_WaitForDone(); + + // set rx buffer iterator vars + //rxBufferIndex = 0; + //rxBufferLength = read; + + rxBufferIndex = 0; + rxBufferLength = quantity; // this is a hack + + return; //read; + } + + uint8_t TwoWire::available(void) { + return rxBufferLength - rxBufferIndex; + } + +#endif diff --git a/osc32_9255_rtimulib/I2CDev/I2Cdev.h b/osc32_9255_rtimulib/I2CDev/I2Cdev.h new file mode 100644 index 0000000..f6f8797 --- /dev/null +++ b/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 +// +// 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 + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY + #include + #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 + // 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_ */ diff --git a/osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.cpp b/osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.cpp new file mode 100644 index 0000000..36e5520 --- /dev/null +++ b/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(); +} + diff --git a/osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.h b/osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.h new file mode 100644 index 0000000..c7404ae --- /dev/null +++ b/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 +#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 diff --git a/osc32_9255_rtimulib/osc32_9255_rtimulib.ino b/osc32_9255_rtimulib/osc32_9255_rtimulib.ino new file mode 100644 index 0000000..edbec31 --- /dev/null +++ b/osc32_9255_rtimulib/osc32_9255_rtimulib.ino @@ -0,0 +1,175 @@ +// ESP32 Dev Module + +#include + +// 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 + +#include +#include + +#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 +#include "BluetoothSerial.h" +#include "SLIPEncodedBluetoothSerial.h" + +BluetoothSerial SerialBT; +SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT); +#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 + 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(); + + // 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 + + // 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()); + + // Some bug below, it seems + #ifdef BT_OSC + SLIPBTSerial.beginPacket(); + bundle.send(SLIPBTSerial); + SLIPBTSerial.endPacket(); + #endif + + bundle.empty(); + } + } +}