Skip to content

Commit 7f2f5f5

Browse files
ModdingearModdingear
committed
add automatic hall sector detection sensor
1 parent ae40a7a commit 7f2f5f5

File tree

2 files changed

+371
-0
lines changed

2 files changed

+371
-0
lines changed

‎src/encoders/hall_any/HallSensorAny.cpp‎

Lines changed: 259 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,259 @@
1+
#include "HallSensorAny.h"
2+
#include "common/time_utils.h"
3+
#include "communication/SimpleFOCDebug.h"
4+
5+
6+
// seq 1 > 5 > 4 > 6 > 2 > 3 > 1 000 001 010 011 100 101 110 111
7+
const int8_t ELECTRIC_SECTORS[8] = { -1, 0, 4, 5, 2, 1, 3 , -1 };
8+
9+
/*
10+
HallSensorAny(int hallA, int hallB , int cpr, int index)
11+
- hallA, hallB, hallC - HallSensorAny A, B and C pins
12+
- pp - pole pairs
13+
*/
14+
HallSensorAny::HallSensorAny(int _hallA, int _hallB, int _hallC, int _pp, HallType _hall_type){
15+
16+
// hardware pins
17+
pinA = _hallA;
18+
pinB = _hallB;
19+
pinC = _hallC;
20+
hall_type = _hall_type;
21+
last_print_type = hall_type;
22+
for (size_t i = 0; i < sizeof(previous_states); i++)
23+
{
24+
previous_states[i] = -1;
25+
}
26+
27+
28+
// hall has 6 segments per electrical revolution
29+
cpr = _pp * 6;
30+
31+
// extern pullup as default
32+
pullup = Pullup::USE_EXTERN;
33+
}
34+
35+
// HallSensorAny interrupt callback functions
36+
// A channel
37+
void HallSensorAny::handleA() {
38+
A_active= digitalRead(pinA);
39+
updateState();
40+
}
41+
// B channel
42+
void HallSensorAny::handleB() {
43+
B_active = digitalRead(pinB);
44+
updateState();
45+
}
46+
47+
// C channel
48+
void HallSensorAny::handleC() {
49+
C_active = digitalRead(pinC);
50+
updateState();
51+
}
52+
53+
/**
54+
* Updates the state and sector following an interrupt
55+
*/
56+
void HallSensorAny::updateState() {
57+
int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2);
58+
// glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed
59+
if (new_hall_state == hall_state_raw) return;
60+
hall_state_raw = new_hall_state;
61+
62+
static const int num_previous_states = sizeof(previous_states);
63+
64+
//flip a line maybe
65+
if (hall_type != HallType::UNKNOWN)
66+
{
67+
new_hall_state ^= static_cast<int8_t>(hall_type);
68+
}
69+
70+
long new_pulse_timestamp = _micros();
71+
if (hall_type == HallType::UNKNOWN) //Store previous steps for hall config detection
72+
{
73+
for (int i = num_previous_states - 2; i >= 0; i--)
74+
{
75+
previous_states[i+1] = previous_states[i];
76+
}
77+
previous_states[0] = new_hall_state;
78+
//7 and 0 are illegal in 120deg mode, so we're gonna try to see which line hel up during that time and flip it so it doesn't happen
79+
if ((previous_states[1] == 0b111 || previous_states[1] == 0b000) && previous_states[2] != -1)
80+
{
81+
if (previous_states[2] == previous_states[0])
82+
{
83+
//went back, can't do anything
84+
}
85+
else
86+
{
87+
hall_type = static_cast<HallType>((0b111 - previous_states[0] ^ previous_states[2])%8);
88+
previous_states[0] ^= static_cast<int8_t>(hall_type);
89+
}
90+
}
91+
if (abs(electric_rotations) > 2)
92+
{
93+
hall_type = HallType::HALL_120;
94+
}
95+
}
96+
97+
98+
99+
100+
int8_t new_electric_sector;
101+
new_electric_sector = ELECTRIC_SECTORS[new_hall_state];
102+
int8_t electric_sector_dif = new_electric_sector - electric_sector;
103+
if (electric_sector_dif > 3) {
104+
//underflow
105+
direction = Direction::CCW;
106+
electric_rotations += direction;
107+
} else if (electric_sector_dif < (-3)) {
108+
//overflow
109+
direction = Direction::CW;
110+
electric_rotations += direction;
111+
} else {
112+
direction = (new_electric_sector > electric_sector)? Direction::CW : Direction::CCW;
113+
}
114+
electric_sector = new_electric_sector;
115+
116+
// glitch avoidance #2 changes in direction can cause velocity spikes. Possible improvements needed in this area
117+
if (direction == old_direction) {
118+
// not oscilating or just changed direction
119+
pulse_diff = new_pulse_timestamp - pulse_timestamp;
120+
} else {
121+
pulse_diff = 0;
122+
}
123+
124+
pulse_timestamp = new_pulse_timestamp;
125+
total_interrupts++;
126+
old_direction = direction;
127+
if (onSectorChange != nullptr) onSectorChange(electric_sector);
128+
}
129+
130+
/**
131+
* Optionally set a function callback to be fired when sector changes
132+
* void onSectorChange(int sector) {
133+
* ... // for debug or call driver directly?
134+
* }
135+
* sensor.attachSectorCallback(onSectorChange);
136+
*/
137+
void HallSensorAny::attachSectorCallback(void (*_onSectorChange)(int sector)) {
138+
onSectorChange = _onSectorChange;
139+
}
140+
141+
142+
143+
// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
144+
void HallSensorAny::update() {
145+
// Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
146+
if (use_interrupt){
147+
noInterrupts();
148+
}else{
149+
A_active = digitalRead(pinA);
150+
B_active = digitalRead(pinB);
151+
C_active = digitalRead(pinC);
152+
updateState();
153+
}
154+
155+
angle_prev_ts = pulse_timestamp;
156+
long last_electric_rotations = electric_rotations;
157+
int8_t last_electric_sector = electric_sector;
158+
if (use_interrupt) interrupts();
159+
angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ;
160+
full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr);
161+
if (last_print_type != hall_type)
162+
{
163+
last_print_type = hall_type;
164+
switch (hall_type)
165+
{
166+
case HallType::HALL_120 :
167+
SIMPLEFOC_DEBUG("HALL: Found type: HALL_120");
168+
break;
169+
case HallType::HALL_60A :
170+
SIMPLEFOC_DEBUG("HALL: Found type: HALL_60A");
171+
break;
172+
case HallType::HALL_60B :
173+
SIMPLEFOC_DEBUG("HALL: Found type: HALL_60B");
174+
break;
175+
case HallType::HALL_60C :
176+
SIMPLEFOC_DEBUG("HALL: Found type: HALL_60C");
177+
break;
178+
179+
default:
180+
SIMPLEFOC_DEBUG("HALL: Type unknown! Wtf!");
181+
break;
182+
}
183+
184+
}
185+
186+
}
187+
188+
189+
190+
/*
191+
Shaft angle calculation
192+
TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost
193+
*/
194+
float HallSensorAny::getSensorAngle() {
195+
return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ;
196+
}
197+
198+
/*
199+
Shaft velocity calculation
200+
function using mixed time and frequency measurement technique
201+
*/
202+
float HallSensorAny::getVelocity(){
203+
noInterrupts();
204+
long last_pulse_timestamp = pulse_timestamp;
205+
long last_pulse_diff = pulse_diff;
206+
interrupts();
207+
if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old
208+
return 0;
209+
} else {
210+
return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f);
211+
}
212+
213+
}
214+
215+
int HallSensorAny::needsSearch()
216+
{
217+
return hall_type == HallType::UNKNOWN;
218+
}
219+
220+
// HallSensorAny initialisation of the hardware pins
221+
// and calculation variables
222+
void HallSensorAny::init(){
223+
// initialise the electrical rotations to 0
224+
electric_rotations = 0;
225+
226+
// HallSensorAny - check if pullup needed for your HallSensorAny
227+
if(pullup == Pullup::USE_INTERN){
228+
pinMode(pinA, INPUT_PULLUP);
229+
pinMode(pinB, INPUT_PULLUP);
230+
pinMode(pinC, INPUT_PULLUP);
231+
}else{
232+
pinMode(pinA, INPUT);
233+
pinMode(pinB, INPUT);
234+
pinMode(pinC, INPUT);
235+
}
236+
237+
// init hall_state
238+
A_active = digitalRead(pinA);
239+
B_active = digitalRead(pinB);
240+
C_active = digitalRead(pinC);
241+
updateState();
242+
243+
pulse_timestamp = _micros();
244+
245+
// we don't call Sensor::init() here because init is handled in HallSensorAny class.
246+
}
247+
248+
// function enabling hardware interrupts for the callback provided
249+
// if callback is not provided then the interrupt is not enabled
250+
void HallSensorAny::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){
251+
// attach interrupt if functions provided
252+
253+
// A, B and C callback
254+
if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
255+
if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
256+
if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE);
257+
258+
use_interrupt = true;
259+
}

‎src/encoders/hall_any/HallSensorAny.h‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
#pragma once
2+
3+
#include "Arduino.h"
4+
#include "common/base_classes/Sensor.h"
5+
6+
class HallSensorAny: public Sensor{
7+
public:
8+
9+
10+
enum class HallType : uint8_t
11+
{
12+
HALL_120 = 0,
13+
HALL_60C = 0b001,
14+
HALL_60B = 0b010,
15+
HALL_60A = 0b100,
16+
UNKNOWN = 0b111
17+
};
18+
19+
/**
20+
HallSensorAny class constructor
21+
@param encA HallSensor A pin
22+
@param encB HallSensor B pin
23+
@param encC HallSensor C pin
24+
@param pp pole pairs (e.g hoverboard motor has 15pp and small gimbals often have 7pp)
25+
@param hall_60deg Indicate if the hall sensors are 60 degrees apart electrically (means that they can all be one or off at the same time). In 60deg mode, B needs to lead, so you may need to swap the connections until you find one that works
26+
*/
27+
HallSensorAny(int encA, int encB, int encC, int pp, HallType hall_type = HallType::UNKNOWN);
28+
29+
/** HallSensor initialise pins */
30+
void init();
31+
/**
32+
* function enabling hardware interrupts for the HallSensor channels with provided callback functions
33+
* if callback is not provided then the interrupt is not enabled
34+
*
35+
* @param doA pointer to the A channel interrupt handler function
36+
* @param doB pointer to the B channel interrupt handler function
37+
* @param doIndex pointer to the Index channel interrupt handler function
38+
*
39+
*/
40+
void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr);
41+
42+
// HallSensor interrupt callback functions
43+
/** A channel callback function */
44+
void handleA();
45+
/** B channel callback function */
46+
void handleB();
47+
/** C channel callback function */
48+
void handleC();
49+
50+
51+
// pins A and B
52+
int pinA; //!< HallSensor hardware pin A
53+
int pinB; //!< HallSensor hardware pin B
54+
int pinC; //!< HallSensor hardware pin C
55+
bool use_interrupt; //!< True if interrupts have been attached
56+
HallType hall_type, last_print_type; //!< Connectivity of hall sensor. The type indicates the pin to be swapped. Hall120 has no swapped pin
57+
58+
// HallSensor configuration
59+
Pullup pullup; //!< Configuration parameter internal or external pullups
60+
int cpr;//!< HallSensor cpr number
61+
62+
// Abstract functions of the Sensor class implementation
63+
/** Interrupt-safe update */
64+
void update() override;
65+
/** get current angle (rad) */
66+
float getSensorAngle() override;
67+
/** get current angular velocity (rad/s) */
68+
float getVelocity() override;
69+
70+
/**
71+
* returns 0 if it does need search for absolute zero
72+
* 0 - magnetic sensor (& encoder with index which is found)
73+
* 1 - ecoder with index (with index not found yet)
74+
*/
75+
int needsSearch() override;
76+
77+
78+
// whether last step was CW (+1) or CCW (-1).
79+
Direction direction;
80+
Direction old_direction;
81+
82+
void attachSectorCallback(void (*onSectorChange)(int a) = nullptr);
83+
84+
//last unique previous states, 0 = recent, used to detect the hall type
85+
volatile uint8_t previous_states[3];
86+
// the current 3bit state of the hall sensors, without any line flipped
87+
volatile int8_t hall_state_raw;
88+
// the current sector of the sensor. Each sector is 60deg electrical
89+
volatile int8_t electric_sector;
90+
// the number of electric rotations
91+
volatile long electric_rotations;
92+
// this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts)
93+
volatile long total_interrupts;
94+
95+
// variable used to filter outliers - rad/s
96+
float velocity_max = 1000.0f;
97+
98+
private:
99+
100+
Direction decodeDirection(int oldState, int newState);
101+
void updateState();
102+
103+
volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us
104+
volatile int A_active; //!< current active states of A channel
105+
volatile int B_active; //!< current active states of B channel
106+
volatile int C_active; //!< current active states of C channel
107+
108+
// function pointer for on sector change call back
109+
void (*onSectorChange)(int sector) = nullptr;
110+
111+
volatile long pulse_diff;
112+
};

0 commit comments

Comments
 (0)