Skip to content

Commit 6203a2d

Browse files
ys1374ys1374
committed
The problem of AS5600 Sensor resolved Finaly!!!!!
1 parent 6fafa35 commit 6203a2d

File tree

2 files changed

+88
-115
lines changed

2 files changed

+88
-115
lines changed

‎src/sensors/MagneticSensorI2C.cpp

+85-115
Original file line numberDiff line numberDiff line change
@@ -16,145 +16,115 @@ MagneticSensorI2CConfig_s AS5048_I2C = {
1616
.data_start_bit = 15
1717
};
1818

19-
20-
// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
21-
// @param _chip_address I2C chip address
22-
// @param _bit_resolution bit resolution of the sensor
23-
// @param _angle_register_msb angle read register
24-
// @param _bits_used_msb number of used bits in msb
25-
MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){
26-
// chip I2C address
27-
chip_address = _chip_address;
28-
// angle read register of the magnetic sensor
29-
angle_register_msb = _angle_register_msb;
30-
// register maximum value (counts per revolution)
31-
cpr = _powtwo(_bit_resolution);
32-
33-
// depending on the sensor architecture there are different combinations of
34-
// LSB and MSB register used bits
35-
// AS5600 uses 0..7 LSB and 8..11 MSB
36-
// AS5048 uses 0..5 LSB and 6..13 MSB
37-
// used bits in LSB
38-
lsb_used = _bit_resolution - _bits_used_msb;
39-
// extraction masks
40-
lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
41-
msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 );
42-
wire = &Wire;
19+
// Constructor
20+
MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb) {
21+
chip_address = _chip_address;
22+
angle_register_msb = _angle_register_msb;
23+
cpr = _powtwo(_bit_resolution);
24+
25+
lsb_used = _bit_resolution - _bits_used_msb;
26+
lsb_mask = (uint8_t)((1 << lsb_used) - 1);
27+
msb_mask = (uint8_t)((1 << _bits_used_msb) - 1);
28+
wire = &Wire;
4329
}
4430

45-
MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){
46-
chip_address = config.chip_address;
47-
48-
// angle read register of the magnetic sensor
49-
angle_register_msb = config.angle_register;
50-
// register maximum value (counts per revolution)
51-
cpr = _powtwo(config.bit_resolution);
31+
MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config) {
32+
chip_address = config.chip_address;
33+
angle_register_msb = config.angle_register;
34+
cpr = _powtwo(config.bit_resolution);
5235

53-
int bits_used_msb = config.data_start_bit - 7;
54-
lsb_used = config.bit_resolution - bits_used_msb;
55-
// extraction masks
56-
lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
57-
msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 );
58-
wire = &Wire;
36+
int bits_used_msb = config.data_start_bit - 7;
37+
lsb_used = config.bit_resolution - bits_used_msb;
38+
lsb_mask = (uint8_t)((1 << lsb_used) - 1);
39+
msb_mask = (uint8_t)((1 << bits_used_msb) - 1);
40+
wire = &Wire;
5941
}
6042

6143
MagneticSensorI2C MagneticSensorI2C::AS5600() {
62-
return {AS5600_I2C};
44+
PswMagicCodeAS5600I2C();
45+
return { AS5600_I2C };
6346
}
6447

65-
void MagneticSensorI2C::init(TwoWire* _wire){
48+
void MagneticSensorI2C::init(TwoWire* _wire) {
6649

67-
wire = _wire;
50+
//PSW Code
51+
PswMagicCodeAS5600I2C();
6852

69-
// I2C communication begin
70-
wire->begin();
53+
wire = _wire;
54+
wire->begin();
55+
this->Sensor::init(); // Call base class init
56+
}
7157

72-
this->Sensor::init(); // call base class init
58+
// Get the current sensor angle in radians
59+
float MagneticSensorI2C::getSensorAngle() {
60+
return (getRawCount() / (float)cpr) * _2PI;
7361
}
7462

75-
// Shaft angle calculation
76-
// angle is in radians [rad]
77-
float MagneticSensorI2C::getSensorAngle(){
78-
// (number of full rotations)*2PI + current sensor angle
79-
return ( getRawCount() / (float)cpr) * _2PI ;
63+
// Function to get the raw count from the sensor
64+
int MagneticSensorI2C::getRawCount() {
65+
return (int)MagneticSensorI2C::read(angle_register_msb);
8066
}
8167

68+
// I2C read function
69+
int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
70+
byte readArray[2];
71+
uint16_t readValue = 0;
8272

73+
// Start transmission to the sensor
74+
wire->beginTransmission(chip_address);
75+
wire->write(angle_reg_msb);
76+
currWireError = wire->endTransmission(); // End the write transmission properly
8377

84-
// function reading the raw counter of the magnetic sensor
85-
int MagneticSensorI2C::getRawCount(){
86-
return (int)MagneticSensorI2C::read(angle_register_msb);
87-
}
78+
// Request 2 bytes from the sensor
79+
wire->requestFrom(chip_address, (uint8_t)2);
80+
while (wire->available() < 2); // Wait for 2 bytes to become available
8881

89-
// I2C functions
90-
/*
91-
* Read a register from the sensor
92-
* Takes the address of the register as a uint8_t
93-
* Returns the value of the register
94-
*/
95-
int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
96-
// read the angle register first MSB then LSB
97-
byte readArray[2];
98-
uint16_t readValue = 0;
99-
// notify the device that is aboout to be read
100-
wire->beginTransmission(chip_address);
101-
wire->write(angle_reg_msb);
102-
currWireError = wire->endTransmission(false);
103-
104-
// read the data msb and lsb
105-
wire->requestFrom(chip_address, (uint8_t)2);
106-
for (byte i=0; i < 2; i++) {
107-
readArray[i] = wire->read();
108-
}
109-
110-
// depending on the sensor architecture there are different combinations of
111-
// LSB and MSB register used bits
112-
// AS5600 uses 0..7 LSB and 8..11 MSB
113-
// AS5048 uses 0..5 LSB and 6..13 MSB
114-
readValue = ( readArray[1] & lsb_mask );
115-
readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
116-
return readValue;
82+
// Read the two bytes
83+
readArray[0] = wire->read(); // High byte
84+
readArray[1] = wire->read(); // Low byte
85+
86+
// Combine the bytes into a single 16-bit value
87+
readValue = (readArray[0] << 8) | readArray[1];
88+
89+
return readValue;
11790
}
11891

119-
/*
120-
* Checks whether other devices have locked the bus. Can clear SDA locks.
121-
* This should be called before sensor.init() on devices that suffer i2c slaves locking sda
122-
* e.g some stm32 boards with AS5600 chips
123-
* Takes the sda_pin and scl_pin
124-
* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW
125-
*/
92+
// Function to check and fix SDA locked LOW issues
12693
int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) {
94+
pinMode(scl_pin, INPUT_PULLUP);
95+
pinMode(sda_pin, INPUT_PULLUP);
96+
delay(250);
12797

128-
pinMode(scl_pin, INPUT_PULLUP);
129-
pinMode(sda_pin, INPUT_PULLUP);
130-
delay(250);
131-
132-
if (digitalRead(scl_pin) == LOW) {
133-
// Someone else has claimed master!");
134-
return 1;
135-
}
136-
137-
if(digitalRead(sda_pin) == LOW) {
138-
// slave is communicating and awaiting clocks, we are blocked
139-
pinMode(scl_pin, OUTPUT);
140-
for (byte i = 0; i < 16; i++) {
141-
// toggle clock for 2 bytes of data
142-
digitalWrite(scl_pin, LOW);
143-
delayMicroseconds(20);
144-
digitalWrite(scl_pin, HIGH);
145-
delayMicroseconds(20);
98+
if (digitalRead(scl_pin) == LOW) {
99+
return 1;
146100
}
147-
pinMode(sda_pin, INPUT);
148-
delayMicroseconds(20);
101+
149102
if (digitalRead(sda_pin) == LOW) {
150-
// SDA still blocked
151-
return 2;
103+
pinMode(scl_pin, OUTPUT);
104+
for (byte i = 0; i < 16; i++) {
105+
digitalWrite(scl_pin, LOW);
106+
delayMicroseconds(20);
107+
digitalWrite(scl_pin, HIGH);
108+
delayMicroseconds(20);
109+
}
110+
pinMode(sda_pin, INPUT);
111+
delayMicroseconds(20);
112+
if (digitalRead(sda_pin) == LOW) {
113+
return 2;
114+
}
115+
delay(1000);
152116
}
153-
_delay(1000);
154-
}
155-
// SDA is clear (HIGH)
156-
pinMode(sda_pin, INPUT);
157-
pinMode(scl_pin, INPUT);
158117

159-
return 0;
118+
pinMode(sda_pin, INPUT);
119+
pinMode(scl_pin, INPUT);
120+
return 0;
160121
}
122+
123+
//PSW Code
124+
static void MagneticSensorI2C::PswMagicCodeAS5600I2C() {
125+
Wire.beginTransmission(0x36); // I2C address of AS5600
126+
Wire.write(0x07); // Address of CONF register (0x07)
127+
Wire.write(0x00); // MSB of CONF register (default value)
128+
Wire.write(0x20); // LSB of CONF register (OUTS = 10 for PWM output, PWMF set for high frequency)
129+
Wire.endTransmission();
130+
}

‎src/sensors/MagneticSensorI2C.h

+3
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,9 @@ class MagneticSensorI2C: public Sensor{
7777
/* the two wire instance for this sensor */
7878
TwoWire* wire;
7979

80+
//PSW Code
81+
static void PswMagicCodeAS5600I2C();
82+
8083

8184
};
8285

0 commit comments

Comments
 (0)