66 - hallA, hallB, hallC - HallSensor A, B and C pins
77 - pp - pole pairs
88*/
9-
109HallSensor::HallSensor (int _hallA, int _hallB, int _hallC, int _pp){
1110
12- // HallSensor measurement structure init
1311 // hardware pins
1412 pinA = _hallA;
1513 pinB = _hallB;
1614 pinC = _hallC;
17- // counter setup
18- pulse_counter = 0 ;
19- pulse_timestamp = 0 ;
20-
21- cpr = _pp * 6 ; // hall has 6 segments per electrical revolution
22- A_active = 0 ;
23- B_active = 0 ;
24- C_active = 0 ;
25-
26- // velocity calculation variables
2715
28- prev_pulse_counter = 0 ;
29- prev_timestamp_us = _micros ();
16+ // hall has 6 segments per electrical revolution
17+ cpr = _pp * 6 ;
3018
3119 // extern pullup as default
3220 pullup = Pullup::EXTERN;
@@ -50,104 +38,89 @@ void HallSensor::handleC() {
5038 updateState ();
5139}
5240
41+ /* *
42+ * Updates the state and sector following an interrupt
43+ */
5344void HallSensor::updateState () {
54- int newState = C_active + (B_active << 1 ) + (A_active << 2 );
55- Direction direction = decodeDirection (state, newState);
56- state = newState;
57-
58- pulse_counter += direction;
59- pulse_timestamp = _micros ();
60- }
61-
62- // determines whether the hallsensr state transition means that it has moved one step CW (+1), CCW (-1) or state transition is invalid (0)
63- // states are 3bits, one for each hall sensor
64- Direction HallSensor::decodeDirection (int oldState, int newState)
65- {
66- // here are expected state transitions (oldState > newState).
67- // CW state transitions are: ( 6 > 2 > 3 > 1 > 5 > 4 > 6 )
68- // CCW state transitions are: ( 6 > 4 > 5 > 1 > 3 > 2 > 6 )
69- // invalid state transitions are oldState == newState or if newState or oldState == 0 | 7 as hallsensors can't be all on or all off
70-
71- int rawDirection;
72-
73- if (
74- (oldState == 6 && newState == 2 ) || \
75- (oldState == 2 && newState == 3 ) || \
76- (oldState == 3 && newState == 1 ) || \
77- (oldState == 1 && newState == 5 ) || \
78- (oldState == 5 && newState == 4 ) || \
79- (oldState == 4 && newState == 6 )
80- ) {
81- rawDirection = Direction::CW;
82- } else if (
83- (oldState == 6 && newState == 4 ) || \
84- (oldState == 4 && newState == 5 ) || \
85- (oldState == 5 && newState == 1 ) || \
86- (oldState == 1 && newState == 3 ) || \
87- (oldState == 3 && newState == 2 ) || \
88- (oldState == 2 && newState == 6 )
89- ) {
90- rawDirection = Direction::CCW;
45+ long new_pulse_timestamp = _micros ();
46+ hall_state = C_active + (B_active << 1 ) + (A_active << 2 );
47+ int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state];
48+ if (new_electric_sector - electric_sector > 3 ) {
49+ // underflow
50+ direction = static_cast <Direction>(natural_direction * -1 );
51+ electric_rotations += direction;
52+ } else if (new_electric_sector - electric_sector < (-3 )) {
53+ // overflow
54+ direction = static_cast <Direction>(natural_direction);
55+ electric_rotations += direction;
9156 } else {
92- rawDirection = Direction::UNKNOWN ;
57+ direction = (new_electric_sector > electric_sector)? static_cast < Direction>(natural_direction) : static_cast <Direction>(natural_direction * (- 1 )) ;
9358 }
59+ electric_sector = new_electric_sector;
60+ pulse_diff = new_pulse_timestamp - pulse_timestamp;
61+ pulse_timestamp = new_pulse_timestamp;
62+ total_interrupts++;
63+ if (onSectorChange != nullptr ) onSectorChange (electric_sector);
64+ }
9465
95- direction = static_cast <Direction>(rawDirection * natural_direction);
96- return direction; // * goofy;
66+ /* *
67+ * Optionally set a function callback to be fired when sector changes
68+ * void onSectorChange(int sector) {
69+ * ... // for debug or call driver directly?
70+ * }
71+ * sensor.attachSectorCallback(onSectorChange);
72+ */
73+ void HallSensor::attachSectorCallback (void (*_onSectorChange)(int sector)) {
74+ onSectorChange = _onSectorChange;
9775}
9876
9977/*
10078 Shaft angle calculation
10179*/
102- float HallSensor::getAngle (){
103-
104- long dN = pulse_counter - prev_pulse_counter;
105-
106- if (dN != 0 )
107- {
108-
109- // time from last impulse
110- float Th = (pulse_timestamp - prev_timestamp_us) * 1e-6 ;
111- if (Th <= 0 || Th > 0.5 )
112- Th = 1e-3 ;
113- // save variables for next pass
114- prev_timestamp_us = pulse_timestamp;
115- prev_pulse_counter = pulse_counter;
116- velocity = (float ) _2PI * dN / (cpr * Th);
117- }
118- angle = (float ) _2PI * pulse_counter / cpr;
119-
120- return angle;
80+ float HallSensor::getAngle () {
81+ return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI;
12182}
83+
12284/*
12385 Shaft velocity calculation
12486 function using mixed time and frequency measurement technique
12587*/
12688float HallSensor::getVelocity (){
127- // this is calculated during the last call to getAngle();
128- return velocity;
89+
90+ if (pulse_diff == 0 || ((_micros () - pulse_timestamp) > STALE_HALL_DATA_MICROS) ) { // last velocity isn't accurate if too old
91+ return 0 ;
92+ } else {
93+ return direction * (_2PI / cpr) / (pulse_diff / 1000000.0 );
94+ }
95+
12996}
13097
13198// getter for index pin
13299// return -1 if no index
133100int HallSensor::needsAbsoluteZeroSearch (){
134101 return 0 ;
135102}
136- // getter for index pin
103+
137104int HallSensor::hasAbsoluteZero (){
138- return 0 ;
105+ return 1 ;
139106}
140- // initialize counter to zero
107+
108+ // set current angle as zero angle
109+ // return the angle [rad] difference
141110float HallSensor::initRelativeZero (){
142-
143- pulse_counter = 0 ;
144- pulse_timestamp = _micros () ;
145- velocity = 0 ;
146- return 0.0 ;
111+
112+ // nothing to do. The interrupts should have changed sector.
113+ electric_rotations = 0 ;
114+ return 0 ;
115+
147116}
148- // initialize index to zero
117+
118+ // set absolute zero angle as zero angle
119+ // return the angle [rad] difference
149120float HallSensor::initAbsoluteZero (){
150- return 0.0 ;
121+
122+ return -getAngle ();
123+
151124}
152125
153126// HallSensor initialisation of the hardware pins
@@ -164,12 +137,14 @@ void HallSensor::init(){
164137 pinMode (pinB, INPUT);
165138 pinMode (pinC, INPUT);
166139 }
140+
141+ // init hall_state
142+ A_active= digitalRead (pinA);
143+ B_active = digitalRead (pinB);
144+ C_active = digitalRead (pinC);
145+ updateState ();
167146
168- // counter setup
169- pulse_counter = 0 ;
170147 pulse_timestamp = _micros ();
171- prev_pulse_counter = 0 ;
172- prev_timestamp_us = _micros ();
173148
174149}
175150
0 commit comments