g1smo
/
pifcamp-2021
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

MPU 9255 sketcha

kegel
Jurij Podgoršek 2022-05-27 14:57:30 +02:00
parent 5e380911f7
commit 9e0a0c5c89
54 changed files with 12977 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
osc32_9255/ArduinoAccel.ino 100644
View File

@ -0,0 +1,127 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMULib/RTIMUSettings.h"
#include "RTIMULib/RTIMU.h"
#include "RTIMULib/RTFusionRTQF.h"
#include "CalLib/CalLib.h"
#include <EEPROM.h>
RTIMU *imu; // the IMU object
RTFusionRTQF fusion; // the fusion object
RTIMUSettings settings; // the settings object
// DISPLAY_INTERVAL sets the rate at which results are displayed
#define DISPLAY_INTERVAL 100 // interval between pose displays
// SERIAL_PORT_SPEED defines the speed to use for the debug serial port
#define SERIAL_PORT_SPEED 115200
unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
RTQuaternion gravity;
void setup()
{
int errcode;
Serial.begin(SERIAL_PORT_SPEED);
Wire.begin();
imu = RTIMU::createIMU(&settings); // create the imu object
Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
if ((errcode = imu->IMUInit()) < 0) {
Serial.print("Failed to init IMU: "); Serial.println(errcode);
}
if (imu->getCalibrationValid())
Serial.println("Using compass calibration");
else
Serial.println("No valid compass calibration data");
lastDisplay = lastRate = millis();
sampleCount = 0;
gravity.setScalar(0);
gravity.setX(0);
gravity.setY(0);
gravity.setZ(1);
}
void loop()
{
unsigned long now = millis();
unsigned long delta;
RTVector3 realAccel;
RTQuaternion rotatedGravity;
RTQuaternion fusedConjugate;
RTQuaternion qTemp;
int loopCount = 0;
while (imu->IMURead()) { // get the latest data if ready yet
// this flushes remaining data in case we are falling behind
if (++loopCount >= 10)
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
// do gravity rotation and subtraction
// create the conjugate of the pose
fusedConjugate = fusion.getFusionQPose().conjugate();
// now do the rotation - takes two steps with qTemp as the intermediate variable
qTemp = gravity * fusion.getFusionQPose();
rotatedGravity = fusedConjugate * qTemp;
// now adjust the measured accel and change the signs to make sense
realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
sampleCount++;
if ((delta = now - lastRate) >= 1000) {
Serial.print("Sample rate: "); Serial.print(sampleCount);
if (!imu->IMUGyroBiasValid())
Serial.println(", calculating gyro bias");
else
Serial.println();
sampleCount = 0;
lastRate = now;
}
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
RTMath::display("Accel:", realAccel);
Serial.println();
}
}
}

95
osc32_9255/CalLib/CalLib.cpp 100644
View File

@ -0,0 +1,95 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "CalLib.h"
#ifdef __SAM3X8E__
// Due version
#include "DueFlash.h"
DueFlash flash;
void calLibErase(byte device)
{
uint32_t data = 0;
flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, &data, 1); // just destroy the valid byte
}
void calLibWrite(byte device, CALLIB_DATA *calData)
{
calData->validL = CALLIB_DATA_VALID_LOW;
calData->validH = CALLIB_DATA_VALID_HIGH;
flash.write(CALLIB_START + sizeof(CALLIB_DATA) * device, (uint32_t *)calData, sizeof(CALLIB_DATA) / 4);
}
boolean calLibRead(byte device, CALLIB_DATA *calData)
{
memcpy(calData, CALLIB_START + sizeof(CALLIB_DATA) * device, sizeof(CALLIB_DATA));
return calData->valid == CALLIB_DATA_VALID;
}
#else
// AVR version
#include <EEPROM.h>
void calLibErase(byte device)
{
EEPROM.write(sizeof(CALLIB_DATA) * device, 0); // just destroy the valid byte
}
void calLibWrite(byte device, CALLIB_DATA *calData)
{
byte *ptr = (byte *)calData;
byte length = sizeof(CALLIB_DATA);
int eeprom = sizeof(CALLIB_DATA) * device;
calData->validL = CALLIB_DATA_VALID_LOW;
calData->validH = CALLIB_DATA_VALID_HIGH;
for (byte i = 0; i < length; i++)
EEPROM.write(eeprom + i, *ptr++);
}
boolean calLibRead(byte device, CALLIB_DATA *calData)
{
byte *ptr = (byte *)calData;
byte length = sizeof(CALLIB_DATA);
int eeprom = sizeof(CALLIB_DATA) * device;
calData->magValid = false;
if ((EEPROM.read(eeprom) != CALLIB_DATA_VALID_LOW) ||
(EEPROM.read(eeprom + 1) != CALLIB_DATA_VALID_HIGH)) {
return false; // invalid data
}
for (byte i = 0; i < length; i++)
*ptr++ = EEPROM.read(eeprom + i);
return true;
}
#endif

58
osc32_9255/CalLib/CalLib.h 100644
View File

@ -0,0 +1,58 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#ifndef _CALLIB_H_
#define _CALLIB_H_
#include <Arduino.h>
#define CALLIB_DATA_VALID_LOW 0xfc // pattern to detect valid config - low byte
#define CALLIB_DATA_VALID_HIGH 0x15 // pattern to detect valid config - high byte
#ifdef __SAM3X8E__
#define CALLIB_START ((uint32_t *)(IFLASH1_ADDR + IFLASH1_SIZE - IFLASH1_PAGE_SIZE))
#endif
typedef struct
{
unsigned char validL; // should contain the valid pattern if a good config
unsigned char validH; // should contain the valid pattern if a good config
unsigned char magValid; // true if data valid
unsigned char pad;
float magMin[3]; // min values
float magMax[3]; // max values
} CALLIB_DATA;
// calLibErase() erases any current data in the EEPROM
void calLibErase(byte device);
// calLibWrite() writes new data to the EEPROM
void calLibWrite(byte device, CALLIB_DATA * calData);
// calLibRead() reads existing data and returns true if valid else false in not.
boolean calLibRead(byte device, CALLIB_DATA * calData);
#endif // _CALLIB_H_

1457
osc32_9255/I2CDev/I2Cdev.cpp 100644
View File

File diff suppressed because it is too large Load Diff

269
osc32_9255/I2CDev/I2Cdev.h 100644
View File

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

231
osc32_9255/RTArduLink/RTArduLink.cpp 100644
View File

@ -0,0 +1,231 @@
///////////////////////////////////////////////////////////
//
// This file is part of RTArduLink
//
// Copyright (c) 2014-2015 richards-tech
//
// Permission is hereby granted, free of charge,
// to any person obtaining a copy of
// this software and associated documentation files
// (the "Software"), to deal in the Software without
// restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute,
// sublicense, and/or sell copies of the Software, and
// to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice
// shall be included in all copies or substantial portions
// of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF
// ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
// TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
// CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
// IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
#include "RTArduLink.h"
#include "RTArduLinkHAL.h"
#include "RTArduLinkUtils.h"
#include <string.h>
RTArduLink::RTArduLink()
{
}
RTArduLink::~RTArduLink()
{
}
void RTArduLink::begin(const char *identitySuffix)
{
RTARDULINK_PORT *portInfo;
if (!RTArduLinkHALEEPROMValid())
RTArduLinkHALEEPROMDefault();
m_identitySuffix = identitySuffix;
// now set up host and subsystem ports based on EEPROM configuration
for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++) {
portInfo = m_ports + i;
portInfo->index = i;
portInfo->inUse = RTArduLinkHALConfigurePort(&(portInfo->portHAL), i);
RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer));
}
m_hostPort = m_ports;
}
void RTArduLink::background()
{
unsigned char index;
RTARDULINK_PORT *portInfo;
for (index = 0; index < RTARDULINKHAL_MAX_PORTS; index++) {
portInfo = m_ports + index;
if (!portInfo->inUse)
continue;
while (RTArduLinkHALPortAvailable(&(portInfo->portHAL))) {
if (!RTArduLinkReassemble(&(portInfo->RXFrame), RTArduLinkHALPortRead(&(portInfo->portHAL)))) {
sendDebugMessage("Reassembly error");
} else {
if (portInfo->RXFrame.complete) {
processReceivedMessage(portInfo);
RTArduLinkRXFrameInit(&(portInfo->RXFrame), &(portInfo->RXFrameBuffer));
}
}
}
}
}
void RTArduLink::processReceivedMessage(RTARDULINK_PORT *portInfo)
{
RTARDULINK_MESSAGE *message; // a pointer to the message part of the frame
unsigned int address;
message = &(portInfo->RXFrameBuffer.message); // get the message pointer
address = RTArduLinkConvertUC2ToUInt(message->messageAddress);
switch (portInfo->index) {
case RTARDULINK_HOST_PORT:
processHostMessage(); // came from this upstream link
return;
case RTARDULINK_DAISY_PORT: // came from dasiy chain port
if (address != RTARDULINK_HOST_PORT) // true if it came from a daisy chained subsystem, not a directly connected subsystem
RTArduLinkConvertIntToUC2(address + RTARDULINKHAL_MAX_PORTS, message->messageAddress);
else
RTArduLinkConvertIntToUC2(RTARDULINK_DAISY_PORT, message->messageAddress);
break;
default:
RTArduLinkConvertIntToUC2(address + portInfo->index, message->messageAddress);
break;
}
// if get here, need to forward to host port
sendFrame(m_hostPort, &(portInfo->RXFrameBuffer), portInfo->RXFrameBuffer.messageLength);
}
void RTArduLink::processHostMessage()
{
RTARDULINK_MESSAGE *message; // a pointer to the message part of the frame
int identityLength;
int suffixLength;
unsigned int address;
message = &(m_hostPort->RXFrameBuffer.message); // get the message pointer
address = RTArduLinkConvertUC2ToUInt(message->messageAddress);
if (address == RTARDULINK_BROADCAST_ADDRESS) { // need to forward to downstream ports also
for (int i = RTARDULINK_HOST_PORT + 1; i < RTARDULINKHAL_MAX_PORTS; i++) {
if (m_ports[i].inUse)
sendFrame(m_ports + i, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
}
}
if ((address == RTARDULINK_MY_ADDRESS) || (address == RTARDULINK_BROADCAST_ADDRESS)) { // it's for me
switch (message->messageType)
{
case RTARDULINK_MESSAGE_POLL:
case RTARDULINK_MESSAGE_ECHO:
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength); // just send the frame back as received
break;
case RTARDULINK_MESSAGE_IDENTITY:
identityLength = strlen(RTArduLinkHALConfig.identity);
suffixLength = strlen(m_identitySuffix);
memcpy(message->data, RTArduLinkHALConfig.identity, identityLength + 1); // copy in identity
if ((identityLength + suffixLength) < RTARDULINK_DATA_MAX_LEN - 1) {
memcpy(message->data + identityLength, m_identitySuffix, suffixLength + 1); // copy in suffix
} else {
suffixLength = 0;
}
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
message->data[RTARDULINK_DATA_MAX_LEN - 1] = 0; // make sure zero terminated if it was truncated
sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + identityLength + suffixLength + 1);
break;
default:
if (message->messageType < RTARDULINK_MESSAGE_CUSTOM) { // illegal code
message->data[0] = RTARDULINK_RESPONSE_ILLEGAL_COMMAND;
message->data[1] = message->messageType; // this is the offending code
message->messageType = RTARDULINK_MESSAGE_ERROR;
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, message->messageAddress);
sendFrame(m_hostPort, &(m_hostPort->RXFrameBuffer), RTARDULINK_MESSAGE_HEADER_LEN + 2);
break;
}
processCustomMessage(message->messageType, message->messageParam, message->data,
m_hostPort->RXFrameBuffer.messageLength - RTARDULINK_MESSAGE_HEADER_LEN); // see if anyone wants to process it
break;
}
return;
}
if (address >= RTARDULINKHAL_MAX_PORTS) { // need to pass it to the first subsystem
if (!m_ports[RTARDULINK_DAISY_PORT].inUse)
return; // there is no daisy chain port
RTArduLinkConvertIntToUC2(address - RTARDULINKHAL_MAX_PORTS, message->messageAddress); // adjust the address
sendFrame(m_ports +RTARDULINK_DAISY_PORT, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
return;
}
// if get here, needs to go to a local subsystem port
if (m_ports[address].inUse) {
RTArduLinkConvertIntToUC2(0, message->messageAddress); // indicates that the target should process it
sendFrame(m_ports + address, &(m_hostPort->RXFrameBuffer), m_hostPort->RXFrameBuffer.messageLength);
}
}
void RTArduLink::sendDebugMessage(const char *debugMessage)
{
RTARDULINK_FRAME frame;
int stringLength;
stringLength = strlen(debugMessage);
if (stringLength >= RTARDULINK_DATA_MAX_LEN)
stringLength = RTARDULINK_DATA_MAX_LEN-1;
memcpy(frame.message.data, debugMessage, stringLength);
frame.message.data[stringLength] = 0;
frame.message.messageType = RTARDULINK_MESSAGE_DEBUG;
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress);
sendFrame(m_hostPort, &frame, RTARDULINK_MESSAGE_HEADER_LEN + stringLength + 1);
}
void RTArduLink::sendMessage(unsigned char messageType, unsigned char messageParam, unsigned char *data, int length)
{
RTARDULINK_FRAME frame;
RTArduLinkConvertIntToUC2(RTARDULINK_MY_ADDRESS, frame.message.messageAddress);
frame.message.messageType = messageType;
frame.message.messageParam = messageParam;
if (length > RTARDULINK_DATA_MAX_LEN)
length = RTARDULINK_DATA_MAX_LEN;
memcpy(frame.message.data, data, length);
sendFrame(m_hostPort, &frame, length + RTARDULINK_MESSAGE_HEADER_LEN);
}
void RTArduLink::sendFrame(RTARDULINK_PORT *portInfo, RTARDULINK_FRAME *frame, int length)
{
frame->sync0 = RTARDULINK_MESSAGE_SYNC0;
frame->sync1 = RTARDULINK_MESSAGE_SYNC1;
frame->messageLength = length; // set length
RTArduLinkSetChecksum(frame); // compute checksum
RTArduLinkHALPortWrite(&(portInfo->portHAL), (unsigned char *)frame, frame->messageLength + RTARDULINK_FRAME_HEADER_LEN);
}

81
osc32_9255/RTArduLink/RTArduLink.h 100644
View File

@ -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

199
osc32_9255/RTArduLink/RTArduLinkDefs.h 100644
View File

@ -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

66
osc32_9255/RTArduLink/RTArduLinkDemoDefs.h 100644
View File

@ -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_ */

194
osc32_9255/RTArduLink/RTArduLinkHAL.cpp 100644
View File

@ -0,0 +1,194 @@
///////////////////////////////////////////////////////////
//
// This file is part of RTArduLink
//
// Copyright (c) 2014-2015 richards-tech
//
// Permission is hereby granted, free of charge,
// to any person obtaining a copy of
// this software and associated documentation files
// (the "Software"), to deal in the Software without
// restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute,
// sublicense, and/or sell copies of the Software, and
// to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice
// shall be included in all copies or substantial portions
// of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF
// ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
// TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
// CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
// IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
#include <string.h>
#include "RTArduLinkHAL.h"
//----------------------------------------------------------
//
// Arduino HAL
// The global config structure
RTARDULINKHAL_EEPROM RTArduLinkHALConfig;
bool RTArduLinkHALAddHardwarePort(RTARDULINKHAL_PORT *port, long portSpeed, unsigned char hardwarePort);
// Port speed map array
unsigned long RTArduLinkHALSpeedMap[] = {0, 9600, 19200, 38400, 57600, 115200};
bool RTArduLinkHALConfigurePort(RTARDULINKHAL_PORT *port, int portIndex)
{
if (RTArduLinkHALConfig.portSpeed[portIndex] == RTARDULINK_PORT_SPEED_OFF)
return false; // port is not enabled
return RTArduLinkHALAddHardwarePort(port, RTArduLinkHALSpeedMap[RTArduLinkHALConfig.portSpeed[portIndex]],
RTArduLinkHALConfig.hardwarePort[portIndex]);
}
int RTArduLinkHALPortAvailable(RTARDULINKHAL_PORT *port)
{
return port->serialPort->available();
}
unsigned char RTArduLinkHALPortRead(RTARDULINKHAL_PORT *port)
{
return port->serialPort->read();
}
void RTArduLinkHALPortWrite(RTARDULINKHAL_PORT *port, unsigned char *data, unsigned char length)
{
port->serialPort->write(data, length);
}
bool RTArduLinkHALAddHardwarePort(RTARDULINKHAL_PORT *port, long portSpeed, unsigned char hardwarePort)
{
HardwareSerial *hardPort;
switch (hardwarePort) {
case 0:
#if defined(USBCON)
/* Leonardo support */
hardPort = &Serial1;
#else
hardPort = &Serial;
#endif
break;
case 1:
#if defined(UBRR1H)
hardPort = &Serial1;
#else
return false;
#endif
break;
case 2:
#if defined(UBRR2H)
hardPort = &Serial2;
#else
return false;
#endif
break;
case 3:
#if defined(UBRR3H)
hardPort = &Serial3;
#else
return false;
#endif
break;
default:
return false;
}
port->serialPort = hardPort;
hardPort->begin(portSpeed); // start the port
return true;
}
bool RTArduLinkHALEEPROMValid()
{
RTArduLinkHALEEPROMRead(); // see what it really is
return (RTArduLinkHALConfig.sig0 == RTARDULINKHAL_SIG0) &&
(RTArduLinkHALConfig.sig1 == RTARDULINKHAL_SIG1);
}
void RTArduLinkHALEEPROMDisplay()
{
Serial.println();
if ((RTArduLinkHALConfig.sig0 != RTARDULINKHAL_SIG0) ||
(RTArduLinkHALConfig.sig1 != RTARDULINKHAL_SIG1)) {
Serial.println("Invalid config");
return;
}
Serial.print("Identity: ");
Serial.println(RTArduLinkHALConfig.identity);
for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++)
RTArduLinkHALEEPROMDisplayPort(i, true);
}
void RTArduLinkHALEEPROMDisplayPort(int index, bool suppress)
{
if (suppress && (RTArduLinkHALConfig.portSpeed[index] == RTARDULINK_PORT_SPEED_OFF))
return;
Serial.print("Port index ");
Serial.print(index);
Serial.print(" speed=");
Serial.print(RTArduLinkHALConfig.portSpeed[index]);
Serial.print(", ");
Serial.print("hardware port number=");
Serial.println(RTArduLinkHALConfig.hardwarePort[index]);
}
void RTArduLinkHALEEPROMDefault()
{
RTArduLinkHALConfig.sig0 = RTARDULINKHAL_SIG0; // set to valid signature
RTArduLinkHALConfig.sig1 = RTARDULINKHAL_SIG1;
strcpy(RTArduLinkHALConfig.identity, "RTArduLink_Arduino");
RTArduLinkHALConfig.portSpeed[0] = RTARDULINK_PORT_SPEED_115200;
for (int i = 1; i < RTARDULINKHAL_MAX_PORTS; i++)
RTArduLinkHALConfig.portSpeed[i] = RTARDULINK_PORT_SPEED_OFF;
for (int i = 0; i < RTARDULINKHAL_MAX_PORTS; i++)
RTArduLinkHALConfig.hardwarePort[i] = i;
RTArduLinkHALEEPROMWrite();
}
void RTArduLinkHALEEPROMRead()
{
unsigned char *data;
data = (unsigned char *)&RTArduLinkHALConfig;
for (int i = 0; i < (int)sizeof(RTARDULINKHAL_EEPROM); i++)
*data++ = EEPROM.read(i + RTARDULINKHAL_EEPROM_OFFSET);
}
void RTArduLinkHALEEPROMWrite()
{
unsigned char *data;
data = (unsigned char *)&RTArduLinkHALConfig;
for (int i = 0; i < (int)sizeof(RTARDULINKHAL_EEPROM); i++)
EEPROM.write(i + RTARDULINKHAL_EEPROM_OFFSET, *data++);
}

141
osc32_9255/RTArduLink/RTArduLinkHAL.h 100644
View File

@ -0,0 +1,141 @@
///////////////////////////////////////////////////////////
//
// This file is part of RTArduLink
//
// Copyright (c) 2014-2015 richards-tech
//
// Permission is hereby granted, free of charge,
// to any person obtaining a copy of
// this software and associated documentation files
// (the "Software"), to deal in the Software without
// restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute,
// sublicense, and/or sell copies of the Software, and
// to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice
// shall be included in all copies or substantial portions
// of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF
// ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
// TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
// CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR
// IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
#ifndef _RTARDULINKHAL_H
#define _RTARDULINKHAL_H
//----------------------------------------------------------
// Target-specific includes
//
//
// Arduino HAL
#include <RTArduLinkDefs.h>
#include <HardwareSerial.h>
#include <EEPROM.h>
#define RTARDULINKHAL_MAX_SUBSYSTEM_PORTS 3 // maximum number of subsystem ports
#define RTARDULINKHAL_MAX_PORTS (RTARDULINKHAL_MAX_SUBSYSTEM_PORTS + 1) // max total ports (including host)
#define RTARDULINKHAL_EEPROM_OFFSET 256 // where the config starts in EEPROM
// RTARDULINKHAL_PORT should be modified as appropriate for the target.
// There is one copy of this per port. It contains all state needed about
// a serial port.
typedef struct
{
HardwareSerial *serialPort; // the serial port structure
} RTARDULINKHAL_PORT;
// RTARDULINKHAL_EEPROM is the target-specific structure used to
// store configs in EEPROM
// Signature bytes indicating valid config
#define RTARDULINKHAL_SIG0 0x38
#define RTARDULINKHAL_SIG1 0xc1
typedef struct
{
unsigned char sig0; // signature byte 0
unsigned char sig1; // signature byte 1
char identity[RTARDULINK_DATA_MAX_LEN]; // identity string
unsigned char portSpeed[RTARDULINKHAL_MAX_PORTS]; // port speed codes
unsigned char hardwarePort[RTARDULINKHAL_MAX_PORTS]; // port number for hardware serial
} RTARDULINKHAL_EEPROM;
// The global config structure
extern RTARDULINKHAL_EEPROM RTArduLinkHALConfig;
//----------------------------------------------------------
//
// These functions must be provided the RTArduLinkHAL for all implementations
// RTArduLinkHALConfigurePort() activates the specified port configuration specified by portIndex in port structure port.
bool RTArduLinkHALConfigurePort(RTARDULINKHAL_PORT *port, int portIndex);
// RTArduLinkHALPortAvailable() returns the number of bytes availabel on the specified port.
int RTArduLinkHALPortAvailable(RTARDULINKHAL_PORT *port);
// RTArduLinkHALPortRead() returns the next available byte from a port. Always check available bytes first
unsigned char RTArduLinkHALPortRead(RTARDULINKHAL_PORT *port);
// RTArduLinkHALPortWrite() writes length bytes of the block pointed to by data to the specified port.
void RTArduLinkHALPortWrite(RTARDULINKHAL_PORT *port, unsigned char *data, unsigned char length);
// RTArduLinkHALEEPROMValid() returns true if the EEPROM contains a valid configuration,
// false otherwise.
bool RTArduLinkHALEEPROMValid(); // returns true if a valid config
// RTArduLinkHALEEPROMDisplay() displays the current configuration
void RTArduLinkHALEEPROMDisplay(); // display the config
// RTArduLinkHALEEPROMDisplayPort() displays the configuration for a single port
// If suppress is true, nothing is displayed if the port is not enabled. If false
// the port's data will be displayed regardless.
void RTArduLinkHALEEPROMDisplayPort(int port, bool suppress); // display the port config
// RTArduLinkHALEEPROMDefault() writes a default config to EEPROM
void RTArduLinkHALEEPROMDefault(); // write and load default settings
// RTArduLinkHALEEPROMRead() loads the EEPROM config into the RTArduLinkHALConfig
// global structure.
void RTArduLinkHALEEPROMRead(); // to load the config
// RTArduLinkHALEEPROMWrite() writes the config in the RTArduLinkHALConfig
// global structure back to EEPROM.
void RTArduLinkHALEEPROMWrite(); // to write the config
#endif // _RTARDULINKHAL_H

181
osc32_9255/RTArduLink/RTArduLinkUtils.cpp 100644
View File

@ -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];
}

55
osc32_9255/RTArduLink/RTArduLinkUtils.h 100644
View File

@ -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

235
osc32_9255/RTIMULib/RTFusionRTQF.cpp 100644
View File

@ -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

101
osc32_9255/RTIMULib/RTFusionRTQF.h 100644
View File

@ -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

357
osc32_9255/RTIMULib/RTIMU.cpp 100644
View File

@ -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;
}

114
osc32_9255/RTIMULib/RTIMU.h 100644
View File

@ -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

180
osc32_9255/RTIMULib/RTIMUBNO055.cpp 100644
View File

@ -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

85
osc32_9255/RTIMULib/RTIMUBNO055.h 100644
View File

@ -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

402
osc32_9255/RTIMULib/RTIMUGD20HM303D.cpp 100644
View File

@ -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

238
osc32_9255/RTIMULib/RTIMUGD20HM303D.h 100644
View File

@ -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

409
osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.cpp 100644
View File

@ -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

208
osc32_9255/RTIMULib/RTIMUGD20HM303DLHC.h 100644
View File

@ -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

382
osc32_9255/RTIMULib/RTIMUGD20M303DLHC.cpp 100644
View File

@ -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

204
osc32_9255/RTIMULib/RTIMUGD20M303DLHC.h 100644
View File

@ -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

363
osc32_9255/RTIMULib/RTIMULSM9DS0.cpp 100644
View File

@ -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

246
osc32_9255/RTIMULib/RTIMULSM9DS0.h 100644
View File

@ -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

103
osc32_9255/RTIMULib/RTIMULibDefs.h 100644
View File

@ -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

545
osc32_9255/RTIMULib/RTIMUMPU9150.cpp 100644
View File

@ -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

171
osc32_9255/RTIMULib/RTIMUMPU9150.h 100644
View File

@ -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

493
osc32_9255/RTIMULib/RTIMUMPU9250.cpp 100644
View File

@ -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

175
osc32_9255/RTIMULib/RTIMUMPU9250.h 100644
View File

@ -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

308
osc32_9255/RTIMULib/RTIMUSettings.cpp 100644
View File

@ -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
}

124
osc32_9255/RTIMULib/RTIMUSettings.h 100644
View File

@ -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

419
osc32_9255/RTIMULib/RTMath.cpp 100644
View File

@ -0,0 +1,419 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "RTMath.h"
#include <Arduino.h>
#ifndef RTARDULINK_MODE
void RTMath::display(const char *label, RTVector3& vec)
{
Serial.print(label);
Serial.print(" x:"); Serial.print(vec.x());
Serial.print(" y:"); Serial.print(vec.y());
Serial.print(" z:"); Serial.print(vec.z());
}
void RTMath::displayDegrees(const char *label, RTVector3& vec)
{
Serial.print(label);
Serial.print(" x:"); Serial.print(vec.x() * RTMATH_RAD_TO_DEGREE);
Serial.print(" y:"); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE);
Serial.print(" z:"); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE);
}
void RTMath::displayRollPitchYaw(const char *label, RTVector3& vec)
{
Serial.print(label);
Serial.print(" roll:"); Serial.print(vec.x() * RTMATH_RAD_TO_DEGREE);
Serial.print(" pitch:"); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE);
Serial.print(" yaw:"); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE);
}
void RTMath::display(const char *label, RTQuaternion& quat)
{
Serial.print(label);
Serial.print(" scalar:"); Serial.print(quat.scalar());
Serial.print(" x:"); Serial.print(quat.x());
Serial.print(" y:"); Serial.print(quat.y());
Serial.print(" z:"); Serial.print(quat.z());
}
RTVector3 RTMath::poseFromAccelMag(const RTVector3& accel, const RTVector3& mag)
{
RTVector3 result;
RTQuaternion m;
RTQuaternion q;
accel.accelToEuler(result);
// q.fromEuler(result);
// since result.z() is always 0, this can be optimized a little
RTFLOAT cosX2 = cos(result.x() / 2.0f);
RTFLOAT sinX2 = sin(result.x() / 2.0f);
RTFLOAT cosY2 = cos(result.y() / 2.0f);
RTFLOAT sinY2 = sin(result.y() / 2.0f);
q.setScalar(cosX2 * cosY2);
q.setX(sinX2 * cosY2);
q.setY(cosX2 * sinY2);
q.setZ(-sinX2 * sinY2);
// q.normalize();
m.setScalar(0);
m.setX(mag.x());
m.setY(mag.y());
m.setZ(mag.z());
m = q * m * q.conjugate();
result.setZ(-atan2(m.y(), m.x()));
return result;
}
void RTMath::convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian)
{
if (bigEndian) {
vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[0] << 8) | (uint16_t)rawData[1])) * scale);
vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[2] << 8) | (uint16_t)rawData[3])) * scale);
vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[4] << 8) | (uint16_t)rawData[5])) * scale);
} else {
vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[1] << 8) | (uint16_t)rawData[0])) * scale);
vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[3] << 8) | (uint16_t)rawData[2])) * scale);
vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[5] << 8) | (uint16_t)rawData[4])) * scale);
}
}
#endif // #ifndef RTARDULINK_MODE
//----------------------------------------------------------
//
// The RTVector3 class
RTVector3::RTVector3()
{
zero();
}
RTVector3::RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z)
{
m_data[0] = x;
m_data[1] = y;
m_data[2] = z;
}
RTVector3& RTVector3::operator =(const RTVector3& vec)
{
if (this == &vec)
return *this;
m_data[0] = vec.m_data[0];
m_data[1] = vec.m_data[1];
m_data[2] = vec.m_data[2];
return *this;
}
const RTVector3& RTVector3::operator +=(RTVector3& vec)
{
for (int i = 0; i < 3; i++)
m_data[i] += vec.m_data[i];
return *this;
}
const RTVector3& RTVector3::operator -=(RTVector3& vec)
{
for (int i = 0; i < 3; i++)
m_data[i] -= vec.m_data[i];
return *this;
}
void RTVector3::zero()
{
for (int i = 0; i < 3; i++)
m_data[i] = 0;
}
#ifndef RTARDULINK_MODE
RTFLOAT RTVector3::dotProduct(const RTVector3& a, const RTVector3& b)
{
return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
}
void RTVector3::crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d)
{
d.setX(a.y() * b.z() - a.z() * b.y());
d.setY(a.z() * b.x() - a.x() * b.z());
d.setZ(a.x() * b.y() - a.y() * b.x());
}
void RTVector3::accelToEuler(RTVector3& rollPitchYaw) const
{
RTVector3 normAccel = *this;
normAccel.normalize();
rollPitchYaw.setX(atan2(normAccel.y(), normAccel.z()));
rollPitchYaw.setY(-atan2(normAccel.x(), sqrt(normAccel.y() * normAccel.y() + normAccel.z() * normAccel.z())));
rollPitchYaw.setZ(0);
}
void RTVector3::accelToQuaternion(RTQuaternion& qPose) const
{
RTVector3 normAccel = *this;
RTVector3 vec;
RTVector3 z(0, 0, 1.0);
normAccel.normalize();
RTFLOAT angle = acos(RTVector3::dotProduct(z, normAccel));
RTVector3::crossProduct(normAccel, z, vec);
vec.normalize();
qPose.fromAngleVector(angle, vec);
}
void RTVector3::normalize()
{
RTFLOAT length = (RTFLOAT)sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
m_data[2] * m_data[2]);
if ((length == 0) || (length == 1))
return;
m_data[0] /= length;
m_data[1] /= length;
m_data[2] /= length;
}
RTFLOAT RTVector3::length()
{
return (RTFLOAT)sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
m_data[2] * m_data[2]);
}
#endif // #ifndef RTARDULINK_MODE
RTFLOAT RTVector3::squareLength()
{
return m_data[0] * m_data[0] + m_data[1] * m_data[1] +
m_data[2] * m_data[2];
}
#ifndef RTARDULINK_MODE
//----------------------------------------------------------
//
// The RTQuaternion class
RTQuaternion::RTQuaternion()
{
zero();
}
RTQuaternion::RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z)
{
m_data[0] = scalar;
m_data[1] = x;
m_data[2] = y;
m_data[3] = z;
}
RTQuaternion& RTQuaternion::operator =(const RTQuaternion& quat)
{
if (this == &quat)
return *this;
m_data[0] = quat.m_data[0];
m_data[1] = quat.m_data[1];
m_data[2] = quat.m_data[2];
m_data[3] = quat.m_data[3];
return *this;
}
RTQuaternion& RTQuaternion::operator +=(const RTQuaternion& quat)
{
for (int i = 0; i < 4; i++)
m_data[i] += quat.m_data[i];
return *this;
}
RTQuaternion& RTQuaternion::operator -=(const RTQuaternion& quat)
{
for (int i = 0; i < 4; i++)
m_data[i] -= quat.m_data[i];
return *this;
}
RTQuaternion& RTQuaternion::operator -=(const RTFLOAT val)
{
for (int i = 0; i < 4; i++)
m_data[i] -= val;
return *this;
}
RTQuaternion& RTQuaternion::operator *=(const RTQuaternion& qb)
{
RTQuaternion qa;
qa = *this;
m_data[0] = qa.scalar() * qb.scalar() - qa.x() * qb.x() - qa.y() * qb.y() - qa.z() * qb.z();
m_data[1] = qa.scalar() * qb.x() + qa.x() * qb.scalar() + qa.y() * qb.z() - qa.z() * qb.y();
m_data[2] = qa.scalar() * qb.y() - qa.x() * qb.z() + qa.y() * qb.scalar() + qa.z() * qb.x();
m_data[3] = qa.scalar() * qb.z() + qa.x() * qb.y() - qa.y() * qb.x() + qa.z() * qb.scalar();
return *this;
}
RTQuaternion& RTQuaternion::operator *=(const RTFLOAT val)
{
m_data[0] *= val;
m_data[1] *= val;
m_data[2] *= val;
m_data[3] *= val;
return *this;
}
const RTQuaternion RTQuaternion::operator *(const RTQuaternion& qb) const
{
RTQuaternion result = *this;
result *= qb;
return result;
}
const RTQuaternion RTQuaternion::operator *(const RTFLOAT val) const
{
RTQuaternion result = *this;
result *= val;
return result;
}
const RTQuaternion RTQuaternion::operator -(const RTQuaternion& qb) const
{
RTQuaternion result = *this;
result -= qb;
return result;
}
const RTQuaternion RTQuaternion::operator -(const RTFLOAT val) const
{
RTQuaternion result = *this;
result -= val;
return result;
}
void RTQuaternion::zero()
{
for (int i = 0; i < 4; i++)
m_data[i] = 0;
}
void RTQuaternion::normalize()
{
RTFLOAT length = sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] +
m_data[2] * m_data[2] + m_data[3] * m_data[3]);
if ((length == 0) || (length == 1))
return;
m_data[0] /= length;
m_data[1] /= length;
m_data[2] /= length;
m_data[3] /= length;
}
void RTQuaternion::toEuler(RTVector3& vec)
{
vec.setX(atan2(2.0 * (m_data[2] * m_data[3] + m_data[0] * m_data[1]),
1 - 2.0 * (m_data[1] * m_data[1] + m_data[2] * m_data[2])));
vec.setY(asin(2.0 * (m_data[0] * m_data[2] - m_data[1] * m_data[3])));
vec.setZ(atan2(2.0 * (m_data[1] * m_data[2] + m_data[0] * m_data[3]),
1 - 2.0 * (m_data[2] * m_data[2] + m_data[3] * m_data[3])));
}
void RTQuaternion::fromEuler(RTVector3& vec)
{
RTFLOAT cosX2 = cos(vec.x() / 2.0f);
RTFLOAT sinX2 = sin(vec.x() / 2.0f);
RTFLOAT cosY2 = cos(vec.y() / 2.0f);
RTFLOAT sinY2 = sin(vec.y() / 2.0f);
RTFLOAT cosZ2 = cos(vec.z() / 2.0f);
RTFLOAT sinZ2 = sin(vec.z() / 2.0f);
m_data[0] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
m_data[1] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
m_data[2] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
m_data[3] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
normalize();
}
RTQuaternion RTQuaternion::conjugate() const
{
RTQuaternion q;
q.setScalar(m_data[0]);
q.setX(-m_data[1]);
q.setY(-m_data[2]);
q.setZ(-m_data[3]);
return q;
}
void RTQuaternion::toAngleVector(RTFLOAT& angle, RTVector3& vec)
{
RTFLOAT halfTheta;
RTFLOAT sinHalfTheta;
halfTheta = acos(m_data[0]);
sinHalfTheta = sin(halfTheta);
if (sinHalfTheta == 0) {
vec.setX(1.0);
vec.setY(0);
vec.setZ(0);
} else {
vec.setX(m_data[1] / sinHalfTheta);
vec.setY(m_data[1] / sinHalfTheta);
vec.setZ(m_data[1] / sinHalfTheta);
}
angle = 2.0 * halfTheta;
}
void RTQuaternion::fromAngleVector(const RTFLOAT& angle, const RTVector3& vec)
{
RTFLOAT sinHalfTheta = sin(angle / 2.0);
m_data[0] = cos(angle / 2.0);
m_data[1] = vec.x() * sinHalfTheta;
m_data[2] = vec.y() * sinHalfTheta;
m_data[3] = vec.z() * sinHalfTheta;
}
#endif // #ifndef RTARDULINK_MODE

161
osc32_9255/RTIMULib/RTMath.h 100644
View File

@ -0,0 +1,161 @@
////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#ifndef _RTMATH_H_
#define _RTMATH_H_
#include <math.h>
#include <stdint.h>
// The fundamental float type
#ifdef RTMATH_USE_DOUBLE
typedef double RTFLOAT;
#else
typedef float RTFLOAT;
#endif
// Useful constants
#define RTMATH_PI 3.1415926535
#define RTMATH_DEGREE_TO_RAD (M_PI / 180.0)
#define RTMATH_RAD_TO_DEGREE (180.0 / M_PI)
class RTVector3;
#ifndef RTARDULINK_MODE
class RTQuaternion;
#endif
class RTMath
{
public:
#ifndef RTARDULINK_MODE
// convenient display routines
static void display(const char *label, RTVector3& vec);
static void displayDegrees(const char *label, RTVector3& vec);
static void displayRollPitchYaw(const char *label, RTVector3& vec);
static void display(const char *label, RTQuaternion& quat);
// poseFromAccelMag generates pose Euler angles from measured settings
static RTVector3 poseFromAccelMag(const RTVector3& accel, const RTVector3& mag);
// Takes signed 16 bit data from a char array and converts it to a vector of scaled RTFLOATs
static void convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian);
#endif // #ifndef RTARDULINK_MODE
};
class RTVector3
{
public:
RTVector3();
RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z);
const RTVector3& operator +=(RTVector3& vec);
const RTVector3& operator -=(RTVector3& vec);
RTVector3& operator =(const RTVector3& vec);
RTFLOAT squareLength();
void zero();
inline RTFLOAT x() const { return m_data[0]; }
inline RTFLOAT y() const { return m_data[1]; }
inline RTFLOAT z() const { return m_data[2]; }
inline RTFLOAT data(const int i) const { return m_data[i]; }
inline void setX(const RTFLOAT val) { m_data[0] = val; }
inline void setY(const RTFLOAT val) { m_data[1] = val; }
inline void setZ(const RTFLOAT val) { m_data[2] = val; }
inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
#ifndef RTARDULINK_MODE
RTFLOAT length();
void normalize();
const char *display();
const char *displayDegrees();
static RTFLOAT dotProduct(const RTVector3& a, const RTVector3& b);
static void crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d);
void accelToEuler(RTVector3& rollPitchYaw) const;
void accelToQuaternion(RTQuaternion& qPose) const;
#endif // #ifndef RTARDULINK_MODE
private:
RTFLOAT m_data[3];
};
#ifndef RTARDULINK_MODE
class RTQuaternion
{
public:
RTQuaternion();
RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z);
RTQuaternion& operator +=(const RTQuaternion& quat);
RTQuaternion& operator -=(const RTQuaternion& quat);
RTQuaternion& operator *=(const RTQuaternion& qb);
RTQuaternion& operator *=(const RTFLOAT val);
RTQuaternion& operator -=(const RTFLOAT val);
RTQuaternion& operator =(const RTQuaternion& quat);
const RTQuaternion operator *(const RTQuaternion& qb) const;
const RTQuaternion operator *(const RTFLOAT val) const;
const RTQuaternion operator -(const RTQuaternion& qb) const;
const RTQuaternion operator -(const RTFLOAT val) const;
void normalize();
void toEuler(RTVector3& vec);
void fromEuler(RTVector3& vec);
RTQuaternion conjugate() const;
void toAngleVector(RTFLOAT& angle, RTVector3& vec);
void fromAngleVector(const RTFLOAT& angle, const RTVector3& vec);
void zero();
const char *display();
inline RTFLOAT scalar() const { return m_data[0]; }
inline RTFLOAT x() const { return m_data[1]; }
inline RTFLOAT y() const { return m_data[2]; }
inline RTFLOAT z() const { return m_data[3]; }
inline RTFLOAT data(const int i) const { return m_data[i]; }
inline void setScalar(const RTFLOAT val) { m_data[0] = val; }
inline void setX(const RTFLOAT val) { m_data[1] = val; }
inline void setY(const RTFLOAT val) { m_data[2] = val; }
inline void setZ(const RTFLOAT val) { m_data[3] = val; }
inline void setData(const int i, RTFLOAT val) { m_data[i] = val; }
private:
RTFLOAT m_data[4];
};
#endif // #ifndef RTARDULINK_MODE
#endif /* _RTMATH_H_ */

59
osc32_9255/RTIMULib/RTPressure.cpp 100644
View File

@ -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()
{
}

56
osc32_9255/RTIMULib/RTPressure.h 100644
View File

@ -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

227
osc32_9255/RTIMULib/RTPressureBMP180.cpp 100644
View File

@ -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

88
osc32_9255/RTIMULib/RTPressureBMP180.h 100644
View File

@ -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_

104
osc32_9255/RTIMULib/RTPressureDefs.h 100644
View File

@ -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

90
osc32_9255/RTIMULib/RTPressureLPS25H.cpp 100644
View File

@ -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

53
osc32_9255/RTIMULib/RTPressureLPS25H.h 100644
View File

@ -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_

163
osc32_9255/RTIMULib/RTPressureMS5611.cpp 100644
View File

@ -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;
}

69
osc32_9255/RTIMULib/RTPressureMS5611.h 100644
View File

@ -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_

181
osc32_9255/SLIPEncodedBluetoothSerial.cpp 100644
View File

@ -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();
}

62
osc32_9255/SLIPEncodedBluetoothSerial.h 100644
View File

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

310
osc32_9255/osc32_9255.ino 100644
View File

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

194
osc32_9255/quaternionFilters.ino 100644
View File

@ -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;
}

1457
osc32_9255_rtimulib/I2CDev/I2Cdev.cpp 100644
View File

File diff suppressed because it is too large Load Diff

269
osc32_9255_rtimulib/I2CDev/I2Cdev.h 100644
View File

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

181
osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.cpp 100644
View File

@ -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();
}

62
osc32_9255_rtimulib/SLIPEncodedBluetoothSerial.h 100644
View File

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

175
osc32_9255_rtimulib/osc32_9255_rtimulib.ino 100644
View File

@ -0,0 +1,175 @@
// ESP32 Dev Module
#include <Wire.h>
// Parameters for device
#define BTNAME "kegel 2"
// IMU libraries
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>
#include <OSCBundle.h>
#include <OSCBoards.h>
#define BT_OSC
#define DISPLAY_INTERVAL 5 // interval between pose displays
// Bluetooth
#ifdef BT_OSC
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
#include <SLIPEncodedSerial.h>
#include "BluetoothSerial.h"
#include "SLIPEncodedBluetoothSerial.h"
BluetoothSerial SerialBT;
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
#endif
// 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();
}
}
}