From 0327384be6d1063d893066f895c45788da5f6b3f Mon Sep 17 00:00:00 2001 From: Qian Qian Date: Tue, 7 Nov 2023 15:06:27 +0100 Subject: [PATCH] Refactor to use ASSERT_SLAVE_ID_VALID --- Sming/Libraries/MPU6050/MPU6050.cpp | 154 ++++++++++++---------------- Sming/Libraries/MPU6050/MPU6050.h | 77 +++++++------- 2 files changed, 105 insertions(+), 126 deletions(-) diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp index 2ae84f87b9..6174f909e8 100644 --- a/Sming/Libraries/MPU6050/MPU6050.cpp +++ b/Sming/Libraries/MPU6050/MPU6050.cpp @@ -37,10 +37,19 @@ THE SOFTWARE. #include "MPU6050.h" #include +#include #define I2C_NUM I2C_NUM_0 using detail::concat; +namespace +{ +//Slave 4’s characteristics differ greatly from those of Slaves 0-3. +//Hence our API support only up to slave 3 +constexpr uint8_t MAX_SLAVE_ID{3}; +#define ASSERT_SLAVE_ID_VALID(slaveId) assert((slaveId <= MAX_SLAVE_ID)) +} // namespace + size_t MPU6050::Motion3::printTo(Print& p) const { size_t n{0}; @@ -109,117 +118,95 @@ uint8_t MPU6050::getGyroZSelfTestFactoryTrim() return (z & 0x1F); } -uint8_t MPU6050::getSlaveAddress(uint8_t num) +uint8_t MPU6050::getSlaveAddress(SlaveId slaveId) { - if(num > 3) { - return 0; - } - return readByte(MPU6050_RA_I2C_SLV0_ADDR + num * 3); + ASSERT_SLAVE_ID_VALID(slaveId); + return readByte(MPU6050_RA_I2C_SLV0_ADDR + slaveId * 3); } -void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) +void MPU6050::setSlaveAddress(SlaveId slaveId, uint8_t address) { - if(num > 3) { - return; - } - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, address); + ASSERT_SLAVE_ID_VALID(slaveId); + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + slaveId * 3, address); } -uint8_t MPU6050::getSlaveRegister(uint8_t num) +uint8_t MPU6050::getSlaveRegister(SlaveId slaveId) { - if(num > 3) { - return 0; - } - return readByte(MPU6050_RA_I2C_SLV0_REG + num * 3); + ASSERT_SLAVE_ID_VALID(slaveId); + return readByte(MPU6050_RA_I2C_SLV0_REG + slaveId * 3); } -void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) +void MPU6050::setSlaveRegister(SlaveId slaveId, uint8_t reg) { - if(num > 3) { - return; - } - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, reg); + ASSERT_SLAVE_ID_VALID(slaveId); + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + slaveId * 3, reg); } -bool MPU6050::getSlaveEnabled(uint8_t num) +bool MPU6050::getSlaveEnabled(SlaveId slaveId) { - if(num > 3) { - return false; - } - return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT); + ASSERT_SLAVE_ID_VALID(slaveId); + return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_EN_BIT); } -void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) +void MPU6050::setSlaveEnabled(SlaveId slaveId, bool enabled) { - if(num > 3) { - return; - } - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled); + ASSERT_SLAVE_ID_VALID(slaveId); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_EN_BIT, enabled); } -bool MPU6050::getSlaveWordByteSwap(uint8_t num) +bool MPU6050::getSlaveWordByteSwap(SlaveId slaveId) { - if(num > 3) { - return false; - } - return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT); + ASSERT_SLAVE_ID_VALID(slaveId); + return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_BYTE_SW_BIT); } -void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) +void MPU6050::setSlaveWordByteSwap(SlaveId slaveId, bool enabled) { - if(num > 3) { - return; - } - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled); + ASSERT_SLAVE_ID_VALID(slaveId); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled); } -bool MPU6050::getSlaveWriteMode(uint8_t num) +bool MPU6050::getSlaveWriteMode(SlaveId slaveId) { - if(num > 3) { - return false; - } - return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT); + ASSERT_SLAVE_ID_VALID(slaveId); + + return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_REG_DIS_BIT); } -void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) +void MPU6050::setSlaveWriteMode(SlaveId slaveId, bool mode) { - if(num > 3) { - return; - } - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode); + ASSERT_SLAVE_ID_VALID(slaveId); + + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode); } -bool MPU6050::getSlaveWordGroupOffset(uint8_t num) +bool MPU6050::getSlaveWordGroupOffset(SlaveId slaveId) { - if(num > 3) { - return false; - } - return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT); + ASSERT_SLAVE_ID_VALID(slaveId); + + return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_GRP_BIT); } -void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) +void MPU6050::setSlaveWordGroupOffset(SlaveId slaveId, bool enabled) { - if(num > 3) { - return; - } - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled); + ASSERT_SLAVE_ID_VALID(slaveId); + + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_GRP_BIT, enabled); } -uint8_t MPU6050::getSlaveDataLength(uint8_t num) +uint8_t MPU6050::getSlaveDataLength(SlaveId slaveId) { - if(num > 3) { - return false; - } - return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH); + ASSERT_SLAVE_ID_VALID(slaveId); + + return readBits(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH); } -void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) +void MPU6050::setSlaveDataLength(SlaveId slaveId, uint8_t length) { - if(num > 3) { - return; - } - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, - length); + ASSERT_SLAVE_ID_VALID(slaveId); + + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_LEN_BIT, + MPU6050_I2C_SLV_LEN_LENGTH, length); } MPU6050::Motion6 MPU6050::getMotion6() @@ -258,30 +245,17 @@ MPU6050::Motion3 MPU6050::getAngularRate() return angularRate; } -void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) +void MPU6050::setSlaveOutputByte(SlaveId slaveId, uint8_t data) { - if(num > 3) { - return; - } - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data); + ASSERT_SLAVE_ID_VALID(slaveId); + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + slaveId, data); } -bool MPU6050::getSlaveDelayEnabled(uint8_t num) +bool MPU6050::getSlaveDelayEnabled(SlaveId slaveId) { // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. - if(num > 4) { - return false; - } - return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, num); -} - -void MPU6050::getFIFOBytes(uint8_t* data, uint8_t length) -{ - if(length > 0) { - I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data); - } else { - *data = 0; - } + ASSERT_SLAVE_ID_VALID(slaveId); + return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, slaveId); } // XA_OFFS_* registers diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h index a775395ae8..cd6931d947 100644 --- a/Sming/Libraries/MPU6050/MPU6050.h +++ b/Sming/Libraries/MPU6050/MPU6050.h @@ -416,6 +416,8 @@ class MPU6050 { public: + using SlaveId = uint8_t; // (0 - 3) + struct Motion3 { int16_t x{}; int16_t y{}; @@ -1355,18 +1357,18 @@ class MPU6050 * Sample Rate or at the reduced rate is determined by the Delay Enable bits in * Register 103. * - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Current address for specified slave * @see MPU6050_RA_I2C_SLV0_ADDR */ - uint8_t getSlaveAddress(uint8_t num); + uint8_t getSlaveAddress(SlaveId slaveId); /** Set the I2C address of the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param address New address for specified slave * @see getSlaveAddress() * @see MPU6050_RA_I2C_SLV0_ADDR */ - void setSlaveAddress(uint8_t num, uint8_t address); + void setSlaveAddress(SlaveId slaveId, uint8_t address); /** Get the active internal register for the specified slave (0-3). * Read/write operations for this slave will be done to whatever internal @@ -1375,35 +1377,35 @@ class MPU6050 * The MPU-6050 supports a total of five slaves, but Slave 4 has unique * characteristics, and so it has its own functions. * - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Current active register for specified slave * @see MPU6050_RA_I2C_SLV0_REG */ - uint8_t getSlaveRegister(uint8_t num); + uint8_t getSlaveRegister(SlaveId slaveId); /** Set the active internal register for the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param reg New active register for specified slave * @see getSlaveRegister() * @see MPU6050_RA_I2C_SLV0_REG */ - void setSlaveRegister(uint8_t num, uint8_t reg); + void setSlaveRegister(SlaveId slaveId, uint8_t reg); /** Get the enabled value for the specified slave (0-3). * When set to 1, this bit enables Slave 0 for data transfer operations. When * cleared to 0, this bit disables Slave 0 from data transfer operations. - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Current enabled value for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ - bool getSlaveEnabled(uint8_t num); + bool getSlaveEnabled(SlaveId slaveId); /** Set the enabled value for the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param enabled New enabled value for specified slave * @see getSlaveEnabled() * @see MPU6050_RA_I2C_SLV0_CTRL */ - void setSlaveEnabled(uint8_t num, bool enabled); + void setSlaveEnabled(SlaveId slaveId, bool enabled); /** Get word pair byte-swapping enabled for the specified slave (0-3). * When set to 1, this bit enables byte swapping. When byte swapping is enabled, @@ -1412,19 +1414,19 @@ class MPU6050 * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA * registers in the order they were transferred. * - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Current word pair byte-swapping enabled value for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ - bool getSlaveWordByteSwap(uint8_t num); + bool getSlaveWordByteSwap(SlaveId slaveId); /** Set word pair byte-swapping enabled for the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param enabled New word pair byte-swapping enabled value for specified slave * @see getSlaveWordByteSwap() * @see MPU6050_RA_I2C_SLV0_CTRL */ - void setSlaveWordByteSwap(uint8_t num, bool enabled); + void setSlaveWordByteSwap(SlaveId slaveId, bool enabled); /** Get write mode for the specified slave (0-3). * When set to 1, the transaction will read or write data only. When cleared to @@ -1432,20 +1434,20 @@ class MPU6050 * data. This should equal 0 when specifying the register address within the * Slave device to/from which the ensuing data transaction will take place. * - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Current write mode for specified slave (0 = register address + data, * 1 = data only) * @see MPU6050_RA_I2C_SLV0_CTRL */ - bool getSlaveWriteMode(uint8_t num); + bool getSlaveWriteMode(SlaveId slaveId); /** Set write mode for the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param mode New write mode for specified slave (0 = register address + data, * 1 = data only) * @see getSlaveWriteMode() * @see MPU6050_RA_I2C_SLV0_CTRL */ - void setSlaveWriteMode(uint8_t num, bool mode); + void setSlaveWriteMode(SlaveId slaveId, bool mode); /** Get word pair grouping order offset for the specified slave (0-3). * This sets specifies the grouping order of word pairs received from registers. @@ -1454,36 +1456,36 @@ class MPU6050 * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even * register addresses) are paired to form a word. * - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Current word pair grouping order offset for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ - bool getSlaveWordGroupOffset(uint8_t num); + bool getSlaveWordGroupOffset(SlaveId slaveId); /** Set word pair grouping order offset for the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param enabled New word pair grouping order offset for specified slave * @see getSlaveWordGroupOffset() * @see MPU6050_RA_I2C_SLV0_CTRL */ - void setSlaveWordGroupOffset(uint8_t num, bool enabled); + void setSlaveWordGroupOffset(SlaveId slaveId, bool enabled); /** Get number of bytes to read for the specified slave (0-3). * Specifies the number of bytes transferred to and from Slave 0. Clearing this * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN. - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @return Number of bytes to read for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ - uint8_t getSlaveDataLength(uint8_t num); + uint8_t getSlaveDataLength(SlaveId slaveId); /** Set number of bytes to read for the specified slave (0-3). - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param length Number of bytes to read for specified slave * @see getSlaveDataLength() * @see MPU6050_RA_I2C_SLV0_CTRL */ - void setSlaveDataLength(uint8_t num, uint8_t length); + void setSlaveDataLength(SlaveId slaveId, uint8_t length); // I2C_SLV* registers (Slave 4) @@ -2484,11 +2486,11 @@ class MPU6050 * This register holds the output data written into Slave when Slave is set to * write mode. For further information regarding Slave control, please * refer to Registers 37 to 39 and immediately following. - * @param num Slave number (0-3) + * @param slaveId Slave ID (0-3) * @param data Byte to write * @see MPU6050_RA_I2C_SLV0_DO */ - void setSlaveOutputByte(uint8_t num, uint8_t data); + void setSlaveOutputByte(SlaveId slaveId, uint8_t data); // I2C_MST_DELAY_CTRL register /** Get external data shadow delay enabled status. @@ -2528,22 +2530,22 @@ class MPU6050 * For further information regarding the Sample Rate, please refer to * register 25. * - * @param num Slave number (0-4) + * @param slaveId Slave ID (0-4) * @return Current slave delay enabled status. * @see MPU6050_RA_I2C_MST_DELAY_CTRL * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT */ - bool getSlaveDelayEnabled(uint8_t num); + bool getSlaveDelayEnabled(SlaveId slaveId); /** Set slave delay enabled status. - * @param num Slave number (0-4) + * @param slaveId Slave ID (0-4) * @param enabled New slave delay enabled status. * @see MPU6050_RA_I2C_MST_DELAY_CTRL * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT */ - void setSlaveDelayEnabled(uint8_t num, bool enabled) + void setSlaveDelayEnabled(SlaveId slaveId, bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, slaveId, enabled); } // SIGNAL_PATH_RESET register @@ -3136,7 +3138,10 @@ class MPU6050 I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data); } - void getFIFOBytes(uint8_t* data, uint8_t length); + void getFIFOBytes(uint8_t* data, uint8_t length) + { + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data); + } // WHO_AM_I register