diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 2b3db0c2..64b7dd44 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -5,7 +5,10 @@ MagneticSensorI2CConfig_s AS5600_I2C = { .chip_address = 0x36, .bit_resolution = 12, .angle_register = 0x0C, - .data_start_bit = 11 + .msb_mask = 0x0F, + .msb_shift = 8, + .lsb_mask = 0xFF, + .lsb_shift = 0 }; /** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */ @@ -13,7 +16,10 @@ MagneticSensorI2CConfig_s AS5048_I2C = { .chip_address = 0x40, // highly configurable. if A1 and A2 are held low, this is probable value .bit_resolution = 14, .angle_register = 0xFE, - .data_start_bit = 15 + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0x3F, + .lsb_shift = 0 }; /** Typical configuration for the 12bit MT6701 magnetic sensor over I2C interface */ @@ -21,7 +27,10 @@ MagneticSensorI2CConfig_s MT6701_I2C = { .chip_address = 0x06, .bit_resolution = 14, .angle_register = 0x03, - .data_start_bit = 15 + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0xFC, + .lsb_shift = 2 }; @@ -30,57 +39,49 @@ MagneticSensorI2CConfig_s MT6701_I2C = { // @param _bit_resolution bit resolution of the sensor // @param _angle_register_msb angle read register // @param _bits_used_msb number of used bits in msb -MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ - // chip I2C address - chip_address = _chip_address; - // angle read register of the magnetic sensor - angle_register_msb = _angle_register_msb; - // register maximum value (counts per revolution) +MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb, bool lsb_right_aligned){ + _conf.chip_address = _chip_address; + _conf.bit_resolution = _bit_resolution; + _conf.angle_register = _angle_register_msb; + _conf.msb_mask = (uint8_t)( (1 << _bits_used_msb) - 1 ); + + uint8_t lsb_used = _bit_resolution - _bits_used_msb; // used bits in LSB + _conf.lsb_mask = (uint8_t)( (1 << (lsb_used)) - 1 ); + if (!lsb_right_aligned) + _conf.lsb_shift = 8-lsb_used; + else + _conf.lsb_shift = 0; + _conf.msb_shift = lsb_used; + cpr = _powtwo(_bit_resolution); - // depending on the sensor architecture there are different combinations of - // LSB and MSB register used bits - // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB - // MT6701 uses 0..5 LSB and 9..15 MSB - // used bits in LSB - lsb_used = _bit_resolution - _bits_used_msb; - // extraction masks - lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); - msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); wire = &Wire; } -MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ - chip_address = config.chip_address; - // angle read register of the magnetic sensor - angle_register_msb = config.angle_register; - // register maximum value (counts per revolution) - cpr = _powtwo(config.bit_resolution); - int bits_used_msb = config.data_start_bit - 7; - lsb_used = config.bit_resolution - bits_used_msb; - // extraction masks - lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); - msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 ); +MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ + _conf = config; + cpr = _powtwo(config.bit_resolution); wire = &Wire; } + + MagneticSensorI2C MagneticSensorI2C::AS5600() { return {AS5600_I2C}; } -void MagneticSensorI2C::init(TwoWire* _wire){ - - wire = _wire; - // I2C communication begin - wire->begin(); +void MagneticSensorI2C::init(TwoWire* _wire){ + wire = _wire; + wire->begin(); // I2C communication begin this->Sensor::init(); // call base class init } + + // Shaft angle calculation // angle is in radians [rad] float MagneticSensorI2C::getSensorAngle(){ @@ -90,39 +91,25 @@ float MagneticSensorI2C::getSensorAngle(){ -// function reading the raw counter of the magnetic sensor -int MagneticSensorI2C::getRawCount(){ - return (int)MagneticSensorI2C::read(angle_register_msb); -} - // I2C functions /* -* Read a register from the sensor -* Takes the address of the register as a uint8_t -* Returns the value of the register +* Read an angle from the angle register of the sensor */ -int MagneticSensorI2C::read(uint8_t angle_reg_msb) { +int MagneticSensorI2C::getRawCount() { // read the angle register first MSB then LSB byte readArray[2]; uint16_t readValue = 0; // notify the device that is aboout to be read - wire->beginTransmission(chip_address); - wire->write(angle_reg_msb); + wire->beginTransmission(_conf.chip_address); + wire->write(_conf.angle_register); currWireError = wire->endTransmission(false); - // read the data msb and lsb - wire->requestFrom(chip_address, (uint8_t)2); + wire->requestFrom(_conf.chip_address, 2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - - // depending on the sensor architecture there are different combinations of - // LSB and MSB register used bits - // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB - // MT6701 uses 0..5 LSB and 6..13 MSB - readValue = ( readArray[1] & lsb_mask ); - readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); + readValue = (readArray[0] & _conf.msb_mask) << _conf.msb_shift; + readValue |= (readArray[1] & _conf.lsb_mask) >> _conf.lsb_shift; return readValue; } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index f8b189fa..381a950a 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -8,13 +8,17 @@ #include "../common/time_utils.h" struct MagneticSensorI2CConfig_s { - int chip_address; - int bit_resolution; - int angle_register; - int data_start_bit; + uint8_t chip_address; + uint8_t bit_resolution; + uint8_t angle_register; + uint8_t msb_mask; + uint8_t msb_shift; + uint8_t lsb_mask; + uint8_t lsb_shift; }; + // some predefined structures -extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C; +extern MagneticSensorI2CConfig_s AS5600_I2C, AS5048_I2C, MT6701_I2C; #if defined(TARGET_RP2040) #define SDA I2C_SDA @@ -31,7 +35,7 @@ class MagneticSensorI2C: public Sensor{ * @param angle_register_msb angle read register msb * @param _bits_used_msb number of used bits in msb */ - MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used); + MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used, bool lsb_right_aligned = true); /** * MagneticSensorI2C class constructor @@ -56,13 +60,7 @@ class MagneticSensorI2C: public Sensor{ private: float cpr; //!< Maximum range of the magnetic sensor - uint16_t lsb_used; //!< Number of bits used in LSB register - uint8_t lsb_mask; - uint8_t msb_mask; - - // I2C variables - uint8_t angle_register_msb; //!< I2C angle register to read - uint8_t chip_address; //!< I2C chip select pins + MagneticSensorI2CConfig_s _conf; // I2C functions /** Read one I2C register value */ @@ -76,8 +74,6 @@ class MagneticSensorI2C: public Sensor{ /* the two wire instance for this sensor */ TwoWire* wire; - - };