diff --git a/src/SensorManager/BMX160/DFRobot_BMX160.cpp b/src/SensorManager/BMX160/DFRobot_BMX160.cpp index 73f46b42..ace06844 100644 --- a/src/SensorManager/BMX160/DFRobot_BMX160.cpp +++ b/src/SensorManager/BMX160/DFRobot_BMX160.cpp @@ -139,7 +139,68 @@ void DFRobot_BMX160::defaultParamSettg(sBmx160Dev_t *dev) dev->prevAccelCfg = dev->accelCfg; } -void DFRobot_BMX160::setMagnConf() +bool DFRobot_BMX160::setMagnConf() +{ + if (setBmi160AuxMagnConf()) { + LOG_INF("BMM150 magnetometer configured through BMI160 auxiliary interface"); + return true; + } + + LOG_WRN("BMM150 auxiliary probe failed; using BMX160 magnetometer interface fallback"); + setBmx160MagnConf(); + + return false; +} + +bool DFRobot_BMX160::setBmi160AuxMagnConf() +{ + uint8_t reg_val = 0; + uint8_t chip_id = 0; + + if (!writeBmxReg(BMX160_COMMAND_REG_ADDR, BMX160_MAGN_NORMAL_MODE)) { + return false; + } + delay(1); + + if (!readReg(BMX160_IF_CONF_ADDR, ®_val, 1)) { + return false; + } + + reg_val |= BMX160_IF_CONF_SECONDARY_IF_EN; + if (!writeBmxReg(BMX160_IF_CONF_ADDR, reg_val)) { + return false; + } + delay(BMX160_MAGN_COM_DELAY); + + if (!setBmi160AuxMode(true, 0x00)) { + return false; + } + + if (!writeBmm150Reg(BMM150_POWER_CONTROL_ADDR, BMM150_POWER_CONTROL_ENABLE)) { + return false; + } + delay(BMX160_MAGN_COM_DELAY); + + if (!readBmm150Reg(BMM150_CHIP_ID_ADDR, &chip_id) || chip_id != BMM150_CHIP_ID) { + LOG_WRN("BMM150 chip id mismatch: 0x%02x", chip_id); + return false; + } + + if (!writeBmm150Reg(BMM150_REP_XY_ADDR, BMM150_REP_XY_REGULAR) || + !writeBmm150Reg(BMM150_REP_Z_ADDR, BMM150_REP_Z_REGULAR) || + !writeBmm150Reg(BMM150_OP_MODE_ADDR, BMM150_OP_MODE_FORCED) || + !setBmi160AuxReadAddr(BMM150_DATA_X_LSB_ADDR) || + !writeBmxReg(BMX160_MAGN_CONFIG_ADDR, BMX160_MAGN_ODR_100HZ) || + !setBmi160AuxMode(false, 0x03)) { + return false; + } + + delay(50); + + return true; +} + +void DFRobot_BMX160::setBmx160MagnConf() { // puts magnetometer into mag_if setup mode writeBmxReg(BMX160_MAGN_IF_0_ADDR, 0x80); @@ -164,6 +225,47 @@ void DFRobot_BMX160::setMagnConf() delay(50); } +bool DFRobot_BMX160::setBmi160AuxMode(bool manual, uint8_t read_burst_len) +{ + uint8_t aux_if[2] = { + static_cast(BMX160_MAGN_BMM150_I2C_ADDR << 1), + static_cast((manual ? BMX160_MANUAL_MODE_EN_MSK : 0x00) | + (read_burst_len & BMX160_MAGN_READ_BURST_MSK)) + }; + + return writeReg(BMX160_AUX_IF_0_ADDR, aux_if, sizeof(aux_if)); +} + +bool DFRobot_BMX160::setBmi160AuxReadAddr(uint8_t reg) +{ + return writeBmxReg(BMX160_AUX_IF_2_ADDR, reg); +} + +bool DFRobot_BMX160::writeBmm150Reg(uint8_t reg, uint8_t value) +{ + if (!writeBmxReg(BMX160_AUX_IF_4_ADDR, value)) { + return false; + } + delay(BMX160_MAGN_COM_DELAY); + + if (!writeBmxReg(BMX160_AUX_IF_3_ADDR, reg)) { + return false; + } + delay(BMX160_MAGN_COM_DELAY); + + return true; +} + +bool DFRobot_BMX160::readBmm150Reg(uint8_t reg, uint8_t *value) +{ + if (!setBmi160AuxReadAddr(reg)) { + return false; + } + delay(BMX160_MAGN_COM_DELAY); + + return readReg(BMX160_MAG_DATA_ADDR, value, 1); +} + void DFRobot_BMX160::setGyroRange(eGyroRange_t bits){ switch (bits){ case eGyroRange_125DPS: @@ -254,13 +356,13 @@ void DFRobot_BMX160::getAllData(sBmx160SensorData_t *magn, sBmx160SensorData_t * } } -void DFRobot_BMX160::writeBmxReg(uint8_t reg, uint8_t value) +bool DFRobot_BMX160::writeBmxReg(uint8_t reg, uint8_t value) { uint8_t buffer[1] = {value}; - writeReg(reg, buffer, 1); + return writeReg(reg, buffer, 1); } -void DFRobot_BMX160::writeReg(uint8_t reg, uint8_t *pBuf, uint16_t len) +bool DFRobot_BMX160::writeReg(uint8_t reg, uint8_t *pBuf, uint16_t len) { _i2c->aquire(); @@ -268,9 +370,11 @@ void DFRobot_BMX160::writeReg(uint8_t reg, uint8_t *pBuf, uint16_t len) if (ret) LOG_WRN("I2C write failed: %d", ret); _i2c->release(); + + return ret == 0; } -void DFRobot_BMX160::readReg(uint8_t reg, uint8_t *pBuf, uint16_t len) +bool DFRobot_BMX160::readReg(uint8_t reg, uint8_t *pBuf, uint16_t len) { _i2c->aquire(); @@ -278,6 +382,8 @@ void DFRobot_BMX160::readReg(uint8_t reg, uint8_t *pBuf, uint16_t len) if (ret) LOG_WRN("I2C read failed: %d", ret); _i2c->release(); + + return ret == 0; } bool DFRobot_BMX160::scan() diff --git a/src/SensorManager/BMX160/DFRobot_BMX160.h b/src/SensorManager/BMX160/DFRobot_BMX160.h index b6585c90..f6f48a51 100644 --- a/src/SensorManager/BMX160/DFRobot_BMX160.h +++ b/src/SensorManager/BMX160/DFRobot_BMX160.h @@ -234,6 +234,27 @@ #define BMX160_SPI_COMM_TEST_ADDR 0x7F #define BMX160_INTL_PULLUP_CONF_ADDR 0x85 +/* BMI160-style auxiliary interface used by development BMX160/BMM150 hardware */ +#define BMX160_AUX_IF_0_ADDR 0x4B +#define BMX160_AUX_IF_1_ADDR 0x4C +#define BMX160_AUX_IF_2_ADDR 0x4D +#define BMX160_AUX_IF_3_ADDR 0x4E +#define BMX160_AUX_IF_4_ADDR 0x4F +#define BMX160_IF_CONF_SECONDARY_IF_EN 0x20 + +/* BMM150 registers behind the BMX160/BMI160 auxiliary interface */ +#define BMM150_CHIP_ID_ADDR 0x40 +#define BMM150_CHIP_ID 0x32 +#define BMM150_DATA_X_LSB_ADDR 0x42 +#define BMM150_POWER_CONTROL_ADDR 0x4B +#define BMM150_OP_MODE_ADDR 0x4C +#define BMM150_REP_XY_ADDR 0x51 +#define BMM150_REP_Z_ADDR 0x52 +#define BMM150_POWER_CONTROL_ENABLE 0x01 +#define BMM150_OP_MODE_FORCED 0x02 +#define BMM150_REP_XY_REGULAR 0x04 +#define BMM150_REP_Z_REGULAR 0x0E + /** Error code definitions */ #define BMX160_OK 0 #define BMX160_E_NULL_PTR -1 @@ -1072,7 +1093,7 @@ class DFRobot_BMX160{ * @param pBuf write the store and buffer of the data * @param len data length to be readed */ - void readReg(uint8_t reg, uint8_t *pBuf, uint16_t len); + bool readReg(uint8_t reg, uint8_t *pBuf, uint16_t len); /** * @fn writeReg @@ -1082,7 +1103,7 @@ class DFRobot_BMX160{ * @param len data length to be written * @return return the actually written length */ - void writeReg(uint8_t reg, uint8_t *pBuf, uint16_t len); + bool writeReg(uint8_t reg, uint8_t *pBuf, uint16_t len); /** * @fn writeBmxReg @@ -1091,7 +1112,7 @@ class DFRobot_BMX160{ * @param value Data written to the BMX register * @return return the actually written length */ - void writeBmxReg(uint8_t reg, uint8_t value); + bool writeBmxReg(uint8_t reg, uint8_t value); /** * @fn scan @@ -1106,7 +1127,13 @@ class DFRobot_BMX160{ * @fn setMagnConf * @brief set magnetometer Config */ - void setMagnConf(); + bool setMagnConf(); + void setBmx160MagnConf(); + bool setBmi160AuxMagnConf(); + bool setBmi160AuxMode(bool manual, uint8_t read_burst_len); + bool writeBmm150Reg(uint8_t reg, uint8_t value); + bool readBmm150Reg(uint8_t reg, uint8_t *value); + bool setBmi160AuxReadAddr(uint8_t reg); float accelRange = BMX160_ACCEL_MG_LSB_2G * EARTH_ACC; float gyroRange = BMX160_GYRO_SENSITIVITY_2000DPS;