Yifei Teng
MPU6050 FreeIMU library
Dependents: FreeIMU FreeIMU_external_magnetometer
Fork of
MPU6050_tmp
by 
Aloïs Wolff
Async MPU6050 library
My port of the MPU6050 library samples the chip at 500Hz using Timer. Async I2C is achieved using a custom I2C library, which supports I2C calls from interrupts. Link given below:
Import libraryMODI2C
Improvements to Olieman's MODI2C library. Supports calls from IRQ.
Last commit 28 Mar 2018 by 
Yifei Teng
Difference between this port and the Arduino MPU6050 library
The getMotion6 function only returns a copy of the last obtained readings, which is sampled at a frequency of 500Hz (adjustable). Hence it can be called at any frequency without taxing the I2C.
Revision 9:d879deb55ae1, committed 2013-11-09
- Comitter:
- tyftyftyf
- Date:
- Sat Nov 09 08:51:07 2013 +0000
- Parent:
- 8:43f4b192e893
- Child:
- 10:11cc1b413f49
- Commit message:
- Implemented async mode
Changed in this revision
--- a/I2Cdev.cpp Tue Nov 05 11:28:54 2013 +0000
+++ b/I2Cdev.cpp Sat Nov 09 08:51:07 2013 +0000
@@ -3,6 +3,7 @@
// Changelog:
// 2013-01-08 - first release
+#include "MODI2C.h"
#include "I2Cdev.h"
//#define useDebugSerial
@@ -17,11 +18,6 @@
}
-I2Cdev::I2Cdev(I2C i2c_): debugSerial(USBTX, USBRX), i2c(i2c_)
-{
-
-}
-
/** Read a single bit from an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
@@ -136,16 +132,17 @@
{
char command[1];
command[0] = regAddr;
- char *redData = (char*)malloc(length);
- i2c.write(devAddr<<1, command, 1, true);
- i2c.read((devAddr<<1)+1, redData, length);
- for(int i =0; i < length; i++) {
- data[i] = redData[i];
- }
- free(redData);
+ i2c.write(devAddr, command, 1, true);
+ i2c.read(devAddr, (char*)data, length);
return length;
}
+void I2Cdev::readBytes_nb(uint8_t devAddr, char *command, uint8_t length, uint8_t *data, uint32_t (*function)(uint32_t), void* param)
+{
+ i2c.write(devAddr, command, 1, true);
+ i2c.read_nb(devAddr, (char*)data, length, function, param);
+}
+
int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout)
{
return 0;
@@ -257,18 +254,41 @@
return writeWords(devAddr, regAddr, 1, &data);
}
+uint32_t writefinhdl(uint32_t param){
+ //free((void*)param);
+ ((I2Cdev*)param)->freebuffer();
+ return 0;
+}
+
bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
- i2c.start();
- i2c.write(devAddr<<1);
- i2c.write(regAddr);
- for(int i = 0; i < length; i++) {
- i2c.write(data[i]);
- }
- i2c.stop();
+ //i2c.start();
+ //i2c.write(devAddr<<1);
+ //i2c.write(regAddr);
+ //for(int i = 0; i < length; i++) {
+ // i2c.write(data[i]);
+ //}
+ //i2c.stop();
+ char* mem = NULL;
+ mem = allocbuffer();//(char*)malloc(length+1);
+ *mem = regAddr;
+ memcpy(mem+1, data, length);
+
+ i2c.write(devAddr, mem, length+1, &writefinhdl, mem);
return true;
}
+char *I2Cdev::allocbuffer(){
+ char *buff = (char *)pool.alloc();
+ queue.put(buff);
+ return buff;
+}
+
+void I2Cdev::freebuffer(){
+ osEvent evt = queue.get(0);
+ pool.free((char(*)[20])evt.value.p);
+}
+
bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
{
return true;
--- a/I2Cdev.h Tue Nov 05 11:28:54 2013 +0000
+++ b/I2Cdev.h Sat Nov 09 08:51:07 2013 +0000
@@ -8,19 +8,22 @@
#ifndef I2Cdev_h
#define I2Cdev_h
-#include "mbed.h"
+#ifndef I2C_SDA
+ #define I2C_SDA p28
+ #define I2C_SCL p27
+#endif
-#define I2C_SDA p28
-#define I2C_SCL p27
+#include "mbed.h"
+#include "MODI2C.h"
class I2Cdev {
private:
- I2C i2c;
+
Serial debugSerial;
public:
I2Cdev();
I2Cdev(PinName i2cSda, PinName i2cScl);
- I2Cdev(I2C i2c_);
+ I2Cdev(MODI2C i2c_);
int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout());
int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout());
@@ -39,8 +42,18 @@
bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+ void readBytes_nb(uint8_t devAddr, char *command, uint8_t length, uint8_t *data, uint32_t (*function)(uint32_t), void* param);
static uint16_t readTimeout(void);
+
+ MODI2C i2c;
+
+ char* allocbuffer();
+ void freebuffer();
+
+ MemoryPool<char[20], 20> pool;
+ Queue<char, 20> queue;
};
#endif
\ No newline at end of file
--- a/MPU6050.cpp Tue Nov 05 11:28:54 2013 +0000
+++ b/MPU6050.cpp Sat Nov 09 08:51:07 2013 +0000
@@ -40,7 +40,9 @@
===============================================
*/
+#include "MODI2C.h"
#include "MPU6050.h"
+#include "rtos.h"
//#define useDebugSerial
@@ -54,12 +56,8 @@
*/
MPU6050::MPU6050() : debugSerial(USBTX, USBRX)
{
- devAddr = MPU6050_DEFAULT_ADDRESS;
-}
-
-MPU6050::MPU6050(I2C i2c) : debugSerial(USBTX, USBRX), i2Cdev(i2c)
-{
- devAddr = MPU6050_DEFAULT_ADDRESS;
+ devAddr = MPU6050_DEFAULT_ADDRESS << 1;
+ debugSerial.baud(115200);
}
/** Specific address constructor.
@@ -70,7 +68,8 @@
*/
MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX)
{
- devAddr = address;
+ devAddr = address << 1;
+ debugSerial.baud(115200);
}
/** Power on and prepare for general usage.
@@ -82,9 +81,7 @@
*/
void MPU6050::initialize()
{
-
#ifdef useDebugSerial
- debugSerial.baud(921600); //uses max serial speed
debugSerial.printf("MPU6050::initialize start\n");
#endif
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
@@ -1890,6 +1887,7 @@
getMotion6(ax, ay, az, gx, gy, gz);
// TODO: magnetometer integration
}
+
/** Get raw 6-axis motion sensor readings (accel/gyro).
* Retrieves all currently available motion sensor values.
* @param ax 16-bit signed integer container for accelerometer X-axis value
@@ -1904,14 +1902,34 @@
*/
void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
- i2Cdev.readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
- *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
- *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
- *az = (((int16_t)buffer[4]) << 8) | buffer[5];
- *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
- *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
- *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
+ *ax = ax_cache;
+ *ay = ay_cache;
+ *az = az_cache;
+ *gx = gx_cache;
+ *gy = gy_cache;
+ *gz = gz_cache;
}
+
+uint32_t mpureadfin(uint32_t param){
+ MPU6050* ins = (MPU6050*)param;
+ ins->ax_cache = (((int16_t)ins->mpu_buffer[0]) << 8) | ins->mpu_buffer[1];
+ ins->ay_cache = (((int16_t)ins->mpu_buffer[2]) << 8) | ins->mpu_buffer[3];
+ ins->az_cache = (((int16_t)ins->mpu_buffer[4]) << 8) | ins->mpu_buffer[5];
+ ins->gx_cache = (((int16_t)ins->mpu_buffer[8]) << 8) | ins->mpu_buffer[9];
+ ins->gy_cache = (((int16_t)ins->mpu_buffer[10]) << 8) | ins->mpu_buffer[11];
+ ins->gz_cache = (((int16_t)ins->mpu_buffer[12]) << 8) | ins->mpu_buffer[13];
+ return 0;
+}
+
+void MPU6050::mpu_sample_func(){
+ i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 14, (uint8_t*)mpu_buffer, &mpureadfin, this);
+}
+
+void MPU6050::start_sampling(){
+ mpu_cmd = MPU6050_RA_ACCEL_XOUT_H;
+ mpu_sampling.attach_us(this, &MPU6050::mpu_sample_func, 2000);
+}
+
/** Get 3-axis accelerometer readings.
* These registers store the most recent accelerometer measurements.
* Accelerometer measurements are written to these registers at the Sample Rate
--- a/MPU6050.h Tue Nov 05 11:28:54 2013 +0000
+++ b/MPU6050.h Sat Nov 09 08:51:07 2013 +0000
@@ -408,8 +408,17 @@
Serial debugSerial;
public:
MPU6050();
- MPU6050(I2C i2c);
+ MPU6050(MODI2C i2c);
MPU6050(uint8_t address);
+
+ void mpu_sample_func();
+ volatile int16_t ax_cache, ay_cache, az_cache, gx_cache, gy_cache, gz_cache;
+
+ Ticker mpu_sampling;
+ char mpu_cmd;
+ uint8_t mpu_buffer[14];
+
+ void start_sampling();
void initialize();
bool testConnection();