From 63dc03a3fcac010b8eee8776fdf004bfe9f942bd Mon Sep 17 00:00:00 2001 From: H1rono Date: Thu, 25 Jan 2024 16:14:54 +0900 Subject: [PATCH 1/2] Add library MPU6050 --- lib/MPU6050/MPU6050.cpp | 233 ++++++++++++++++++++++++++++++++++ lib/MPU6050/MPU6050.h | 270 ++++++++++++++++++++++++++++++++++++++++ lib/MPU6050/README.md | 3 + 3 files changed, 506 insertions(+) create mode 100644 lib/MPU6050/MPU6050.cpp create mode 100644 lib/MPU6050/MPU6050.h create mode 100644 lib/MPU6050/README.md diff --git a/lib/MPU6050/MPU6050.cpp b/lib/MPU6050/MPU6050.cpp new file mode 100644 index 0000000..47bd78a --- /dev/null +++ b/lib/MPU6050/MPU6050.cpp @@ -0,0 +1,233 @@ +/** + * Includes + */ +#include "MPU6050.h" + +MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { + this->setSleepMode(false); + + // Initializations: + currentGyroRange = 0; + currentAcceleroRange = 0; +} + +//-------------------------------------------------- +//-------------------General------------------------ +//-------------------------------------------------- + +void MPU6050::write(char address, char data) { + char temp[2]; + temp[0] = address; + temp[1] = data; + + connection.write(MPU6050_ADDRESS * 2, temp, 2); +} + +char MPU6050::read(char address) { + char retval; + connection.write(MPU6050_ADDRESS * 2, &address, 1, true); + connection.read(MPU6050_ADDRESS * 2, &retval, 1); + return retval; +} + +void MPU6050::read(char address, char *data, int length) { + connection.write(MPU6050_ADDRESS * 2, &address, 1, true); + connection.read(MPU6050_ADDRESS * 2, data, length); +} + +void MPU6050::setSleepMode(bool state) { + char temp; + temp = this->read(MPU6050_PWR_MGMT_1_REG); + if (state == true) temp |= 1 << MPU6050_SLP_BIT; + if (state == false) temp &= ~(1 << MPU6050_SLP_BIT); + this->write(MPU6050_PWR_MGMT_1_REG, temp); +} + +bool MPU6050::testConnection(void) { + char temp; + temp = this->read(MPU6050_WHO_AM_I_REG); + return (temp == (MPU6050_ADDRESS & 0xFE)); +} + +void MPU6050::setBW(char BW) { + char temp; + BW = BW & 0x07; + temp = this->read(MPU6050_CONFIG_REG); + temp &= 0xF8; + temp = temp + BW; + this->write(MPU6050_CONFIG_REG, temp); +} + +void MPU6050::setI2CBypass(bool state) { + char temp; + temp = this->read(MPU6050_INT_PIN_CFG); + if (state == true) temp |= 1 << MPU6050_BYPASS_BIT; + if (state == false) temp &= ~(1 << MPU6050_BYPASS_BIT); + this->write(MPU6050_INT_PIN_CFG, temp); +} + +//-------------------------------------------------- +//----------------Accelerometer--------------------- +//-------------------------------------------------- + +void MPU6050::setAcceleroRange(char range) { + char temp; + range = range & 0x03; + currentAcceleroRange = range; + + temp = this->read(MPU6050_ACCELERO_CONFIG_REG); + temp &= ~(3 << 3); + temp = temp + (range << 3); + this->write(MPU6050_ACCELERO_CONFIG_REG, temp); +} + +int MPU6050::getAcceleroRawX(void) { + short retval; + char data[2]; + this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +int MPU6050::getAcceleroRawY(void) { + short retval; + char data[2]; + this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +int MPU6050::getAcceleroRawZ(void) { + short retval; + char data[2]; + this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +void MPU6050::getAcceleroRaw(int *data) { + char temp[6]; + this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); + data[0] = (int)(short)((temp[0] << 8) + temp[1]); + data[1] = (int)(short)((temp[2] << 8) + temp[3]); + data[2] = (int)(short)((temp[4] << 8) + temp[5]); +} + +void MPU6050::getAccelero(float *data) { + int temp[3]; + this->getAcceleroRaw(temp); + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { + data[0] = (float)temp[0] / 16384.0 * 9.81; + data[1] = (float)temp[1] / 16384.0 * 9.81; + data[2] = (float)temp[2] / 16384.0 * 9.81; + } + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G) { + data[0] = (float)temp[0] / 8192.0 * 9.81; + data[1] = (float)temp[1] / 8192.0 * 9.81; + data[2] = (float)temp[2] / 8192.0 * 9.81; + } + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G) { + data[0] = (float)temp[0] / 4096.0 * 9.81; + data[1] = (float)temp[1] / 4096.0 * 9.81; + data[2] = (float)temp[2] / 4096.0 * 9.81; + } + if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G) { + data[0] = (float)temp[0] / 2048.0 * 9.81; + data[1] = (float)temp[1] / 2048.0 * 9.81; + data[2] = (float)temp[2] / 2048.0 * 9.81; + } + +#ifdef DOUBLE_ACCELERO + data[0] *= 2; + data[1] *= 2; + data[2] *= 2; +#endif +} + +//-------------------------------------------------- +//------------------Gyroscope----------------------- +//-------------------------------------------------- +void MPU6050::setGyroRange(char range) { + char temp; + currentGyroRange = range; + range = range & 0x03; + temp = this->read(MPU6050_GYRO_CONFIG_REG); + temp &= ~(3 << 3); + temp = temp + range << 3; + this->write(MPU6050_GYRO_CONFIG_REG, temp); +} + +int MPU6050::getGyroRawX(void) { + short retval; + char data[2]; + this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +int MPU6050::getGyroRawY(void) { + short retval; + char data[2]; + this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +int MPU6050::getGyroRawZ(void) { + short retval; + char data[2]; + this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +void MPU6050::getGyroRaw(int *data) { + char temp[6]; + this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); + data[0] = (int)(short)((temp[0] << 8) + temp[1]); + data[1] = (int)(short)((temp[2] << 8) + temp[3]); + data[2] = (int)(short)((temp[4] << 8) + temp[5]); +} + +void MPU6050::getGyro(float *data) { + int temp[3]; + this->getGyroRaw(temp); + if (currentGyroRange == MPU6050_GYRO_RANGE_250) { + data[0] = (float)temp[0] / 7505.7; + data[1] = (float)temp[1] / 7505.7; + data[2] = (float)temp[2] / 7505.7; + } + if (currentGyroRange == MPU6050_GYRO_RANGE_500) { + data[0] = (float)temp[0] / 3752.9; + data[1] = (float)temp[1] / 3752.9; + data[2] = (float)temp[2] / 3752.9; + } + if (currentGyroRange == MPU6050_GYRO_RANGE_1000) { + data[0] = (float)temp[0] / 1879.3; + ; + data[1] = (float)temp[1] / 1879.3; + data[2] = (float)temp[2] / 1879.3; + } + if (currentGyroRange == MPU6050_GYRO_RANGE_2000) { + data[0] = (float)temp[0] / 939.7; + data[1] = (float)temp[1] / 939.7; + data[2] = (float)temp[2] / 939.7; + } +} +//-------------------------------------------------- +//-------------------Temperature-------------------- +//-------------------------------------------------- +int MPU6050::getTempRaw(void) { + short retval; + char data[2]; + this->read(MPU6050_TEMP_H_REG, data, 2); + retval = (data[0] << 8) + data[1]; + return (int)retval; +} + +float MPU6050::getTemp(void) { + float retval; + retval = (float)this->getTempRaw(); + retval = (retval + 521.0) / 340.0 + 35.0; + return retval; +} diff --git a/lib/MPU6050/MPU6050.h b/lib/MPU6050/MPU6050.h new file mode 100644 index 0000000..529caa1 --- /dev/null +++ b/lib/MPU6050/MPU6050.h @@ -0,0 +1,270 @@ +// https://os.mbed.com/users/Sissors/code/MPU6050//file/5c63e20c50f3/MPU6050.h/ + +/*Use #define MPU6050_ES before you include this file if you have an engineering sample (older EVBs +will have them), to find out if you have one, check your accelerometer output. If it is half of what +you expected, and you still are on the correct planet, you got an engineering sample +*/ + +#ifndef MPU6050_H +#define MPU6050_H + +/** + * Includes + */ +#include "mbed.h" + +/** + * Defines + */ +#ifndef MPU6050_ADDRESS +#define MPU6050_ADDRESS 0x69 // address pin low (GND), default for InvenSense evaluation board +#endif + +#ifdef MPU6050_ES +#define DOUBLE_ACCELERO +#endif + +/** + * Registers + */ +#define MPU6050_CONFIG_REG 0x1A +#define MPU6050_GYRO_CONFIG_REG 0x1B +#define MPU6050_ACCELERO_CONFIG_REG 0x1C + +#define MPU6050_INT_PIN_CFG 0x37 + +#define MPU6050_ACCEL_XOUT_H_REG 0x3B +#define MPU6050_ACCEL_YOUT_H_REG 0x3D +#define MPU6050_ACCEL_ZOUT_H_REG 0x3F + +#define MPU6050_TEMP_H_REG 0x41 + +#define MPU6050_GYRO_XOUT_H_REG 0x43 +#define MPU6050_GYRO_YOUT_H_REG 0x45 +#define MPU6050_GYRO_ZOUT_H_REG 0x47 + +#define MPU6050_PWR_MGMT_1_REG 0x6B +#define MPU6050_WHO_AM_I_REG 0x75 + +/** + * Definitions + */ +#define MPU6050_SLP_BIT 6 +#define MPU6050_BYPASS_BIT 1 + +#define MPU6050_BW_256 0 +#define MPU6050_BW_188 1 +#define MPU6050_BW_98 2 +#define MPU6050_BW_42 3 +#define MPU6050_BW_20 4 +#define MPU6050_BW_10 5 +#define MPU6050_BW_5 6 + +#define MPU6050_ACCELERO_RANGE_2G 0 +#define MPU6050_ACCELERO_RANGE_4G 1 +#define MPU6050_ACCELERO_RANGE_8G 2 +#define MPU6050_ACCELERO_RANGE_16G 3 + +#define MPU6050_GYRO_RANGE_250 0 +#define MPU6050_GYRO_RANGE_500 1 +#define MPU6050_GYRO_RANGE_1000 2 +#define MPU6050_GYRO_RANGE_2000 3 + +/** MPU6050 IMU library. + * + * Example: + * @code + * Later, maybe + * @endcode + */ +class MPU6050 { +public: + /** + * Constructor. + * + * Sleep mode of MPU6050 is immediatly disabled + * + * @param sda - mbed pin to use for the SDA I2C line. + * @param scl - mbed pin to use for the SCL I2C line. + */ + MPU6050(PinName sda, PinName scl); + + /** + * Tests the I2C connection by reading the WHO_AM_I register. + * + * @return True for a working connection, false for an error + */ + bool testConnection(void); + + /** + * Sets the bandwidth of the digital low-pass filter + * + * Macros: MPU6050_BW_256 - MPU6050_BW_188 - MPU6050_BW_98 - MPU6050_BW_42 - MPU6050_BW_20 - + * MPU6050_BW_10 - MPU6050_BW_5 Last number is the gyro's BW in Hz (accelero BW is virtually + * identical) + * + * @param BW - The three bits that set the bandwidth (use the predefined macros) + */ + void setBW(char BW); + + /** + * Sets the auxiliary I2C bus in bypass mode to read the sensors behind the MPU6050 (useful for + * eval board, otherwise just connect them to primary I2C bus) + * + * @param state - Enables/disables the I2C bypass mode + */ + void setI2CBypass(bool state); + + /** + * Sets the Accelero full-scale range + * + * Macros: MPU6050_ACCELERO_RANGE_2G - MPU6050_ACCELERO_RANGE_4G - MPU6050_ACCELERO_RANGE_8G - + * MPU6050_ACCELERO_RANGE_16G + * + * @param range - The two bits that set the full-scale range (use the predefined macros) + */ + void setAcceleroRange(char range); + + /** + * Reads the accelero x-axis. + * + * @return 16-bit signed integer x-axis accelero data + */ + int getAcceleroRawX(void); + + /** + * Reads the accelero y-axis. + * + * @return 16-bit signed integer y-axis accelero data + */ + int getAcceleroRawY(void); + + /** + * Reads the accelero z-axis. + * + * @return 16-bit signed integer z-axis accelero data + */ + int getAcceleroRawZ(void); + + /** + * Reads all accelero data. + * + * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, + * data[2] = Z + */ + void getAcceleroRaw(int *data); + + /** + * Reads all accelero data, gives the acceleration in m/s2 + * + * Function uses the last setup value of the full scale range, if you manually set in another + * range, this won't work. + * + * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z + */ + void getAccelero(float *data); + + /** + * Sets the Gyro full-scale range + * + * Macros: MPU6050_GYRO_RANGE_250 - MPU6050_GYRO_RANGE_500 - MPU6050_GYRO_RANGE_1000 - + * MPU6050_GYRO_RANGE_2000 + * + * @param range - The two bits that set the full-scale range (use the predefined macros) + */ + void setGyroRange(char range); + + /** + * Reads the gyro x-axis. + * + * @return 16-bit signed integer x-axis gyro data + */ + int getGyroRawX(void); + + /** + * Reads the gyro y-axis. + * + * @return 16-bit signed integer y-axis gyro data + */ + int getGyroRawY(void); + + /** + * Reads the gyro z-axis. + * + * @return 16-bit signed integer z-axis gyro data + */ + int getGyroRawZ(void); + + /** + * Reads all gyro data. + * + * @param data - pointer to signed integer array with length three: data[0] = X, data[1] = Y, + * data[2] = Z + */ + void getGyroRaw(int *data); + + /** + * Reads all gyro data, gives the gyro in rad/s + * + * Function uses the last setup value of the full scale range, if you manually set in another + * range, this won't work. + * + * @param data - pointer to float array with length three: data[0] = X, data[1] = Y, data[2] = Z + */ + void getGyro(float *data); + + /** + * Reads temperature data. + * + * @return 16 bit signed integer with the raw temperature register value + */ + int getTempRaw(void); + + /** + * Returns current temperature + * + * @returns float with the current temperature + */ + float getTemp(void); + + /** + * Sets the sleep mode of the MPU6050 + * + * @param state - true for sleeping, false for wake up + */ + void setSleepMode(bool state); + + /** + * Writes data to the device, could be private, but public is handy so you can transmit directly + * to the MPU. + * + * @param adress - register address to write to + * @param data - data to write + */ + void write(char address, char data); + + /** + * Read data from the device, could be private, but public is handy so you can transmit directly + * to the MPU. + * + * @param adress - register address to write to + * @return - data from the register specified by RA + */ + char read(char adress); + + /** + * Read multtiple regigsters from the device, more efficient than using multiple normal reads. + * + * @param adress - register address to write to + * @param length - number of bytes to read + * @param data - pointer where the data needs to be written to + */ + void read(char adress, char *data, int length); + +private: + I2C connection; + char currentAcceleroRange; + char currentGyroRange; +}; + +#endif diff --git a/lib/MPU6050/README.md b/lib/MPU6050/README.md new file mode 100644 index 0000000..3e9aafb --- /dev/null +++ b/lib/MPU6050/README.md @@ -0,0 +1,3 @@ +# MPU6050 + +https://os.mbed.com/users/Sissors/code/MPU6050/ From 9b2981a452e95bbcc43903d93593605c31383719 Mon Sep 17 00:00:00 2001 From: H1rono Date: Thu, 25 Jan 2024 16:15:17 +0900 Subject: [PATCH 2/2] Remove platformio lib_deps --- platformio.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index ef43589..527634a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,6 +15,6 @@ framework = mbed platform_packages = platformio/toolchain-gccarmnoneeabi@^1.100301.220327 lib_deps = - mbed-sissors/MPU6050 + ; mbed-sissors/MPU6050 build_flags = -DMPU6050_ADDRESS=0x68