Skip to content

Commit 31ed492

Browse files
Merge pull request #424 from Candas1/hall_no_interrupt
Hall sensor without interrupt
2 parents c27bc4b + d7d4488 commit 31ed492

File tree

2 files changed

+20
-10
lines changed

2 files changed

+20
-10
lines changed

Diff for: src/sensors/HallSensor.cpp

+19-10
Original file line numberDiff line numberDiff line change
@@ -42,22 +42,21 @@ void HallSensor::handleC() {
4242
* Updates the state and sector following an interrupt
4343
*/
4444
void HallSensor::updateState() {
45-
long new_pulse_timestamp = _micros();
46-
4745
int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2);
4846

4947
// 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();
5351
hall_state = new_hall_state;
5452

5553
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) {
5756
//underflow
5857
direction = Direction::CCW;
5958
electric_rotations += direction;
60-
} else if (new_electric_sector - electric_sector < (-3)) {
59+
} else if (electric_sector_dif < (-3)) {
6160
//overflow
6261
direction = Direction::CW;
6362
electric_rotations += direction;
@@ -96,11 +95,19 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) {
9695
// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
9796
void HallSensor::update() {
9897
// 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+
100107
angle_prev_ts = pulse_timestamp;
101108
long last_electric_rotations = electric_rotations;
102109
int8_t last_electric_sector = electric_sector;
103-
interrupts();
110+
if (use_interrupt) interrupts();
104111
angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ;
105112
full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr);
106113
}
@@ -150,7 +157,7 @@ void HallSensor::init(){
150157
}
151158

152159
// init hall_state
153-
A_active= digitalRead(pinA);
160+
A_active = digitalRead(pinA);
154161
B_active = digitalRead(pinB);
155162
C_active = digitalRead(pinC);
156163
updateState();
@@ -169,4 +176,6 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){
169176
if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
170177
if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
171178
if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE);
179+
180+
use_interrupt = true;
172181
}

Diff for: src/sensors/HallSensor.h

+1
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ class HallSensor: public Sensor{
4747
int pinA; //!< HallSensor hardware pin A
4848
int pinB; //!< HallSensor hardware pin B
4949
int pinC; //!< HallSensor hardware pin C
50+
int use_interrupt; //!< True if interrupts have been attached
5051

5152
// HallSensor configuration
5253
Pullup pullup; //!< Configuration parameter internal or external pullups

0 commit comments

Comments
 (0)