fix for MagneticSensorI2C uses incorrect bit mask calculation #402402

Author: runger1101001

src/sensors/MagneticSensorI2C.cpp

@@ -5,23 +5,32 @@ 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 */
 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 */
 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;
 }
 

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;
-
-
 };