mirror of
https://github.com/PotentiaRobotics/engine-software.git
synced 2025-04-30 16:29:51 -04:00
_v1.1 = good measurements, need to work on two slave
This commit is contained in:
FluffyCube9343
parent
f0ff42a82f
commit
f1129307c1
311
communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/I2Cdev.h
Normal file
311
communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/I2Cdev.h
Normal file
|
|
@ -0,0 +1,311 @@
|
|||
// I2Cdev library collection - Main I2C device class header file
|
||||
// Abstracts bit and byte I2C R/W functions into a convenient class
|
||||
// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
|
||||
//
|
||||
// Changelog:
|
||||
// 2021-09-28 - allow custom Wire object as transaction function argument
|
||||
// 2020-01-20 - hardija : complete support for Teensy 3.x
|
||||
// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
|
||||
// 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_
|
||||
|
||||
// -----------------------------------------------------------------------------
|
||||
// Enable deprecated pgmspace typedefs in avr-libc
|
||||
// -----------------------------------------------------------------------------
|
||||
#define __PROG_TYPES_COMPAT__
|
||||
|
||||
// -----------------------------------------------------------------------------
|
||||
// I2C interface implementation setting
|
||||
// -----------------------------------------------------------------------------
|
||||
#ifndef I2CDEV_IMPLEMENTATION
|
||||
#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
|
||||
//#define I2CDEV_IMPLEMENTATION I2CDEV_TEENSY_3X_WIRE
|
||||
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE
|
||||
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE
|
||||
#endif // I2CDEV_IMPLEMENTATION
|
||||
|
||||
// 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
|
||||
#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire
|
||||
#define I2CDEV_TEENSY_3X_WIRE 6 // Teensy 3.x support using i2c_t3 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_TEENSY_3X_WIRE
|
||||
#include <i2c_t3.h>
|
||||
#endif
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
|
||||
#include <I2C.h>
|
||||
#endif
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
|
||||
#include "SBWire.h"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef SPARK
|
||||
#include "application.h"
|
||||
#define ARDUINO 101
|
||||
#define BUFFER_LENGTH 32
|
||||
#endif
|
||||
|
||||
#ifndef I2CDEVLIB_WIRE_BUFFER_LENGTH
|
||||
#if defined(I2C_BUFFER_LENGTH)
|
||||
// Arduino ESP32 core Wire uses this
|
||||
#define I2CDEVLIB_WIRE_BUFFER_LENGTH I2C_BUFFER_LENGTH
|
||||
#elif defined(BUFFER_LENGTH)
|
||||
// Arduino AVR core Wire and many others use this
|
||||
#define I2CDEVLIB_WIRE_BUFFER_LENGTH BUFFER_LENGTH
|
||||
#elif defined(SERIAL_BUFFER_SIZE)
|
||||
// Arduino SAMD core Wire uses this
|
||||
#define I2CDEVLIB_WIRE_BUFFER_LENGTH SERIAL_BUFFER_SIZE
|
||||
#else
|
||||
// should be a safe fallback, though possibly inefficient
|
||||
#define I2CDEVLIB_WIRE_BUFFER_LENGTH 32
|
||||
#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, void *wireObj=0);
|
||||
static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
|
||||
|
||||
static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj=0);
|
||||
static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj=0);
|
||||
static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj=0);
|
||||
static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj=0);
|
||||
static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj=0);
|
||||
static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj=0);
|
||||
static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj=0);
|
||||
static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj=0);
|
||||
|
||||
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 ((1 << TWS7)|(1 << TWS6)|(1 << TWS5)|(1 << TWS4)|(1 << 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) |= (1 << bit))
|
||||
#endif // sbi
|
||||
|
||||
#ifndef cbi // clear bit
|
||||
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~(1 << bit))
|
||||
#endif // cbi
|
||||
|
||||
extern TwoWire Wire;
|
||||
|
||||
#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
|
||||
|
||||
#endif /* _I2CDEV_H_ */
|
||||
|
|
@ -0,0 +1,369 @@
|
|||
// I2C device class (I2Cdev) demonstration Arduino sketch for moose6050 class using DMP (MotionApps v2.0)
|
||||
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2013-05-08 - added seamless Fastwire support
|
||||
// - added note about gyro calibration
|
||||
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
|
||||
// 2012-06-20 - improved FIFO overflow handling and simplified read process
|
||||
// 2012-06-19 - completely rearranged DMP initialization code and simplification
|
||||
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
|
||||
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
|
||||
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
|
||||
// - add 3D math helper file to DMP6 example sketch
|
||||
// - add Euler output and Yaw/Pitch/Roll output formats
|
||||
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
|
||||
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
|
||||
// 2012-05-30 - basic DMP initialization working
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2012 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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// I2Cdev and moose6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 moose(0x68); // <-- use for AD0 high
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the moose-6050's INT pin being connected to the Arduino's
|
||||
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
||||
digital I/O pin 2.
|
||||
* ========================================================================= */
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
||||
when using Serial.write(buf, len). The Teapot output uses this method.
|
||||
The solution requires a modification to the Arduino USBAPI.h file, which
|
||||
is fortunately simple, but annoying. This will be fixed in the next IDE
|
||||
release. For more info, see these links:
|
||||
|
||||
http://arduino.cc/forum/index.php/topic,109987.0.html
|
||||
http://code.google.com/p/arduino/issues/detail?id=958
|
||||
* ========================================================================= */
|
||||
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
//#define OUTPUT_TEAPOT
|
||||
|
||||
|
||||
|
||||
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
||||
bool blinkState = false;
|
||||
|
||||
// moose control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mooseIntStatus; // holds actual interrupt status byte from moose
|
||||
uint8_t 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
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
float quaternion[3];
|
||||
|
||||
// packet structure for InvenSense teapot demo
|
||||
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mooseInterrupt = false; // indicates whether moose interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mooseInterrupt = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INITIAL SETUP ===
|
||||
// ================================================================
|
||||
|
||||
void setup() {
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (115200 chosen because it is required for Teapot Demo output, but it's
|
||||
// really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
||||
|
||||
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
|
||||
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
|
||||
// the baud timing being too misaligned with processor ticks. You must use
|
||||
// 38400 or slower in these cases, or use some kind of external separate
|
||||
// crystal solution for the UART timer.
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
moose.initialize();
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(moose.testConnection() ? F("moose6050 connection successful") : F("moose6050 connection failed"));
|
||||
|
||||
// wait for ready
|
||||
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
||||
while (Serial.available() && Serial.read()); // empty buffer
|
||||
while (!Serial.available()); // wait for data
|
||||
while (Serial.available() && Serial.read()); // empty buffer again
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = moose.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
moose.setXGyroOffset(220);
|
||||
moose.setYGyroOffset(76);
|
||||
moose.setZGyroOffset(-85);
|
||||
moose.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
moose.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
|
||||
attachInterrupt(0, dmpDataReady, RISING);
|
||||
mooseIntStatus = moose.getIntStatus();
|
||||
|
||||
// set our DMP Ready flag so the main loop() function knows it's okay to use it
|
||||
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
||||
dmpReady = true;
|
||||
|
||||
// get expected DMP packet size for later comparison
|
||||
packetSize = moose.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
Serial.print(F("DMP Initialization failed (code "));
|
||||
Serial.print(devStatus);
|
||||
Serial.println(F(")"));
|
||||
}
|
||||
|
||||
// configure LED for output
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === MAIN PROGRAM LOOP ===
|
||||
// ================================================================
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
// wait for moose interrupt or extra packet(s) available
|
||||
while (!mooseInterrupt && fifoCount < packetSize) {
|
||||
// other program behavior stuff here
|
||||
// .
|
||||
// .
|
||||
// .
|
||||
// if you are really paranoid you can frequently test in between other
|
||||
// stuff to see if mooseInterrupt is true, and if so, "break;" from the
|
||||
// while() loop to immediately process the moose data
|
||||
// .
|
||||
// .
|
||||
// .
|
||||
}
|
||||
|
||||
// reset interrupt flag and get INT_STATUS byte
|
||||
mooseInterrupt = false;
|
||||
mooseIntStatus = moose.getIntStatus();
|
||||
|
||||
// get current FIFO count
|
||||
fifoCount = moose.getFIFOCount();
|
||||
|
||||
// check for overflow (this should never happen unless our code is too inefficient)
|
||||
if ((mooseIntStatus & 0x10) || fifoCount == 1024) {
|
||||
// reset so we can continue cleanly
|
||||
moose.resetFIFO();
|
||||
Serial.println(F("FIFO overflow!"));
|
||||
|
||||
// otherwise, check for DMP data ready interrupt (this should happen frequently)
|
||||
} else if (mooseIntStatus & 0x02) {
|
||||
// wait for correct available data length, should be a VERY short wait
|
||||
while (fifoCount < packetSize) fifoCount = moose.getFIFOCount();
|
||||
|
||||
// read a packet from FIFO
|
||||
moose.getFIFOBytes(fifoBuffer, packetSize);
|
||||
|
||||
// track FIFO count here in case there is > 1 packet available
|
||||
// (this lets us immediately read more without waiting for an interrupt)
|
||||
fifoCount -= packetSize;
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
moose.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat\t");
|
||||
Serial.print((q.x+0.03)*180);
|
||||
Serial.print("\t");
|
||||
Serial.print((q.y+0.03)*180);
|
||||
Serial.print("\t");
|
||||
Serial.println((q.z+0.52)*180);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
moose.dmpGetQuaternion(&q, fifoBuffer);
|
||||
moose.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
moose.dmpGetQuaternion(&q, fifoBuffer);
|
||||
moose.dmpGetGravity(&gravity, &q);
|
||||
moose.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
moose.dmpGetQuaternion(&q, fifoBuffer);
|
||||
moose.dmpGetAccel(&aa, fifoBuffer);
|
||||
moose.dmpGetGravity(&gravity, &q);
|
||||
moose.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
moose.dmpGetQuaternion(&q, fifoBuffer);
|
||||
moose.dmpGetAccel(&aa, fifoBuffer);
|
||||
moose.dmpGetGravity(&gravity, &q);
|
||||
moose.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
moose.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer[0];
|
||||
teapotPacket[3] = fifoBuffer[1];
|
||||
teapotPacket[4] = fifoBuffer[4];
|
||||
teapotPacket[5] = fifoBuffer[5];
|
||||
teapotPacket[6] = fifoBuffer[8];
|
||||
teapotPacket[7] = fifoBuffer[9];
|
||||
teapotPacket[8] = fifoBuffer[12];
|
||||
teapotPacket[9] = fifoBuffer[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
|
||||
}
|
||||
848
communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/MPU6050.h
Normal file
848
communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/MPU6050.h
Normal file
|
|
@ -0,0 +1,848 @@
|
|||
// I2Cdev library collection - MPU6050 I2C device class
|
||||
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
|
||||
// 10/3/2011 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2021/09/27 - split implementations out of header files, finally
|
||||
// ... - ongoing debug release
|
||||
|
||||
// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
|
||||
// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF
|
||||
// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING.
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2012 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 _MPU6050_H_
|
||||
#define _MPU6050_H_
|
||||
|
||||
#include "I2Cdev.h"
|
||||
#include "helper_3dmath.h"
|
||||
|
||||
// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517
|
||||
// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
|
||||
|
||||
#ifdef __AVR__
|
||||
#include <avr/pgmspace.h>
|
||||
#elif defined(ESP32)
|
||||
#include <pgmspace.h>
|
||||
#else
|
||||
//#define PROGMEM /* empty */
|
||||
//#define pgm_read_byte(x) (*(x))
|
||||
//#define pgm_read_word(x) (*(x))
|
||||
//#define pgm_read_float(x) (*(x))
|
||||
//#define PSTR(STR) STR
|
||||
#endif
|
||||
|
||||
|
||||
#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
|
||||
#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
|
||||
#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
|
||||
|
||||
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
|
||||
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
|
||||
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
|
||||
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
|
||||
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
|
||||
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
|
||||
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
|
||||
#define MPU6050_RA_XA_OFFS_L_TC 0x07
|
||||
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
|
||||
#define MPU6050_RA_YA_OFFS_L_TC 0x09
|
||||
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
|
||||
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
|
||||
#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0]
|
||||
#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0]
|
||||
#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0]
|
||||
#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0]
|
||||
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
|
||||
#define MPU6050_RA_XG_OFFS_USRL 0x14
|
||||
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
|
||||
#define MPU6050_RA_YG_OFFS_USRL 0x16
|
||||
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
|
||||
#define MPU6050_RA_ZG_OFFS_USRL 0x18
|
||||
#define MPU6050_RA_SMPLRT_DIV 0x19
|
||||
#define MPU6050_RA_CONFIG 0x1A
|
||||
#define MPU6050_RA_GYRO_CONFIG 0x1B
|
||||
#define MPU6050_RA_ACCEL_CONFIG 0x1C
|
||||
#define MPU6050_RA_FF_THR 0x1D
|
||||
#define MPU6050_RA_FF_DUR 0x1E
|
||||
#define MPU6050_RA_MOT_THR 0x1F
|
||||
#define MPU6050_RA_MOT_DUR 0x20
|
||||
#define MPU6050_RA_ZRMOT_THR 0x21
|
||||
#define MPU6050_RA_ZRMOT_DUR 0x22
|
||||
#define MPU6050_RA_FIFO_EN 0x23
|
||||
#define MPU6050_RA_I2C_MST_CTRL 0x24
|
||||
#define MPU6050_RA_I2C_SLV0_ADDR 0x25
|
||||
#define MPU6050_RA_I2C_SLV0_REG 0x26
|
||||
#define MPU6050_RA_I2C_SLV0_CTRL 0x27
|
||||
#define MPU6050_RA_I2C_SLV1_ADDR 0x28
|
||||
#define MPU6050_RA_I2C_SLV1_REG 0x29
|
||||
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
|
||||
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
|
||||
#define MPU6050_RA_I2C_SLV2_REG 0x2C
|
||||
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
|
||||
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
|
||||
#define MPU6050_RA_I2C_SLV3_REG 0x2F
|
||||
#define MPU6050_RA_I2C_SLV3_CTRL 0x30
|
||||
#define MPU6050_RA_I2C_SLV4_ADDR 0x31
|
||||
#define MPU6050_RA_I2C_SLV4_REG 0x32
|
||||
#define MPU6050_RA_I2C_SLV4_DO 0x33
|
||||
#define MPU6050_RA_I2C_SLV4_CTRL 0x34
|
||||
#define MPU6050_RA_I2C_SLV4_DI 0x35
|
||||
#define MPU6050_RA_I2C_MST_STATUS 0x36
|
||||
#define MPU6050_RA_INT_PIN_CFG 0x37
|
||||
#define MPU6050_RA_INT_ENABLE 0x38
|
||||
#define MPU6050_RA_DMP_INT_STATUS 0x39
|
||||
#define MPU6050_RA_INT_STATUS 0x3A
|
||||
#define MPU6050_RA_ACCEL_XOUT_H 0x3B
|
||||
#define MPU6050_RA_ACCEL_XOUT_L 0x3C
|
||||
#define MPU6050_RA_ACCEL_YOUT_H 0x3D
|
||||
#define MPU6050_RA_ACCEL_YOUT_L 0x3E
|
||||
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
|
||||
#define MPU6050_RA_ACCEL_ZOUT_L 0x40
|
||||
#define MPU6050_RA_TEMP_OUT_H 0x41
|
||||
#define MPU6050_RA_TEMP_OUT_L 0x42
|
||||
#define MPU6050_RA_GYRO_XOUT_H 0x43
|
||||
#define MPU6050_RA_GYRO_XOUT_L 0x44
|
||||
#define MPU6050_RA_GYRO_YOUT_H 0x45
|
||||
#define MPU6050_RA_GYRO_YOUT_L 0x46
|
||||
#define MPU6050_RA_GYRO_ZOUT_H 0x47
|
||||
#define MPU6050_RA_GYRO_ZOUT_L 0x48
|
||||
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
|
||||
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
|
||||
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
|
||||
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
|
||||
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
|
||||
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
|
||||
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
|
||||
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
|
||||
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
|
||||
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
|
||||
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
|
||||
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
|
||||
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
|
||||
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
|
||||
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
|
||||
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
|
||||
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
|
||||
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
|
||||
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
|
||||
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
|
||||
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
|
||||
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
|
||||
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
|
||||
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
|
||||
#define MPU6050_RA_MOT_DETECT_STATUS 0x61
|
||||
#define MPU6050_RA_I2C_SLV0_DO 0x63
|
||||
#define MPU6050_RA_I2C_SLV1_DO 0x64
|
||||
#define MPU6050_RA_I2C_SLV2_DO 0x65
|
||||
#define MPU6050_RA_I2C_SLV3_DO 0x66
|
||||
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
|
||||
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
|
||||
#define MPU6050_RA_MOT_DETECT_CTRL 0x69
|
||||
#define MPU6050_RA_USER_CTRL 0x6A
|
||||
#define MPU6050_RA_PWR_MGMT_1 0x6B
|
||||
#define MPU6050_RA_PWR_MGMT_2 0x6C
|
||||
#define MPU6050_RA_BANK_SEL 0x6D
|
||||
#define MPU6050_RA_MEM_START_ADDR 0x6E
|
||||
#define MPU6050_RA_MEM_R_W 0x6F
|
||||
#define MPU6050_RA_DMP_CFG_1 0x70
|
||||
#define MPU6050_RA_DMP_CFG_2 0x71
|
||||
#define MPU6050_RA_FIFO_COUNTH 0x72
|
||||
#define MPU6050_RA_FIFO_COUNTL 0x73
|
||||
#define MPU6050_RA_FIFO_R_W 0x74
|
||||
#define MPU6050_RA_WHO_AM_I 0x75
|
||||
|
||||
#define MPU6050_SELF_TEST_XA_1_BIT 0x07
|
||||
#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03
|
||||
#define MPU6050_SELF_TEST_XA_2_BIT 0x05
|
||||
#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02
|
||||
#define MPU6050_SELF_TEST_YA_1_BIT 0x07
|
||||
#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03
|
||||
#define MPU6050_SELF_TEST_YA_2_BIT 0x03
|
||||
#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02
|
||||
#define MPU6050_SELF_TEST_ZA_1_BIT 0x07
|
||||
#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03
|
||||
#define MPU6050_SELF_TEST_ZA_2_BIT 0x01
|
||||
#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02
|
||||
|
||||
#define MPU6050_SELF_TEST_XG_1_BIT 0x04
|
||||
#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05
|
||||
#define MPU6050_SELF_TEST_YG_1_BIT 0x04
|
||||
#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05
|
||||
#define MPU6050_SELF_TEST_ZG_1_BIT 0x04
|
||||
#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05
|
||||
|
||||
#define MPU6050_TC_PWR_MODE_BIT 7
|
||||
#define MPU6050_TC_OFFSET_BIT 6
|
||||
#define MPU6050_TC_OFFSET_LENGTH 6
|
||||
#define MPU6050_TC_OTP_BNK_VLD_BIT 0
|
||||
|
||||
#define MPU6050_VDDIO_LEVEL_VLOGIC 0
|
||||
#define MPU6050_VDDIO_LEVEL_VDD 1
|
||||
|
||||
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5
|
||||
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
|
||||
#define MPU6050_CFG_DLPF_CFG_BIT 2
|
||||
#define MPU6050_CFG_DLPF_CFG_LENGTH 3
|
||||
|
||||
#define MPU6050_EXT_SYNC_DISABLED 0x0
|
||||
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1
|
||||
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2
|
||||
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3
|
||||
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4
|
||||
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5
|
||||
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6
|
||||
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7
|
||||
|
||||
#define MPU6050_DLPF_BW_256 0x00
|
||||
#define MPU6050_DLPF_BW_188 0x01
|
||||
#define MPU6050_DLPF_BW_98 0x02
|
||||
#define MPU6050_DLPF_BW_42 0x03
|
||||
#define MPU6050_DLPF_BW_20 0x04
|
||||
#define MPU6050_DLPF_BW_10 0x05
|
||||
#define MPU6050_DLPF_BW_5 0x06
|
||||
|
||||
#define MPU6050_GCONFIG_FS_SEL_BIT 4
|
||||
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2
|
||||
|
||||
#define MPU6050_GYRO_FS_250 0x00
|
||||
#define MPU6050_GYRO_FS_500 0x01
|
||||
#define MPU6050_GYRO_FS_1000 0x02
|
||||
#define MPU6050_GYRO_FS_2000 0x03
|
||||
|
||||
#define MPU6050_ACONFIG_XA_ST_BIT 7
|
||||
#define MPU6050_ACONFIG_YA_ST_BIT 6
|
||||
#define MPU6050_ACONFIG_ZA_ST_BIT 5
|
||||
#define MPU6050_ACONFIG_AFS_SEL_BIT 4
|
||||
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
|
||||
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2
|
||||
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3
|
||||
|
||||
#define MPU6050_ACCEL_FS_2 0x00
|
||||
#define MPU6050_ACCEL_FS_4 0x01
|
||||
#define MPU6050_ACCEL_FS_8 0x02
|
||||
#define MPU6050_ACCEL_FS_16 0x03
|
||||
|
||||
#define MPU6050_DHPF_RESET 0x00
|
||||
#define MPU6050_DHPF_5 0x01
|
||||
#define MPU6050_DHPF_2P5 0x02
|
||||
#define MPU6050_DHPF_1P25 0x03
|
||||
#define MPU6050_DHPF_0P63 0x04
|
||||
#define MPU6050_DHPF_HOLD 0x07
|
||||
|
||||
#define MPU6050_TEMP_FIFO_EN_BIT 7
|
||||
#define MPU6050_XG_FIFO_EN_BIT 6
|
||||
#define MPU6050_YG_FIFO_EN_BIT 5
|
||||
#define MPU6050_ZG_FIFO_EN_BIT 4
|
||||
#define MPU6050_ACCEL_FIFO_EN_BIT 3
|
||||
#define MPU6050_SLV2_FIFO_EN_BIT 2
|
||||
#define MPU6050_SLV1_FIFO_EN_BIT 1
|
||||
#define MPU6050_SLV0_FIFO_EN_BIT 0
|
||||
|
||||
#define MPU6050_MULT_MST_EN_BIT 7
|
||||
#define MPU6050_WAIT_FOR_ES_BIT 6
|
||||
#define MPU6050_SLV_3_FIFO_EN_BIT 5
|
||||
#define MPU6050_I2C_MST_P_NSR_BIT 4
|
||||
#define MPU6050_I2C_MST_CLK_BIT 3
|
||||
#define MPU6050_I2C_MST_CLK_LENGTH 4
|
||||
|
||||
#define MPU6050_CLOCK_DIV_348 0x0
|
||||
#define MPU6050_CLOCK_DIV_333 0x1
|
||||
#define MPU6050_CLOCK_DIV_320 0x2
|
||||
#define MPU6050_CLOCK_DIV_308 0x3
|
||||
#define MPU6050_CLOCK_DIV_296 0x4
|
||||
#define MPU6050_CLOCK_DIV_286 0x5
|
||||
#define MPU6050_CLOCK_DIV_276 0x6
|
||||
#define MPU6050_CLOCK_DIV_267 0x7
|
||||
#define MPU6050_CLOCK_DIV_258 0x8
|
||||
#define MPU6050_CLOCK_DIV_500 0x9
|
||||
#define MPU6050_CLOCK_DIV_471 0xA
|
||||
#define MPU6050_CLOCK_DIV_444 0xB
|
||||
#define MPU6050_CLOCK_DIV_421 0xC
|
||||
#define MPU6050_CLOCK_DIV_400 0xD
|
||||
#define MPU6050_CLOCK_DIV_381 0xE
|
||||
#define MPU6050_CLOCK_DIV_364 0xF
|
||||
|
||||
#define MPU6050_I2C_SLV_RW_BIT 7
|
||||
#define MPU6050_I2C_SLV_ADDR_BIT 6
|
||||
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
|
||||
#define MPU6050_I2C_SLV_EN_BIT 7
|
||||
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
|
||||
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
|
||||
#define MPU6050_I2C_SLV_GRP_BIT 4
|
||||
#define MPU6050_I2C_SLV_LEN_BIT 3
|
||||
#define MPU6050_I2C_SLV_LEN_LENGTH 4
|
||||
|
||||
#define MPU6050_I2C_SLV4_RW_BIT 7
|
||||
#define MPU6050_I2C_SLV4_ADDR_BIT 6
|
||||
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7
|
||||
#define MPU6050_I2C_SLV4_EN_BIT 7
|
||||
#define MPU6050_I2C_SLV4_INT_EN_BIT 6
|
||||
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5
|
||||
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4
|
||||
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
|
||||
|
||||
#define MPU6050_MST_PASS_THROUGH_BIT 7
|
||||
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6
|
||||
#define MPU6050_MST_I2C_LOST_ARB_BIT 5
|
||||
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4
|
||||
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3
|
||||
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2
|
||||
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1
|
||||
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0
|
||||
|
||||
#define MPU6050_INTCFG_INT_LEVEL_BIT 7
|
||||
#define MPU6050_INTCFG_INT_OPEN_BIT 6
|
||||
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5
|
||||
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4
|
||||
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3
|
||||
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2
|
||||
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1
|
||||
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0
|
||||
|
||||
#define MPU6050_INTMODE_ACTIVEHIGH 0x00
|
||||
#define MPU6050_INTMODE_ACTIVELOW 0x01
|
||||
|
||||
#define MPU6050_INTDRV_PUSHPULL 0x00
|
||||
#define MPU6050_INTDRV_OPENDRAIN 0x01
|
||||
|
||||
#define MPU6050_INTLATCH_50USPULSE 0x00
|
||||
#define MPU6050_INTLATCH_WAITCLEAR 0x01
|
||||
|
||||
#define MPU6050_INTCLEAR_STATUSREAD 0x00
|
||||
#define MPU6050_INTCLEAR_ANYREAD 0x01
|
||||
|
||||
#define MPU6050_INTERRUPT_FF_BIT 7
|
||||
#define MPU6050_INTERRUPT_MOT_BIT 6
|
||||
#define MPU6050_INTERRUPT_ZMOT_BIT 5
|
||||
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4
|
||||
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3
|
||||
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2
|
||||
#define MPU6050_INTERRUPT_DMP_INT_BIT 1
|
||||
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0
|
||||
|
||||
// TODO: figure out what these actually do
|
||||
// UMPL source code is not very obivous
|
||||
#define MPU6050_DMPINT_5_BIT 5
|
||||
#define MPU6050_DMPINT_4_BIT 4
|
||||
#define MPU6050_DMPINT_3_BIT 3
|
||||
#define MPU6050_DMPINT_2_BIT 2
|
||||
#define MPU6050_DMPINT_1_BIT 1
|
||||
#define MPU6050_DMPINT_0_BIT 0
|
||||
|
||||
#define MPU6050_MOTION_MOT_XNEG_BIT 7
|
||||
#define MPU6050_MOTION_MOT_XPOS_BIT 6
|
||||
#define MPU6050_MOTION_MOT_YNEG_BIT 5
|
||||
#define MPU6050_MOTION_MOT_YPOS_BIT 4
|
||||
#define MPU6050_MOTION_MOT_ZNEG_BIT 3
|
||||
#define MPU6050_MOTION_MOT_ZPOS_BIT 2
|
||||
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0
|
||||
|
||||
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7
|
||||
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4
|
||||
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3
|
||||
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2
|
||||
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1
|
||||
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0
|
||||
|
||||
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2
|
||||
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1
|
||||
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0
|
||||
|
||||
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5
|
||||
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2
|
||||
#define MPU6050_DETECT_FF_COUNT_BIT 3
|
||||
#define MPU6050_DETECT_FF_COUNT_LENGTH 2
|
||||
#define MPU6050_DETECT_MOT_COUNT_BIT 1
|
||||
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2
|
||||
|
||||
#define MPU6050_DETECT_DECREMENT_RESET 0x0
|
||||
#define MPU6050_DETECT_DECREMENT_1 0x1
|
||||
#define MPU6050_DETECT_DECREMENT_2 0x2
|
||||
#define MPU6050_DETECT_DECREMENT_4 0x3
|
||||
|
||||
#define MPU6050_USERCTRL_DMP_EN_BIT 7
|
||||
#define MPU6050_USERCTRL_FIFO_EN_BIT 6
|
||||
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5
|
||||
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4
|
||||
#define MPU6050_USERCTRL_DMP_RESET_BIT 3
|
||||
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2
|
||||
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1
|
||||
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0
|
||||
|
||||
#define MPU6050_PWR1_DEVICE_RESET_BIT 7
|
||||
#define MPU6050_PWR1_SLEEP_BIT 6
|
||||
#define MPU6050_PWR1_CYCLE_BIT 5
|
||||
#define MPU6050_PWR1_TEMP_DIS_BIT 3
|
||||
#define MPU6050_PWR1_CLKSEL_BIT 2
|
||||
#define MPU6050_PWR1_CLKSEL_LENGTH 3
|
||||
|
||||
#define MPU6050_CLOCK_INTERNAL 0x00
|
||||
#define MPU6050_CLOCK_PLL_XGYRO 0x01
|
||||
#define MPU6050_CLOCK_PLL_YGYRO 0x02
|
||||
#define MPU6050_CLOCK_PLL_ZGYRO 0x03
|
||||
#define MPU6050_CLOCK_PLL_EXT32K 0x04
|
||||
#define MPU6050_CLOCK_PLL_EXT19M 0x05
|
||||
#define MPU6050_CLOCK_KEEP_RESET 0x07
|
||||
|
||||
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7
|
||||
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2
|
||||
#define MPU6050_PWR2_STBY_XA_BIT 5
|
||||
#define MPU6050_PWR2_STBY_YA_BIT 4
|
||||
#define MPU6050_PWR2_STBY_ZA_BIT 3
|
||||
#define MPU6050_PWR2_STBY_XG_BIT 2
|
||||
#define MPU6050_PWR2_STBY_YG_BIT 1
|
||||
#define MPU6050_PWR2_STBY_ZG_BIT 0
|
||||
|
||||
#define MPU6050_WAKE_FREQ_1P25 0x0
|
||||
#define MPU6050_WAKE_FREQ_2P5 0x1
|
||||
#define MPU6050_WAKE_FREQ_5 0x2
|
||||
#define MPU6050_WAKE_FREQ_10 0x3
|
||||
|
||||
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6
|
||||
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5
|
||||
#define MPU6050_BANKSEL_MEM_SEL_BIT 4
|
||||
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5
|
||||
|
||||
#define MPU6050_WHO_AM_I_BIT 6
|
||||
#define MPU6050_WHO_AM_I_LENGTH 6
|
||||
|
||||
#define MPU6050_DMP_MEMORY_BANKS 8
|
||||
#define MPU6050_DMP_MEMORY_BANK_SIZE 256
|
||||
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
|
||||
|
||||
#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000
|
||||
|
||||
class MPU6050_Base {
|
||||
public:
|
||||
MPU6050_Base(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0);
|
||||
|
||||
void initialize();
|
||||
bool testConnection();
|
||||
|
||||
// AUX_VDDIO register
|
||||
uint8_t getAuxVDDIOLevel();
|
||||
void setAuxVDDIOLevel(uint8_t level);
|
||||
|
||||
// SMPLRT_DIV register
|
||||
uint8_t getRate();
|
||||
void setRate(uint8_t rate);
|
||||
|
||||
// CONFIG register
|
||||
uint8_t getExternalFrameSync();
|
||||
void setExternalFrameSync(uint8_t sync);
|
||||
uint8_t getDLPFMode();
|
||||
void setDLPFMode(uint8_t bandwidth);
|
||||
|
||||
// GYRO_CONFIG register
|
||||
uint8_t getFullScaleGyroRange();
|
||||
void setFullScaleGyroRange(uint8_t range);
|
||||
|
||||
// SELF_TEST registers
|
||||
uint8_t getAccelXSelfTestFactoryTrim();
|
||||
uint8_t getAccelYSelfTestFactoryTrim();
|
||||
uint8_t getAccelZSelfTestFactoryTrim();
|
||||
|
||||
uint8_t getGyroXSelfTestFactoryTrim();
|
||||
uint8_t getGyroYSelfTestFactoryTrim();
|
||||
uint8_t getGyroZSelfTestFactoryTrim();
|
||||
|
||||
// ACCEL_CONFIG register
|
||||
bool getAccelXSelfTest();
|
||||
void setAccelXSelfTest(bool enabled);
|
||||
bool getAccelYSelfTest();
|
||||
void setAccelYSelfTest(bool enabled);
|
||||
bool getAccelZSelfTest();
|
||||
void setAccelZSelfTest(bool enabled);
|
||||
uint8_t getFullScaleAccelRange();
|
||||
void setFullScaleAccelRange(uint8_t range);
|
||||
uint8_t getDHPFMode();
|
||||
void setDHPFMode(uint8_t mode);
|
||||
|
||||
// FF_THR register
|
||||
uint8_t getFreefallDetectionThreshold();
|
||||
void setFreefallDetectionThreshold(uint8_t threshold);
|
||||
|
||||
// FF_DUR register
|
||||
uint8_t getFreefallDetectionDuration();
|
||||
void setFreefallDetectionDuration(uint8_t duration);
|
||||
|
||||
// MOT_THR register
|
||||
uint8_t getMotionDetectionThreshold();
|
||||
void setMotionDetectionThreshold(uint8_t threshold);
|
||||
|
||||
// MOT_DUR register
|
||||
uint8_t getMotionDetectionDuration();
|
||||
void setMotionDetectionDuration(uint8_t duration);
|
||||
|
||||
// ZRMOT_THR register
|
||||
uint8_t getZeroMotionDetectionThreshold();
|
||||
void setZeroMotionDetectionThreshold(uint8_t threshold);
|
||||
|
||||
// ZRMOT_DUR register
|
||||
uint8_t getZeroMotionDetectionDuration();
|
||||
void setZeroMotionDetectionDuration(uint8_t duration);
|
||||
|
||||
// FIFO_EN register
|
||||
bool getTempFIFOEnabled();
|
||||
void setTempFIFOEnabled(bool enabled);
|
||||
bool getXGyroFIFOEnabled();
|
||||
void setXGyroFIFOEnabled(bool enabled);
|
||||
bool getYGyroFIFOEnabled();
|
||||
void setYGyroFIFOEnabled(bool enabled);
|
||||
bool getZGyroFIFOEnabled();
|
||||
void setZGyroFIFOEnabled(bool enabled);
|
||||
bool getAccelFIFOEnabled();
|
||||
void setAccelFIFOEnabled(bool enabled);
|
||||
bool getSlave2FIFOEnabled();
|
||||
void setSlave2FIFOEnabled(bool enabled);
|
||||
bool getSlave1FIFOEnabled();
|
||||
void setSlave1FIFOEnabled(bool enabled);
|
||||
bool getSlave0FIFOEnabled();
|
||||
void setSlave0FIFOEnabled(bool enabled);
|
||||
|
||||
// I2C_MST_CTRL register
|
||||
bool getMultiMasterEnabled();
|
||||
void setMultiMasterEnabled(bool enabled);
|
||||
bool getWaitForExternalSensorEnabled();
|
||||
void setWaitForExternalSensorEnabled(bool enabled);
|
||||
bool getSlave3FIFOEnabled();
|
||||
void setSlave3FIFOEnabled(bool enabled);
|
||||
bool getSlaveReadWriteTransitionEnabled();
|
||||
void setSlaveReadWriteTransitionEnabled(bool enabled);
|
||||
uint8_t getMasterClockSpeed();
|
||||
void setMasterClockSpeed(uint8_t speed);
|
||||
|
||||
// I2C_SLV* registers (Slave 0-3)
|
||||
uint8_t getSlaveAddress(uint8_t num);
|
||||
void setSlaveAddress(uint8_t num, uint8_t address);
|
||||
uint8_t getSlaveRegister(uint8_t num);
|
||||
void setSlaveRegister(uint8_t num, uint8_t reg);
|
||||
bool getSlaveEnabled(uint8_t num);
|
||||
void setSlaveEnabled(uint8_t num, bool enabled);
|
||||
bool getSlaveWordByteSwap(uint8_t num);
|
||||
void setSlaveWordByteSwap(uint8_t num, bool enabled);
|
||||
bool getSlaveWriteMode(uint8_t num);
|
||||
void setSlaveWriteMode(uint8_t num, bool mode);
|
||||
bool getSlaveWordGroupOffset(uint8_t num);
|
||||
void setSlaveWordGroupOffset(uint8_t num, bool enabled);
|
||||
uint8_t getSlaveDataLength(uint8_t num);
|
||||
void setSlaveDataLength(uint8_t num, uint8_t length);
|
||||
|
||||
// I2C_SLV* registers (Slave 4)
|
||||
uint8_t getSlave4Address();
|
||||
void setSlave4Address(uint8_t address);
|
||||
uint8_t getSlave4Register();
|
||||
void setSlave4Register(uint8_t reg);
|
||||
void setSlave4OutputByte(uint8_t data);
|
||||
bool getSlave4Enabled();
|
||||
void setSlave4Enabled(bool enabled);
|
||||
bool getSlave4InterruptEnabled();
|
||||
void setSlave4InterruptEnabled(bool enabled);
|
||||
bool getSlave4WriteMode();
|
||||
void setSlave4WriteMode(bool mode);
|
||||
uint8_t getSlave4MasterDelay();
|
||||
void setSlave4MasterDelay(uint8_t delay);
|
||||
uint8_t getSlate4InputByte();
|
||||
|
||||
// I2C_MST_STATUS register
|
||||
bool getPassthroughStatus();
|
||||
bool getSlave4IsDone();
|
||||
bool getLostArbitration();
|
||||
bool getSlave4Nack();
|
||||
bool getSlave3Nack();
|
||||
bool getSlave2Nack();
|
||||
bool getSlave1Nack();
|
||||
bool getSlave0Nack();
|
||||
|
||||
// INT_PIN_CFG register
|
||||
bool getInterruptMode();
|
||||
void setInterruptMode(bool mode);
|
||||
bool getInterruptDrive();
|
||||
void setInterruptDrive(bool drive);
|
||||
bool getInterruptLatch();
|
||||
void setInterruptLatch(bool latch);
|
||||
bool getInterruptLatchClear();
|
||||
void setInterruptLatchClear(bool clear);
|
||||
bool getFSyncInterruptLevel();
|
||||
void setFSyncInterruptLevel(bool level);
|
||||
bool getFSyncInterruptEnabled();
|
||||
void setFSyncInterruptEnabled(bool enabled);
|
||||
bool getI2CBypassEnabled();
|
||||
void setI2CBypassEnabled(bool enabled);
|
||||
bool getClockOutputEnabled();
|
||||
void setClockOutputEnabled(bool enabled);
|
||||
|
||||
// INT_ENABLE register
|
||||
uint8_t getIntEnabled();
|
||||
void setIntEnabled(uint8_t enabled);
|
||||
bool getIntFreefallEnabled();
|
||||
void setIntFreefallEnabled(bool enabled);
|
||||
bool getIntMotionEnabled();
|
||||
void setIntMotionEnabled(bool enabled);
|
||||
bool getIntZeroMotionEnabled();
|
||||
void setIntZeroMotionEnabled(bool enabled);
|
||||
bool getIntFIFOBufferOverflowEnabled();
|
||||
void setIntFIFOBufferOverflowEnabled(bool enabled);
|
||||
bool getIntI2CMasterEnabled();
|
||||
void setIntI2CMasterEnabled(bool enabled);
|
||||
bool getIntDataReadyEnabled();
|
||||
void setIntDataReadyEnabled(bool enabled);
|
||||
|
||||
// INT_STATUS register
|
||||
uint8_t getIntStatus();
|
||||
bool getIntFreefallStatus();
|
||||
bool getIntMotionStatus();
|
||||
bool getIntZeroMotionStatus();
|
||||
bool getIntFIFOBufferOverflowStatus();
|
||||
bool getIntI2CMasterStatus();
|
||||
bool getIntDataReadyStatus();
|
||||
|
||||
// ACCEL_*OUT_* registers
|
||||
void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
|
||||
void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
|
||||
void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
|
||||
int16_t getAccelerationX();
|
||||
int16_t getAccelerationY();
|
||||
int16_t getAccelerationZ();
|
||||
|
||||
// TEMP_OUT_* registers
|
||||
int16_t getTemperature();
|
||||
|
||||
// GYRO_*OUT_* registers
|
||||
void getRotation(int16_t* x, int16_t* y, int16_t* z);
|
||||
int16_t getRotationX();
|
||||
int16_t getRotationY();
|
||||
int16_t getRotationZ();
|
||||
|
||||
// EXT_SENS_DATA_* registers
|
||||
uint8_t getExternalSensorByte(int position);
|
||||
uint16_t getExternalSensorWord(int position);
|
||||
uint32_t getExternalSensorDWord(int position);
|
||||
|
||||
// MOT_DETECT_STATUS register
|
||||
uint8_t getMotionStatus();
|
||||
bool getXNegMotionDetected();
|
||||
bool getXPosMotionDetected();
|
||||
bool getYNegMotionDetected();
|
||||
bool getYPosMotionDetected();
|
||||
bool getZNegMotionDetected();
|
||||
bool getZPosMotionDetected();
|
||||
bool getZeroMotionDetected();
|
||||
|
||||
// I2C_SLV*_DO register
|
||||
void setSlaveOutputByte(uint8_t num, uint8_t data);
|
||||
|
||||
// I2C_MST_DELAY_CTRL register
|
||||
bool getExternalShadowDelayEnabled();
|
||||
void setExternalShadowDelayEnabled(bool enabled);
|
||||
bool getSlaveDelayEnabled(uint8_t num);
|
||||
void setSlaveDelayEnabled(uint8_t num, bool enabled);
|
||||
|
||||
// SIGNAL_PATH_RESET register
|
||||
void resetGyroscopePath();
|
||||
void resetAccelerometerPath();
|
||||
void resetTemperaturePath();
|
||||
|
||||
// MOT_DETECT_CTRL register
|
||||
uint8_t getAccelerometerPowerOnDelay();
|
||||
void setAccelerometerPowerOnDelay(uint8_t delay);
|
||||
uint8_t getFreefallDetectionCounterDecrement();
|
||||
void setFreefallDetectionCounterDecrement(uint8_t decrement);
|
||||
uint8_t getMotionDetectionCounterDecrement();
|
||||
void setMotionDetectionCounterDecrement(uint8_t decrement);
|
||||
|
||||
// USER_CTRL register
|
||||
bool getFIFOEnabled();
|
||||
void setFIFOEnabled(bool enabled);
|
||||
bool getI2CMasterModeEnabled();
|
||||
void setI2CMasterModeEnabled(bool enabled);
|
||||
void switchSPIEnabled(bool enabled);
|
||||
void resetFIFO();
|
||||
void resetI2CMaster();
|
||||
void resetSensors();
|
||||
|
||||
// PWR_MGMT_1 register
|
||||
void reset();
|
||||
bool getSleepEnabled();
|
||||
void setSleepEnabled(bool enabled);
|
||||
bool getWakeCycleEnabled();
|
||||
void setWakeCycleEnabled(bool enabled);
|
||||
bool getTempSensorEnabled();
|
||||
void setTempSensorEnabled(bool enabled);
|
||||
uint8_t getClockSource();
|
||||
void setClockSource(uint8_t source);
|
||||
|
||||
// PWR_MGMT_2 register
|
||||
uint8_t getWakeFrequency();
|
||||
void setWakeFrequency(uint8_t frequency);
|
||||
bool getStandbyXAccelEnabled();
|
||||
void setStandbyXAccelEnabled(bool enabled);
|
||||
bool getStandbyYAccelEnabled();
|
||||
void setStandbyYAccelEnabled(bool enabled);
|
||||
bool getStandbyZAccelEnabled();
|
||||
void setStandbyZAccelEnabled(bool enabled);
|
||||
bool getStandbyXGyroEnabled();
|
||||
void setStandbyXGyroEnabled(bool enabled);
|
||||
bool getStandbyYGyroEnabled();
|
||||
void setStandbyYGyroEnabled(bool enabled);
|
||||
bool getStandbyZGyroEnabled();
|
||||
void setStandbyZGyroEnabled(bool enabled);
|
||||
|
||||
// FIFO_COUNT_* registers
|
||||
uint16_t getFIFOCount();
|
||||
|
||||
// FIFO_R_W register
|
||||
uint8_t getFIFOByte();
|
||||
int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length);
|
||||
void setFIFOByte(uint8_t data);
|
||||
void getFIFOBytes(uint8_t *data, uint8_t length);
|
||||
void setFIFOTimeout(uint32_t fifoTimeout);
|
||||
uint32_t getFIFOTimeout();
|
||||
|
||||
// WHO_AM_I register
|
||||
uint8_t getDeviceID();
|
||||
void setDeviceID(uint8_t id);
|
||||
|
||||
// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
|
||||
|
||||
// XG_OFFS_TC register
|
||||
uint8_t getOTPBankValid();
|
||||
void setOTPBankValid(bool enabled);
|
||||
int8_t getXGyroOffsetTC();
|
||||
void setXGyroOffsetTC(int8_t offset);
|
||||
|
||||
// YG_OFFS_TC register
|
||||
int8_t getYGyroOffsetTC();
|
||||
void setYGyroOffsetTC(int8_t offset);
|
||||
|
||||
// ZG_OFFS_TC register
|
||||
int8_t getZGyroOffsetTC();
|
||||
void setZGyroOffsetTC(int8_t offset);
|
||||
|
||||
// X_FINE_GAIN register
|
||||
int8_t getXFineGain();
|
||||
void setXFineGain(int8_t gain);
|
||||
|
||||
// Y_FINE_GAIN register
|
||||
int8_t getYFineGain();
|
||||
void setYFineGain(int8_t gain);
|
||||
|
||||
// Z_FINE_GAIN register
|
||||
int8_t getZFineGain();
|
||||
void setZFineGain(int8_t gain);
|
||||
|
||||
// XA_OFFS_* registers
|
||||
int16_t getXAccelOffset();
|
||||
void setXAccelOffset(int16_t offset);
|
||||
|
||||
// YA_OFFS_* register
|
||||
int16_t getYAccelOffset();
|
||||
void setYAccelOffset(int16_t offset);
|
||||
|
||||
// ZA_OFFS_* register
|
||||
int16_t getZAccelOffset();
|
||||
void setZAccelOffset(int16_t offset);
|
||||
|
||||
// XG_OFFS_USR* registers
|
||||
int16_t getXGyroOffset();
|
||||
void setXGyroOffset(int16_t offset);
|
||||
|
||||
// YG_OFFS_USR* register
|
||||
int16_t getYGyroOffset();
|
||||
void setYGyroOffset(int16_t offset);
|
||||
|
||||
// ZG_OFFS_USR* register
|
||||
int16_t getZGyroOffset();
|
||||
void setZGyroOffset(int16_t offset);
|
||||
|
||||
// INT_ENABLE register (DMP functions)
|
||||
bool getIntPLLReadyEnabled();
|
||||
void setIntPLLReadyEnabled(bool enabled);
|
||||
bool getIntDMPEnabled();
|
||||
void setIntDMPEnabled(bool enabled);
|
||||
|
||||
// DMP_INT_STATUS
|
||||
bool getDMPInt5Status();
|
||||
bool getDMPInt4Status();
|
||||
bool getDMPInt3Status();
|
||||
bool getDMPInt2Status();
|
||||
bool getDMPInt1Status();
|
||||
bool getDMPInt0Status();
|
||||
|
||||
// INT_STATUS register (DMP functions)
|
||||
bool getIntPLLReadyStatus();
|
||||
bool getIntDMPStatus();
|
||||
|
||||
// USER_CTRL register (DMP functions)
|
||||
bool getDMPEnabled();
|
||||
void setDMPEnabled(bool enabled);
|
||||
void resetDMP();
|
||||
|
||||
// BANK_SEL register
|
||||
void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false);
|
||||
|
||||
// MEM_START_ADDR register
|
||||
void setMemoryStartAddress(uint8_t address);
|
||||
|
||||
// MEM_R_W register
|
||||
uint8_t readMemoryByte();
|
||||
void writeMemoryByte(uint8_t data);
|
||||
void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0);
|
||||
bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false);
|
||||
bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true);
|
||||
|
||||
bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false);
|
||||
bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
|
||||
|
||||
// DMP_CFG_1 register
|
||||
uint8_t getDMPConfig1();
|
||||
void setDMPConfig1(uint8_t config);
|
||||
|
||||
// DMP_CFG_2 register
|
||||
uint8_t getDMPConfig2();
|
||||
void setDMPConfig2(uint8_t config);
|
||||
|
||||
// Calibration Routines
|
||||
void CalibrateGyro(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
|
||||
void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops.
|
||||
void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math
|
||||
void PrintActiveOffsets(); // See the results of the Calibration
|
||||
|
||||
protected:
|
||||
uint8_t devAddr;
|
||||
void *wireObj;
|
||||
uint8_t buffer[14];
|
||||
uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT;
|
||||
};
|
||||
|
||||
#ifndef I2CDEVLIB_MPU6050_TYPEDEF
|
||||
#define I2CDEVLIB_MPU6050_TYPEDEF
|
||||
typedef MPU6050_Base MPU6050;
|
||||
#endif
|
||||
|
||||
#endif /* _MPU6050_H_ */
|
||||
|
|
@ -0,0 +1,153 @@
|
|||
// I2Cdev library collection - MPU6050 I2C device class
|
||||
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
|
||||
// 10/3/2011 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2021/09/27 - split implementations out of header files, finally
|
||||
// ... - ongoing debug release
|
||||
|
||||
// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
|
||||
// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF
|
||||
// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING.
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2012 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 _MPU6050_6AXIS_MOTIONAPPS20_H_
|
||||
#define _MPU6050_6AXIS_MOTIONAPPS20_H_
|
||||
|
||||
// take ownership of the "MPU6050" typedef
|
||||
#define I2CDEVLIB_MPU6050_TYPEDEF
|
||||
|
||||
#include "MPU6050.h"
|
||||
|
||||
class MPU6050_6Axis_MotionApps20 : public MPU6050_Base {
|
||||
public:
|
||||
MPU6050_6Axis_MotionApps20(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { }
|
||||
|
||||
uint8_t dmpInitialize();
|
||||
bool dmpPacketAvailable();
|
||||
|
||||
uint8_t dmpSetFIFORate(uint8_t fifoRate);
|
||||
uint8_t dmpGetFIFORate();
|
||||
uint8_t dmpGetSampleStepSizeMS();
|
||||
uint8_t dmpGetSampleFrequency();
|
||||
int32_t dmpDecodeTemperature(int8_t tempReg);
|
||||
|
||||
// Register callbacks after a packet of FIFO data is processed
|
||||
//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
|
||||
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
|
||||
uint8_t dmpRunFIFORateProcesses();
|
||||
|
||||
// Setup FIFO for various output
|
||||
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
|
||||
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
|
||||
uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
|
||||
// Get Fixed Point data from FIFO
|
||||
uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
|
||||
uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
|
||||
uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
|
||||
uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
|
||||
uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
|
||||
uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
|
||||
uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
|
||||
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
|
||||
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
|
||||
|
||||
uint8_t dmpGetEuler(float *data, Quaternion *q);
|
||||
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
|
||||
|
||||
// Get Floating Point data from FIFO
|
||||
uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
|
||||
uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
|
||||
|
||||
uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
|
||||
uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
|
||||
|
||||
uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
|
||||
|
||||
uint8_t dmpInitFIFOParam();
|
||||
uint8_t dmpCloseFIFO();
|
||||
uint8_t dmpSetGyroDataSource(uint8_t source);
|
||||
uint8_t dmpDecodeQuantizedAccel();
|
||||
uint32_t dmpGetGyroSumOfSquare();
|
||||
uint32_t dmpGetAccelSumOfSquare();
|
||||
void dmpOverrideQuaternion(long *q);
|
||||
uint16_t dmpGetFIFOPacketSize();
|
||||
uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof
|
||||
|
||||
private:
|
||||
uint8_t *dmpPacketBuffer;
|
||||
uint16_t dmpPacketSize;
|
||||
};
|
||||
|
||||
typedef MPU6050_6Axis_MotionApps20 MPU6050;
|
||||
|
||||
#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
|
||||
Binary file not shown.
345
communication/IMU/MPU6050_DMP6_edit/MPU6050_DMP6_edit.ino
Normal file
345
communication/IMU/MPU6050_DMP6_edit/MPU6050_DMP6_edit.ino
Normal file
|
|
@ -0,0 +1,345 @@
|
|||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
|
||||
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2019-07-08 - Added Auto Calibration and offset generator
|
||||
// - and altered FIFO retrieval sequence to avoid using blocking code
|
||||
// 2016-04-18 - Eliminated a potential infinite loop
|
||||
// 2013-05-08 - added seamless Fastwire support
|
||||
// - added note about gyro calibration
|
||||
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
|
||||
// 2012-06-20 - improved FIFO overflow handling and simplified read process
|
||||
// 2012-06-19 - completely rearranged DMP initialization code and simplification
|
||||
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
|
||||
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
|
||||
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
|
||||
// - add 3D math helper file to DMP6 example sketch
|
||||
// - add Euler output and Yaw/Pitch/Roll output formats
|
||||
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
|
||||
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
|
||||
// 2012-05-30 - basic DMP initialization working
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2012 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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 mpu;
|
||||
//MPU6050 mpu(0x69); // <-- use for AD0 high
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the MPU-6050's INT pin being connected to the Arduino's
|
||||
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
||||
digital I/O pin 2.
|
||||
* ========================================================================= */
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
||||
when using Serial.write(buf, len). The Teapot output uses this method.
|
||||
The solution requires a modification to the Arduino USBAPI.h file, which
|
||||
is fortunately simple, but annoying. This will be fixed in the next IDE
|
||||
release. For more info, see these links:
|
||||
|
||||
http://arduino.cc/forum/index.php/topic,109987.0.html
|
||||
http://code.google.com/p/arduino/issues/detail?id=958
|
||||
* ========================================================================= */
|
||||
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
//#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
//#define OUTPUT_TEAPOT
|
||||
|
||||
|
||||
|
||||
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
|
||||
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
||||
bool blinkState = false;
|
||||
|
||||
// MPU control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
||||
uint8_t 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
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
|
||||
// packet structure for InvenSense teapot demo
|
||||
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mpuInterrupt = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INITIAL SETUP ===
|
||||
// ================================================================
|
||||
|
||||
void setup() {
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (115200 chosen because it is required for Teapot Demo output, but it's
|
||||
// really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
||||
|
||||
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
|
||||
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
|
||||
// the baud timing being too misaligned with processor ticks. You must use
|
||||
// 38400 or slower in these cases, or use some kind of external separate
|
||||
// crystal solution for the UART timer.
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
mpu.initialize();
|
||||
pinMode(INTERRUPT_PIN, INPUT);
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
||||
|
||||
// wait for ready
|
||||
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
||||
while (Serial.available() && Serial.read()); // empty buffer
|
||||
while (!Serial.available()); // wait for data
|
||||
while (Serial.available() && Serial.read()); // empty buffer again
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
mpu.setXGyroOffset(220);
|
||||
mpu.setYGyroOffset(76);
|
||||
mpu.setZGyroOffset(-85);
|
||||
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
// Calibration Time: generate offsets and calibrate our MPU6050
|
||||
mpu.CalibrateAccel(6);
|
||||
mpu.CalibrateGyro(6);
|
||||
mpu.PrintActiveOffsets();
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
mpu.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
|
||||
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
|
||||
Serial.println(F(")..."));
|
||||
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// set our DMP Ready flag so the main loop() function knows it's okay to use it
|
||||
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
||||
dmpReady = true;
|
||||
|
||||
// get expected DMP packet size for later comparison
|
||||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
Serial.print(F("DMP Initialization failed (code "));
|
||||
Serial.print(devStatus);
|
||||
Serial.println(F(")"));
|
||||
}
|
||||
|
||||
// configure LED for output
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === MAIN PROGRAM LOOP ===
|
||||
// ================================================================
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
// read a packet from FIFO
|
||||
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer[0];
|
||||
teapotPacket[3] = fifoBuffer[1];
|
||||
teapotPacket[4] = fifoBuffer[4];
|
||||
teapotPacket[5] = fifoBuffer[5];
|
||||
teapotPacket[6] = fifoBuffer[8];
|
||||
teapotPacket[7] = fifoBuffer[9];
|
||||
teapotPacket[8] = fifoBuffer[12];
|
||||
teapotPacket[9] = fifoBuffer[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
}
|
||||
389
communication/IMU/MPU6050_DMP6_v1.1/MPU6050_DMP6_v1.1.ino
Normal file
389
communication/IMU/MPU6050_DMP6_v1.1/MPU6050_DMP6_v1.1.ino
Normal file
|
|
@ -0,0 +1,389 @@
|
|||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
|
||||
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2019-07-08 - Added Auto Calibration and offset generator
|
||||
// - and altered FIFO retrieval sequence to avoid using blocking code
|
||||
// 2016-04-18 - Eliminated a potential infinite loop
|
||||
// 2013-05-08 - added seamless Fastwire support
|
||||
// - added note about gyro calibration
|
||||
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
|
||||
// 2012-06-20 - improved FIFO overflow handling and simplified read process
|
||||
// 2012-06-19 - completely rearranged DMP initialization code and simplification
|
||||
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
|
||||
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
|
||||
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
|
||||
// - add 3D math helper file to DMP6 example sketch
|
||||
// - add Euler output and Yaw/Pitch/Roll output formats
|
||||
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
|
||||
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
|
||||
// 2012-05-30 - basic DMP initialization working
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2012 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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 mpu;
|
||||
MPU6050 mpu2(0x69); // <-- use for AD0 high
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the MPU-6050's INT pin being connected to the Arduino's
|
||||
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
||||
digital I/O pin 2.
|
||||
* ========================================================================= */
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
||||
when using Serial.write(buf, len). The Teapot output uses this method.
|
||||
The solution requires a modification to the Arduino USBAPI.h file, which
|
||||
is fortunately simple, but annoying. This will be fixed in the next IDE
|
||||
release. For more info, see these links:
|
||||
|
||||
http://arduino.cc/forum/index.php/topic,109987.0.html
|
||||
http://code.google.com/p/arduino/issues/detail?id=958
|
||||
* ========================================================================= */
|
||||
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
//#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
//#define OUTPUT_TEAPOT
|
||||
|
||||
|
||||
|
||||
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
|
||||
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
||||
bool blinkState = false;
|
||||
|
||||
// MPU control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
||||
|
||||
uint8_t mpu2IntStatus; // holds actual interrupt status byte from MPU
|
||||
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
||||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||
|
||||
uint16_t packetSize2; // 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
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
|
||||
float ypr2[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
// packet structure for InvenSense teapot demo
|
||||
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mpuInterrupt = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INITIAL SETUP ===
|
||||
// ================================================================
|
||||
|
||||
void setup() {
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (115200 chosen because it is required for Teapot Demo output, but it's
|
||||
// really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
||||
|
||||
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
|
||||
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
|
||||
// the baud timing being too misaligned with processor ticks. You must use
|
||||
// 38400 or slower in these cases, or use some kind of external separate
|
||||
// crystal solution for the UART timer.
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
mpu.initialize();
|
||||
|
||||
mpu2.initialize();
|
||||
pinMode(INTERRUPT_PIN, INPUT);
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
||||
|
||||
Serial.println(mpu2.testConnection() ? F("MPU6050 #2 connection successful") : F("MPU6050 #2 connection failed"));
|
||||
|
||||
// wait for ready
|
||||
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
||||
while (Serial.available() && Serial.read()); // empty buffer
|
||||
while (!Serial.available()); // wait for data
|
||||
while (Serial.available() && Serial.read()); // empty buffer again
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
devStatus = mpu2.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
mpu.setXGyroOffset(220);
|
||||
mpu.setYGyroOffset(76);
|
||||
mpu.setZGyroOffset(-85);
|
||||
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
mpu2.setXGyroOffset(220);
|
||||
mpu2.setYGyroOffset(76);
|
||||
mpu2.setZGyroOffset(-85);
|
||||
mpu2.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
// Calibration Time: generate offsets and calibrate our MPU6050
|
||||
mpu.CalibrateAccel(6);
|
||||
mpu.CalibrateGyro(6);
|
||||
mpu.PrintActiveOffsets();
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
mpu.setDMPEnabled(true);
|
||||
|
||||
mpu2.CalibrateAccel(6);
|
||||
mpu2.CalibrateGyro(6);
|
||||
mpu2.PrintActiveOffsets();
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP on 0x69..."));
|
||||
mpu2.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
|
||||
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
|
||||
Serial.println(F(")..."));
|
||||
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
mpu2IntStatus = mpu.getIntStatus();
|
||||
// set our DMP Ready flag so the main loop() function knows it's okay to use it
|
||||
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
||||
dmpReady = true;
|
||||
|
||||
// get expected DMP packet size for later comparison
|
||||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||||
|
||||
packetSize2 = mpu.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
Serial.print(F("DMP Initialization failed (code "));
|
||||
Serial.print(devStatus);
|
||||
Serial.println(F(")"));
|
||||
}
|
||||
|
||||
// configure LED for output
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === MAIN PROGRAM LOOP ===
|
||||
// ================================================================
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
// read a packet from FIFO
|
||||
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat2\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu2.dmpGetGravity(&gravity, &q);
|
||||
mpu2.dmpGetYawPitchRoll(ypr2, &q, &gravity);
|
||||
Serial.print("ypr2\t");
|
||||
Serial.print(ypr2[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr2[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr2[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer[0];
|
||||
teapotPacket[3] = fifoBuffer[1];
|
||||
teapotPacket[4] = fifoBuffer[4];
|
||||
teapotPacket[5] = fifoBuffer[5];
|
||||
teapotPacket[6] = fifoBuffer[8];
|
||||
teapotPacket[7] = fifoBuffer[9];
|
||||
teapotPacket[8] = fifoBuffer[12];
|
||||
teapotPacket[9] = fifoBuffer[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,6 +1,5 @@
|
|||
#include <Wire.h>
|
||||
//from https://create.arduino.cc/projecthub/MinukaThesathYapa/arduino-mpu6050-accelerometer-f92d8b
|
||||
//currently trying to tweak why the thing is increasing a lot :(
|
||||
//https://create.arduino.cc/projecthub/MinukaThesathYapa/arduino-mpu6050-accelerometer-f92d8b
|
||||
const int MPU = 0x68;
|
||||
float AccX, AccY, AccZ;
|
||||
float GyroX, GyroY, GyroZ;
|
||||
|
|
@ -9,14 +8,11 @@ float roll, pitch, yaw;
|
|||
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
|
||||
float elapsedTime, currentTime, previousTime;
|
||||
int c = 0;
|
||||
float roc_p = 0;
|
||||
float roc_y = 0;
|
||||
float roc_r = 0;
|
||||
int ticker = 1;
|
||||
float c_r = 0.02666666666666666666666666666666666;
|
||||
float c_p = 0.00734734734734734734734734734734734;
|
||||
float c_y = 0.040754754754754754754754754754755;
|
||||
|
||||
float p_p = 0;
|
||||
float p_y = 0;
|
||||
float p_r = 0;
|
||||
int ticker = 0;
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(19200);
|
||||
|
|
@ -59,22 +55,13 @@ void loop()
|
|||
yaw = yaw + GyroZ * elapsedTime;
|
||||
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
|
||||
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
|
||||
roll = roll*2;
|
||||
pitch = pitch*2;
|
||||
yaw = yaw*2;
|
||||
while(roll<0.0){roll+=360.0;}
|
||||
while(pitch<0.0){pitch+=360.0;}
|
||||
while(yaw<0.0){yaw+=360.0;}
|
||||
while(roll>=360.0){roll-=360.0;}
|
||||
while(pitch>=360.0){pitch-=360.0;}
|
||||
while(yaw>=360.0){yaw-=360.0;}
|
||||
Serial.print(roll+c_r*ticker);
|
||||
Serial.print("/");
|
||||
Serial.print(pitch+c_p*ticker);
|
||||
Serial.print("/");
|
||||
Serial.println(yaw+c_y*ticker);
|
||||
ticker++;
|
||||
|
||||
Serial.print(roll);
|
||||
Serial.print("/");
|
||||
Serial.print(pitch);
|
||||
Serial.print("/");
|
||||
Serial.println(yaw);
|
||||
if(ticker<100){ticker+=1;}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,232 @@
|
|||
//
|
||||
// MPU-6050 Mahony IMU (yaw angle is relative to starting orientation)
|
||||
// last update 7/9/2020
|
||||
//
|
||||
|
||||
#include "Wire.h"
|
||||
|
||||
// AD0 low = 0x68 (default for Sparkfun module)
|
||||
// AD0 high = 0x69
|
||||
int MPU_addr = 0x68;
|
||||
|
||||
// vvvvvvvvvvvvvvvvvv VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv
|
||||
//These are the previously determined offsets and scale factors for accelerometer and gyro for
|
||||
// a particular example of an MPU-6050. They are not correct for other examples.
|
||||
//The IMU code will NOT work well or at all if these are not correct
|
||||
|
||||
float A_cal[6] = {265.0, -80.0, -700.0, 0.994, 1.000, 1.014}; // 0..2 offset xyz, 3..5 scale xyz
|
||||
float G_off[3] = { -499.5, -17.7, -82.0}; //raw offsets, determined for gyro at rest
|
||||
#define gscale ((250./32768.0)*(PI/180.0)) //gyro default 250 LSB per d/s -> rad/s
|
||||
|
||||
// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
// GLOBALLY DECLARED, required for Mahony filter
|
||||
// vector to hold quaternion
|
||||
float q[4] = {1.0, 0.0, 0.0, 0.0};
|
||||
|
||||
// Free parameters in the Mahony filter and fusion scheme,
|
||||
// Kp for proportional feedback, Ki for integral
|
||||
float Kp = 30.0;
|
||||
float Ki = 0.0;
|
||||
|
||||
// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
|
||||
// with MPU-6050, some instability observed at Kp=100 Now set to 30.
|
||||
|
||||
// char s[60]; //snprintf buffer, if needed
|
||||
|
||||
// globals for AHRS loop timing
|
||||
unsigned long now_ms, last_ms = 0; //millis() timers
|
||||
|
||||
// print interval
|
||||
unsigned long print_ms = 200; //print angles every "print_ms" milliseconds
|
||||
float yaw, pitch, roll; //Euler angle output
|
||||
|
||||
void setup() {
|
||||
|
||||
Wire.begin();
|
||||
Serial.begin(9600);
|
||||
Serial.println("starting");
|
||||
|
||||
// initialize sensor
|
||||
// defaults for gyro and accel sensitivity are 250 dps and +/- 2 g
|
||||
Wire.beginTransmission(MPU_addr);
|
||||
Wire.write(0x6B); // PWR_MGMT_1 register
|
||||
Wire.write(0); // set to zero (wakes up the MPU-6050)
|
||||
Wire.endTransmission(true);
|
||||
|
||||
}
|
||||
|
||||
// AHRS loop
|
||||
|
||||
void loop()
|
||||
{
|
||||
static int i = 0;
|
||||
static float deltat = 0; //loop time in seconds
|
||||
static unsigned long now = 0, last = 0; //micros() timers
|
||||
|
||||
//raw data
|
||||
int16_t ax, ay, az;
|
||||
int16_t gx, gy, gz;
|
||||
int16_t Tmp; //temperature
|
||||
|
||||
//scaled data as vector
|
||||
float Axyz[3];
|
||||
float Gxyz[3];
|
||||
|
||||
|
||||
Wire.beginTransmission(MPU_addr);
|
||||
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
|
||||
Wire.endTransmission(false);
|
||||
Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
|
||||
int t = Wire.read() << 8;
|
||||
ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
|
||||
t = Wire.read() << 8;
|
||||
ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
|
||||
t = Wire.read() << 8;
|
||||
az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
|
||||
t = Wire.read() << 8;
|
||||
Tmp = t | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
|
||||
t = Wire.read() << 8;
|
||||
gx = t | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
|
||||
t = Wire.read() << 8;
|
||||
gy = t | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
|
||||
t = Wire.read() << 8;
|
||||
gz = t | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
|
||||
|
||||
Axyz[0] = (float) ax;
|
||||
Axyz[1] = (float) ay;
|
||||
Axyz[2] = (float) az;
|
||||
|
||||
//apply offsets and scale factors from Magneto
|
||||
for (int i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - A_cal[i]) * A_cal[i + 3];
|
||||
|
||||
Gxyz[0] = ((float) gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
|
||||
Gxyz[1] = ((float) gy - G_off[1]) * gscale;
|
||||
Gxyz[2] = ((float) gz - G_off[2]) * gscale;
|
||||
|
||||
// snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz);
|
||||
// Serial.println(s);
|
||||
|
||||
now = micros();
|
||||
deltat = (now - last) * 1.0e-6; //seconds since last update
|
||||
last = now;
|
||||
|
||||
Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);
|
||||
|
||||
// Compute Tait-Bryan angles.
|
||||
// In this coordinate system, the positive z-axis is down toward Earth.
|
||||
// Yaw is the angle between Sensor x-axis and Earth magnetic North
|
||||
// (or true North if corrected for local declination, looking down on the sensor
|
||||
// positive yaw is counterclockwise, which is not conventional for NED navigation.
|
||||
// Pitch is angle between sensor x-axis and Earth ground plane, toward the
|
||||
// Earth is positive, up toward the sky is negative. Roll is angle between
|
||||
// sensor y-axis and Earth ground plane, y-axis up is positive roll. These
|
||||
// arise from the definition of the homogeneous rotation matrix constructed
|
||||
// from quaternions. Tait-Bryan angles as well as Euler angles are
|
||||
// non-commutative; that is, the get the correct orientation the rotations
|
||||
// must be applied in the correct order which for this configuration is yaw,
|
||||
// pitch, and then roll.
|
||||
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
|
||||
// which has additional links.
|
||||
|
||||
roll = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
|
||||
pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
|
||||
//conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is.
|
||||
yaw = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));
|
||||
// to degrees
|
||||
yaw *= 180.0 / PI;
|
||||
if (yaw < 0) yaw += 360.0; //compass circle
|
||||
pitch *= 180.0 / PI;
|
||||
roll *= 180.0 / PI;
|
||||
|
||||
now_ms = millis(); //time to print?
|
||||
if (now_ms - last_ms >= print_ms) {
|
||||
last_ms = now_ms;
|
||||
// print angles for serial plotter...
|
||||
// Serial.print("ypr ");
|
||||
Serial.print(yaw, 2);
|
||||
Serial.print(", ");
|
||||
Serial.print(pitch, 2);
|
||||
Serial.print(", ");
|
||||
Serial.println(roll, 2);
|
||||
}
|
||||
}
|
||||
//--------------------------------------------------------------------------------------------------
|
||||
// Mahony scheme uses proportional and integral filtering on
|
||||
// the error between estimated reference vector (gravity) and measured one.
|
||||
// Madgwick's implementation of Mayhony's AHRS algorithm.
|
||||
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
// last update 07/09/2020 SJR minor edits
|
||||
//--------------------------------------------------------------------------------------------------
|
||||
// IMU algorithm update
|
||||
|
||||
void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
|
||||
float recipNorm;
|
||||
float vx, vy, vz;
|
||||
float ex, ey, ez; //error terms
|
||||
float qa, qb, qc;
|
||||
static float ix = 0.0, iy = 0.0, iz = 0.0; //integral feedback terms
|
||||
float tmp;
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
tmp = ax * ax + ay * ay + az * az;
|
||||
if (tmp > 0.0)
|
||||
{
|
||||
|
||||
// Normalise accelerometer (assumed to measure the direction of gravity in body frame)
|
||||
recipNorm = 1.0 / sqrt(tmp);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Estimated direction of gravity in the body frame (factor of two divided out)
|
||||
vx = q[1] * q[3] - q[0] * q[2];
|
||||
vy = q[0] * q[1] + q[2] * q[3];
|
||||
vz = q[0] * q[0] - 0.5f + q[3] * q[3];
|
||||
|
||||
// Error is cross product between estimated and measured direction of gravity in body frame
|
||||
// (half the actual magnitude)
|
||||
ex = (ay * vz - az * vy);
|
||||
ey = (az * vx - ax * vz);
|
||||
ez = (ax * vy - ay * vx);
|
||||
|
||||
// Compute and apply to gyro term the integral feedback, if enabled
|
||||
if (Ki > 0.0f) {
|
||||
ix += Ki * ex * deltat; // integral error scaled by Ki
|
||||
iy += Ki * ey * deltat;
|
||||
iz += Ki * ez * deltat;
|
||||
gx += ix; // apply integral feedback
|
||||
gy += iy;
|
||||
gz += iz;
|
||||
}
|
||||
|
||||
// Apply proportional feedback to gyro term
|
||||
gx += Kp * ex;
|
||||
gy += Kp * ey;
|
||||
gz += Kp * ez;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion, q cross gyro term
|
||||
deltat = 0.5 * deltat;
|
||||
gx *= deltat; // pre-multiply common factors
|
||||
gy *= deltat;
|
||||
gz *= deltat;
|
||||
qa = q[0];
|
||||
qb = q[1];
|
||||
qc = q[2];
|
||||
q[0] += (-qb * gx - qc * gy - q[3] * gz);
|
||||
q[1] += (qa * gx + qc * gz - q[3] * gy);
|
||||
q[2] += (qa * gy - qb * gz + q[3] * gx);
|
||||
q[3] += (qa * gz + qb * gy - qc * gx);
|
||||
|
||||
// renormalise quaternion
|
||||
recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
|
||||
q[0] = q[0] * recipNorm;
|
||||
q[1] = q[1] * recipNorm;
|
||||
q[2] = q[2] * recipNorm;
|
||||
q[3] = q[3] * recipNorm;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user