diff --git a/src/F_BMI160.cpp b/src/F_BMI160.cpp index 18ddb3a..596956b 100644 --- a/src/F_BMI160.cpp +++ b/src/F_BMI160.cpp @@ -277,4 +277,67 @@ void BMI160::calibrateAccelGyro(calData* cal) cal->gyroBias[1] = (float)gyro_bias[1]; cal->gyroBias[2] = (float)gyro_bias[2]; cal->valid = true; -} \ No newline at end of file +} + +int BMI160::setAccelODR(uint8_t odr) +{ + // BMI160 accel ODR encoding in ACC_CONF[3:0]: + // 0x06 = 25 Hz, 0x07 = 50 Hz, 0x08 = 100 Hz, 0x09 = 200 Hz + // 0x0A = 400 Hz, 0x0B = 800 Hz, 0x0C = 1600 Hz + if (odr < 0x06 || odr > 0x0C) + { + return -1; // Invalid ODR code + } + + uint8_t c = readByteI2C(wire, IMUAddress, BMI160_ACC_CONF); + c = (c & 0xF0) | (odr & 0x0F); // Clear [3:0], set new ODR + writeByteI2C(wire, IMUAddress, BMI160_ACC_CONF, c); + return 0; +} + +int BMI160::setGyroODR(uint8_t odr) +{ + // BMI160 gyro ODR encoding in GYR_CONF[3:0]: + // 0x06 = 25 Hz, 0x07 = 50 Hz, 0x08 = 100 Hz, 0x09 = 200 Hz + // 0x0A = 400 Hz, 0x0B = 800 Hz, 0x0C = 1600 Hz, 0x0D = 3200 Hz (gyro only) + if (odr < 0x06 || odr > 0x0D) + { + return -1; // Invalid ODR code + } + + uint8_t c = readByteI2C(wire, IMUAddress, BMI160_GYR_CONF); + c = (uint8_t)((c & 0xF0) | (odr & 0x0F)); // Clear [3:0], set new ODR + writeByteI2C(wire, IMUAddress, BMI160_GYR_CONF, c); + return 0; +} + +int BMI160::setAccelBandwidth(uint8_t bwp) +{ + // BMI160 accel bandwidth/OSR in ACC_CONF[6:4]: + // 0x00 = OSR4 (low noise), 0x01 = OSR2, 0x02 = Normal, 0x03 = CIC, ... + // Consult the datasheet/driver for the exact enumeration you want to expose. + if (bwp > 0x07) + { + return -1; // Only 3 bits + } + + uint8_t c = readByteI2C(wire, IMUAddress, BMI160_ACC_CONF); + c = (uint8_t)((c & 0x8F) | ((bwp & 0x07) << 4)); // clear [6:4], set new BWP + writeByteI2C(wire, IMUAddress, BMI160_ACC_CONF, c); + return 0; +} + +int BMI160::setGyroBandwidth(uint8_t bwp) +{ + // BMI160 gyro bandwidth/OSR in GYR_CONF[5:4]: + // 0x00 = OSR4 (low noise), 0x01 = OSR2, 0x02 = Normal mode, 0x03 = CIC mode + if (bwp > 0x03) + { + return -1; // Invalid bandwidth parameter + } + + uint8_t c = readByteI2C(wire, IMUAddress, BMI160_GYR_CONF); + c = (c & 0x0F) | ((bwp & 0x03) << 4); // Preserve ODR bits [3:0], set bits [5:4] + writeByteI2C(wire, IMUAddress, BMI160_GYR_CONF, c); + return 0; +} diff --git a/src/F_BMI160.hpp b/src/F_BMI160.hpp index 268f3c4..c06bc0f 100644 --- a/src/F_BMI160.hpp +++ b/src/F_BMI160.hpp @@ -117,6 +117,10 @@ class BMI160 : public IMUBase { void getQuat(Quaternion* out) override {}; float getTemp() override { return temperature; }; + int setAccelODR(uint8_t odr); // Set accel ODR: 0x06=25Hz, 0x0A=400Hz, 0x0C=1600Hz + int setGyroODR(uint8_t odr); // Set gyro ODR: 0x06=25Hz, 0x0A=400Hz, 0x0C=1600Hz, 0x0D=3200Hz + int setAccelBandwidth(uint8_t bwp); // Set accel bandwidth: 0=normal, 1=RES_OSR4 + int setGyroBandwidth(uint8_t bwp); // Set gyro bandwidth/OSR: 0=OSR4, 1=OSR2, 2=normal, 3=CIC int setGyroRange(int range) override; int setAccelRange(int range) override; int setIMUGeometry(int index) override { geometryIndex = index; return 0; };