Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix for MagneticSensorI2C uses incorrect bit mask calculation #402 #412

Open
wants to merge 4 commits into
base: dev
Choose a base branch
from
+54 −71
Open

Fix for MagneticSensorI2C uses incorrect bit mask calculation #402 #412

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c28ad9d
fix for MagneticSensorI2C uses incorrect bit mask calculation #402402
runger1101001 Jun 10, 2024
87ea185
Merge branch 'simplefoc:dev' into dev
runger1101001 Jun 22, 2024
9281dce
Merge branch 'dev' of https://github.com/simplefoc/Arduino-FOC into dev
runger1101001 Jun 23, 2024
4395c22
Merge remote-tracking branch 'simplefoc/dev' into dev
runger1101001 Jul 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
99 changes: 43 additions & 56 deletions src/sensors/MagneticSensorI2C.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
};


Expand All @@ -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(){
Expand All @@ -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;
}

Expand Down
26 changes: 11 additions & 15 deletions src/sensors/MagneticSensorI2C.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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 */
Expand All @@ -76,8 +74,6 @@ class MagneticSensorI2C: public Sensor{

/* the two wire instance for this sensor */
TwoWire* wire;


};


Expand Down
Loading