@@ -42,22 +42,21 @@ void HallSensor::handleC() {
42
42
* Updates the state and sector following an interrupt
43
43
*/
44
44
void HallSensor::updateState () {
45
- long new_pulse_timestamp = _micros ();
46
-
47
45
int8_t new_hall_state = C_active + (B_active << 1 ) + (A_active << 2 );
48
46
49
47
// glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed
50
- if (new_hall_state == hall_state) {
51
- return ;
52
- }
48
+ if (new_hall_state == hall_state) return ;
49
+
50
+ long new_pulse_timestamp = _micros ();
53
51
hall_state = new_hall_state;
54
52
55
53
int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state];
56
- if (new_electric_sector - electric_sector > 3 ) {
54
+ int8_t electric_sector_dif = new_electric_sector - electric_sector;
55
+ if (electric_sector_dif > 3 ) {
57
56
// underflow
58
57
direction = Direction::CCW;
59
58
electric_rotations += direction;
60
- } else if (new_electric_sector - electric_sector < (-3 )) {
59
+ } else if (electric_sector_dif < (-3 )) {
61
60
// overflow
62
61
direction = Direction::CW;
63
62
electric_rotations += direction;
@@ -96,11 +95,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) {
96
95
// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
97
96
void HallSensor::update () {
98
97
// Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
99
- noInterrupts ();
98
+ if (use_interrupt){
99
+ noInterrupts ();
100
+ }else {
101
+ A_active = digitalRead (pinA);
102
+ B_active = digitalRead (pinB);
103
+ C_active = digitalRead (pinC);
104
+ updateState ();
105
+ }
106
+
100
107
angle_prev_ts = pulse_timestamp;
101
108
long last_electric_rotations = electric_rotations;
102
109
int8_t last_electric_sector = electric_sector;
103
- interrupts ();
110
+ if (use_interrupt) interrupts ();
104
111
angle_prev = ((float )((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float )cpr) * _2PI ;
105
112
full_rotations = (int32_t )((last_electric_rotations * 6 + last_electric_sector) / cpr);
106
113
}
@@ -150,7 +157,7 @@ void HallSensor::init(){
150
157
}
151
158
152
159
// init hall_state
153
- A_active= digitalRead (pinA);
160
+ A_active = digitalRead (pinA);
154
161
B_active = digitalRead (pinB);
155
162
C_active = digitalRead (pinC);
156
163
updateState ();
@@ -169,4 +176,6 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){
169
176
if (doA != nullptr ) attachInterrupt (digitalPinToInterrupt (pinA), doA, CHANGE);
170
177
if (doB != nullptr ) attachInterrupt (digitalPinToInterrupt (pinB), doB, CHANGE);
171
178
if (doC != nullptr ) attachInterrupt (digitalPinToInterrupt (pinC), doC, CHANGE);
179
+
180
+ use_interrupt = true ;
172
181
}
0 commit comments