Skip to content

Commit

Permalink
Introduce readReg function and remove buffer member from class
Browse files Browse the repository at this point in the history
  • Loading branch information
@xeonqq
xeonqq committed Sep 11, 2023
1 parent 9ffd406 commit 2e34555
Show file tree
Hide file tree
Showing 2 changed files with 73 additions and 55 deletions.
91 changes: 42 additions & 49 deletions Sming/Libraries/MPU6050/MPU6050.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,7 @@ THE SOFTWARE.
#include <string.h>

#define I2C_NUM I2C_NUM_0

inline int16_t concat(uint8_t bits_15_8, uint8_t bits_7_0)
{
return (((int16_t)bits_15_8) << 8) | bits_7_0;
}
using detail::concat;

size_t MPU6050::Motion3::printTo(Print& p) const
{
Expand Down Expand Up @@ -214,7 +210,6 @@ void MPU6050::setExternalFrameSync(uint8_t sync)
uint8_t MPU6050::getDLPFMode()
{
return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH);
return buffer[0];
}
/** Set digital low-pass filter configuration.
* @param mode New DLFP configuration setting
Expand Down Expand Up @@ -296,6 +291,7 @@ uint8_t MPU6050::getAccelYSelfTestFactoryTrim()
*/
uint8_t MPU6050::getAccelZSelfTestFactoryTrim()
{
uint8_t buffer[2] = {0};
I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer);
return (buffer[0] >> 3) | (buffer[1] & 0x03);
}
Expand Down Expand Up @@ -1068,8 +1064,7 @@ bool MPU6050::getSlaveEnabled(uint8_t num)
if(num > 3) {
return false;
}
I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, buffer);
return buffer[0];
return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT);
}
/** Set the enabled value for the specified slave (0-3).
* @param num Slave number (0-3)
Expand Down Expand Up @@ -1100,8 +1095,7 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num)
if(num > 3) {
return false;
}
I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
return buffer[0];
return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
}
/** Set word pair byte-swapping enabled for the specified slave (0-3).
* @param num Slave number (0-3)
Expand Down Expand Up @@ -1132,8 +1126,7 @@ bool MPU6050::getSlaveWriteMode(uint8_t num)
if(num > 3) {
return false;
}
I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer);
return buffer[0];
return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
}
/** Set write mode for the specified slave (0-3).
* @param num Slave number (0-3)
Expand Down Expand Up @@ -1165,8 +1158,7 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
if(num > 3) {
return false;
}
I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, buffer);
return buffer[0];
return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT);
}
/** Set word pair grouping order offset for the specified slave (0-3).
* @param num Slave number (0-3)
Expand Down Expand Up @@ -1877,7 +1869,7 @@ bool MPU6050::getIntDataReadyStatus()
* Retrieves all currently available motion sensor values.
* @return container for 3-axis accelerometer and 3-axis gyroscope values
* @see getAcceleration()
* @see getRotation()
* @see getAngularRate()
* @see MPU6050_RA_ACCEL_XOUT_H
*/
MPU6050::Motion6 MPU6050::getMotion6()
Expand Down Expand Up @@ -1948,8 +1940,7 @@ MPU6050::Motion3 MPU6050::getAcceleration()
*/
int16_t MPU6050::getAccelerationX()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_ACCEL_XOUT_H);
}
/** Get Y-axis accelerometer reading.
* @return Y-axis acceleration measurement in 16-bit 2's complement format
Expand All @@ -1958,8 +1949,7 @@ int16_t MPU6050::getAccelerationX()
*/
int16_t MPU6050::getAccelerationY()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_ACCEL_YOUT_H);
}
/** Get Z-axis accelerometer reading.
* @return Z-axis acceleration measurement in 16-bit 2's complement format
Expand All @@ -1968,8 +1958,7 @@ int16_t MPU6050::getAccelerationY()
*/
int16_t MPU6050::getAccelerationZ()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_ACCEL_ZOUT_H);
}

// TEMP_OUT_* registers
Expand All @@ -1980,8 +1969,7 @@ int16_t MPU6050::getAccelerationZ()
*/
int16_t MPU6050::getTemperature()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_TEMP_OUT_H);
}

// GYRO_*OUT_* registers
Expand Down Expand Up @@ -2012,48 +2000,52 @@ int16_t MPU6050::getTemperature()
* 3 | +/- 2000 degrees/s | 16.4 LSB/deg/s
* </pre>
*
* @param x 16-bit signed integer container for X-axis rotation
* @param y 16-bit signed integer container for Y-axis rotation
* @param z 16-bit signed integer container for Z-axis rotation
* @return container for 3-axis gyro values
* @see getMotion6()
* @see MPU6050_RA_GYRO_XOUT_H
*/
void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z)
MPU6050::Motion3 MPU6050::getAngularRate()
{
Motion3 angularRate;
uint8_t buffer[6] = {0};
I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
*x = (((int16_t)buffer[0]) << 8) | buffer[1];
*y = (((int16_t)buffer[2]) << 8) | buffer[3];
*z = (((int16_t)buffer[4]) << 8) | buffer[5];
angularRate.x = concat(buffer[0], buffer[1]);
angularRate.y = concat(buffer[2], buffer[3]);
angularRate.z = concat(buffer[4], buffer[5]);
return angularRate;
}

/** Get X-axis gyroscope reading.
* @return X-axis rotation measurement in 16-bit 2's complement format
* @see getMotion6()
* @see MPU6050_RA_GYRO_XOUT_H
*/
int16_t MPU6050::getRotationX()
int16_t MPU6050::getAngularRateX()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_GYRO_XOUT_H);
}
/** Get Y-axis gyroscope reading.
* @return Y-axis rotation measurement in 16-bit 2's complement format
* @see getMotion6()
* @see MPU6050_RA_GYRO_YOUT_H
*/
int16_t MPU6050::getRotationY()
int16_t MPU6050::getAngularRateY()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_GYRO_YOUT_H);
}
/** Get Z-axis gyroscope reading.
* @return Z-axis rotation measurement in 16-bit 2's complement format
* @see getMotion6()
* @see MPU6050_RA_GYRO_ZOUT_H
*/
int16_t MPU6050::getRotationZ()
int16_t MPU6050::getAngularRateZ()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
}

int16_t MPU6050::getAngularRateZ2()
{
return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
}

// EXT_SENS_DATA_* registers
Expand Down Expand Up @@ -2143,8 +2135,7 @@ uint8_t MPU6050::getExternalSensorByte(int position)
*/
uint16_t MPU6050::getExternalSensorWord(int position)
{
I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
return (((uint16_t)buffer[0]) << 8) | buffer[1];
return readReg<uint16_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
}
/** Read double word (4 bytes) from external sensor data registers.
* @param position Starting position (0-20)
Expand All @@ -2153,8 +2144,7 @@ uint16_t MPU6050::getExternalSensorWord(int position)
*/
uint32_t MPU6050::getExternalSensorDWord(int position)
{
I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3];
return readReg<uint32_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
}

// MOT_DETECT_STATUS register
Expand Down Expand Up @@ -2855,6 +2845,7 @@ void MPU6050::setStandbyZGyroEnabled(bool enabled)
*/
uint16_t MPU6050::getFIFOCount()
{
uint8_t buffer[2] = {0};
I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
return (((uint16_t)buffer[0]) << 8) | buffer[1];
}
Expand Down Expand Up @@ -3014,6 +3005,7 @@ void MPU6050::setZFineGain(int8_t gain)

int16_t MPU6050::getXAccelOffset()
{
uint8_t buffer[2] = {0};
I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
Expand All @@ -3026,6 +3018,7 @@ void MPU6050::setXAccelOffset(int16_t offset)

int16_t MPU6050::getYAccelOffset()
{
uint8_t buffer[2] = {0};
I2Cdev::readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
Expand All @@ -3038,6 +3031,7 @@ void MPU6050::setYAccelOffset(int16_t offset)

int16_t MPU6050::getZAccelOffset()
{
uint8_t buffer[2] = {0};
I2Cdev::readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
}
Expand All @@ -3050,8 +3044,7 @@ void MPU6050::setZAccelOffset(int16_t offset)

int16_t MPU6050::getXGyroOffset()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_XG_OFFS_USRH);
}
void MPU6050::setXGyroOffset(int16_t offset)
{
Expand All @@ -3062,9 +3055,9 @@ void MPU6050::setXGyroOffset(int16_t offset)

int16_t MPU6050::getYGyroOffset()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_YG_OFFS_USRH);
}

void MPU6050::setYGyroOffset(int16_t offset)
{
I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
Expand All @@ -3074,9 +3067,9 @@ void MPU6050::setYGyroOffset(int16_t offset)

int16_t MPU6050::getZGyroOffset()
{
I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
return (((int16_t)buffer[0]) << 8) | buffer[1];
return readReg<int16_t>(MPU6050_RA_ZG_OFFS_USRH);
}

void MPU6050::setZGyroOffset(int16_t offset)
{
I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
Expand Down
37 changes: 31 additions & 6 deletions Sming/Libraries/MPU6050/MPU6050.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -642,7 +642,6 @@ class MPU6050
Motion6 getMotion6();

Motion3 getAcceleration();
void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
int16_t getAccelerationX();
int16_t getAccelerationY();
int16_t getAccelerationZ();
Expand All @@ -651,10 +650,11 @@ class MPU6050
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();
Motion3 getAngularRate();
int16_t getAngularRateX();
int16_t getAngularRateY();
int16_t getAngularRateZ();
int16_t getAngularRateZ2();

// EXT_SENS_DATA_* registers
uint8_t getExternalSensorByte(int position);
Expand Down Expand Up @@ -853,6 +853,31 @@ class MPU6050
uint8_t readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length);
uint8_t readByte(uint8_t regAddr);

template <typename T> T readReg(uint8_t regAddr);

uint8_t devAddr;
uint8_t buffer[14] = {0};
};

template <typename T> T MPU6050::readReg(uint8_t regAddr)
{
static_assert(std::is_fundamental<T>::value, "T must be an fundamental type.");

const auto sz = sizeof(T);
uint8_t buffer[sz] = {0};
//data follow big endien convention
I2Cdev::readBytes(devAddr, regAddr, sz, buffer);

T result{};
for(size_t i{0}; i < sz; ++i) {
result |= static_cast<T>(buffer[i]) << (8 * (sz - i - 1));
}
return result;
}

namespace detail
{
template <typename T = int16_t> inline T concat(uint8_t bits_15_8, uint8_t bits_7_0)
{
return (static_cast<T>(bits_15_8) << 8) | bits_7_0;
}
} // namespace detail

0 comments on commit 2e34555

Please sign in to comment.