Skip to content

Commit a3581a7

Browse files
runger1101001runger1101001
committed
added STM32 HW encoder by @conroy-cheers 
1 parent dea7b99 commit a3581a7

File tree

4 files changed

+289
-3
lines changed

4 files changed

+289
-3
lines changed

‎README.md‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,10 @@ What's here? See the sections below. Each driver or function should come with it
1414

1515
### Encoders
1616

17-
[AS5048A SPI driver](src/encoders/as5048a/) - SPI driver for the AMS AS5048A absolute position magnetic rotary encoder IC.
18-
[AS5047 SPI driver](src/encoders/as5047/) - SPI driver for the AMS AS5047P and AS5047D absolute position magnetic rotary encoder ICs.
19-
17+
- [AS5048A SPI driver](src/encoders/as5048a/) - SPI driver for the AMS AS5048A absolute position magnetic rotary encoder IC.
18+
- [AS5047 SPI driver](src/encoders/as5047/) - SPI driver for the AMS AS5047P and AS5047D absolute position magnetic rotary encoder ICs.
19+
- [MA730 SPI driver](src/encoders/ma730/) - SPI driver for the MPS MagAlpha MA730 absolute position magnetic rotary encoder IC.
20+
- [STM32 Hardware Encoder](src/encoders/stm32hwencoder/) - Hardware timer based encoder driver for ABI type quadrature encoders.
2021

2122

2223

‎src/encoders/stm32hwencoder/README.md‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
# SimpleFOC Driver for STM32 hardware encoder
2+
3+
This encoder driver uses the STM32 timer hardware to track the A/B impulses, which is much more efficient (and will support higher speeds) than the generic interrupt-based solution used by the standard encoder driver.
4+
5+
Big thank you to @conroy-cheers for originally contributing this code in pull request #114 to the simplefoc repository. Due its hardware-specific nature we moved the code to this drivers repository.
6+
7+
## Warning
8+
9+
This code has not been tested much! Use at your own risk, your milage may vary.
10+
I have tested on: STM32F401CC/TIM4-PB6,PB7
11+
12+
## Hardware setup
13+
14+
The ABI signals of your encoder will have to be connected to the STM32 MCU in such a way that all signals are connected to the same timer. Consult your STM32 chip's datasheet for more information on which pins connect to which timer.
15+
16+
Not all of the timers support the encoder mode, so check the datasheet which timers can be used.
17+
18+
An excellent option can be to use the STM32CubeIDE software's pin assignment view to quickly check which pins connect to which timer.
19+
20+
Note that the index (I) pin is currently not used.
21+
22+
23+
## Software setup
24+
25+
There is no need for interrupt functions or their configuration with this encoder class, so usage is quite simple:
26+
27+
```c++
28+
#include "Arduino.h"
29+
#include "Wire.h"
30+
#include "SPI.h"
31+
#include "SimpleFOC.h"
32+
#include "SimpleFOCDrivers.h"
33+
#include "encoders/stm32hwencoder/STM32HWEncoder.h"
34+
35+
36+
#define ENCODER_PPR 500
37+
#define ENCODER_PIN_A PB6
38+
#define ENCODER_PIN_B PB7
39+
#define ENCODER_PIN_I PC13
40+
41+
42+
STM32HWEncoder encoder = STM32HWEncoder(ENCODER_PPR, ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PIN_I);
43+
44+
45+
void setup() {
46+
encoder.init();
47+
}
48+
49+
```

‎src/encoders/stm32hwencoder/STM32HWEncoder.cpp‎

Lines changed: 166 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
#include "STM32HWEncoder.h"
2+
3+
#if defined(_STM32_DEF_)
4+
5+
/*
6+
HardwareEncoder(int cpr)
7+
*/
8+
STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int8_t pinA, int8_t pinB, int8_t pinI) {
9+
rotations_per_overflow = 0;
10+
ticks_per_overflow = 0;
11+
12+
overflow_count = 0;
13+
count = 0;
14+
prev_count = 0;
15+
prev_overflow_count = 0;
16+
pulse_timestamp = 0;
17+
18+
cpr = _ppr * 4; // 4x for quadrature
19+
20+
// velocity calculation variables
21+
prev_timestamp = getCurrentMicros();
22+
pulse_timestamp = getCurrentMicros();
23+
24+
_pinA = pinA;
25+
_pinB = pinB;
26+
_pinI = pinI;
27+
}
28+
29+
30+
31+
void STM32HWEncoder::update() {
32+
// handle overflow of the 16-bit counter here
33+
// must be called at least twice per traversal of overflow range
34+
// TODO(conroy-cheers): investigate performance impact
35+
prev_count = count;
36+
count = encoder_handle.Instance->CNT;
37+
38+
prev_timestamp = pulse_timestamp;
39+
pulse_timestamp = getCurrentMicros();
40+
41+
prev_overflow_count = overflow_count;
42+
if (prev_count > (ticks_per_overflow - overflow_margin) &&
43+
prev_count <= ticks_per_overflow && count < overflow_margin)
44+
++overflow_count;
45+
if (prev_count >= 0 && prev_count < overflow_margin &&
46+
count >= (ticks_per_overflow - overflow_margin))
47+
--overflow_count;
48+
}
49+
50+
51+
52+
/*
53+
Shaft angle calculation
54+
*/
55+
float STM32HWEncoder::getSensorAngle() { return getAngle(); }
56+
57+
float STM32HWEncoder::getMechanicalAngle() {
58+
return _2PI * (count % static_cast<int>(cpr)) / static_cast<float>(cpr);
59+
}
60+
float STM32HWEncoder::getAngle() {
61+
return _2PI * (count / static_cast<float>(cpr) +
62+
overflow_count * rotations_per_overflow);
63+
}
64+
double STM32HWEncoder::getPreciseAngle() {
65+
return _2PI * (count / static_cast<double>(cpr) +
66+
overflow_count * rotations_per_overflow);
67+
}
68+
int32_t STM32HWEncoder::getFullRotations() {
69+
return count / static_cast<int>(cpr) +
70+
overflow_count * rotations_per_overflow;
71+
}
72+
73+
/*
74+
Shaft velocity calculation
75+
*/
76+
float STM32HWEncoder::getVelocity() {
77+
// sampling time calculation
78+
float dt = (pulse_timestamp - prev_timestamp) * 1e-6f;
79+
// quick fix for strange cases (micros overflow)
80+
if (dt <= 0 || dt > 0.5f)
81+
dt = 1e-3f;
82+
83+
// time from last impulse
84+
int32_t overflow_diff = overflow_count - prev_overflow_count;
85+
int32_t dN = (count - prev_count) + (ticks_per_overflow * overflow_diff);
86+
87+
float pulse_per_second = dN / dt;
88+
89+
// velocity calculation
90+
return pulse_per_second / (static_cast<float>(cpr)) * _2PI;
91+
}
92+
93+
// getter for index pin
94+
int STM32HWEncoder::needsSearch() { return false; }
95+
96+
// private function used to determine if encoder has index
97+
int STM32HWEncoder::hasIndex() { return 0; }
98+
99+
// encoder initialisation of the hardware pins
100+
// and calculation variables
101+
void STM32HWEncoder::init() {
102+
// overflow handling
103+
rotations_per_overflow = 0xFFFF / cpr;
104+
ticks_per_overflow = cpr * rotations_per_overflow;
105+
106+
// set up GPIO
107+
GPIO_InitTypeDef gpio;
108+
109+
PinName pinA = digitalPinToPinName(_pinA);
110+
TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(pinA, PinMap_PWM);
111+
gpio.Pin = digitalPinToBitMask(_pinA);
112+
gpio.Mode = GPIO_MODE_AF_PP;
113+
gpio.Pull = GPIO_NOPULL;
114+
gpio.Speed = GPIO_SPEED_FREQ_MEDIUM;
115+
gpio.Alternate = pinmap_function(pinA, PinMap_PWM);
116+
HAL_GPIO_Init(digitalPinToPort(_pinA), &gpio);
117+
118+
// lets assume pinB is on the same timer as pinA... otherwise it can't work but the API currently doesn't allow us to fail gracefully
119+
gpio.Pin = digitalPinToBitMask(_pinB);
120+
gpio.Mode = GPIO_MODE_AF_PP;
121+
gpio.Pull = GPIO_NOPULL;
122+
gpio.Speed = GPIO_SPEED_FREQ_MEDIUM;
123+
gpio.Alternate = pinmap_function(digitalPinToPinName(_pinB), PinMap_PWM);
124+
HAL_GPIO_Init(digitalPinToPort(_pinB), &gpio);
125+
126+
// set up timer for encoder
127+
encoder_handle.Init.Period = ticks_per_overflow;
128+
encoder_handle.Init.Prescaler = 0;
129+
encoder_handle.Init.ClockDivision = 0;
130+
encoder_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
131+
encoder_handle.Init.RepetitionCounter = 0;
132+
encoder_handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
133+
134+
TIM_Encoder_InitTypeDef encoder_config;
135+
136+
encoder_config.EncoderMode = TIM_ENCODERMODE_TI12;
137+
138+
encoder_config.IC1Polarity = TIM_ICPOLARITY_RISING;
139+
encoder_config.IC1Selection = TIM_ICSELECTION_DIRECTTI;
140+
encoder_config.IC1Prescaler = TIM_ICPSC_DIV1;
141+
encoder_config.IC1Filter = 0;
142+
143+
encoder_config.IC2Polarity = TIM_ICPOLARITY_RISING;
144+
encoder_config.IC2Selection = TIM_ICSELECTION_DIRECTTI;
145+
encoder_config.IC2Prescaler = TIM_ICPSC_DIV1;
146+
encoder_config.IC2Filter = 0;
147+
148+
encoder_handle.Instance = InstanceA; // e.g. TIM4;
149+
enableTimerClock(&encoder_handle);
150+
if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
151+
_Error_Handler(__FILE__, __LINE__);
152+
}
153+
154+
HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1);
155+
156+
// counter setup
157+
overflow_count = 0;
158+
count = 0;
159+
prev_count = 0;
160+
prev_overflow_count = 0;
161+
162+
prev_timestamp = getCurrentMicros();
163+
pulse_timestamp = getCurrentMicros();
164+
}
165+
166+
#endif

‎src/encoders/stm32hwencoder/STM32HWEncoder.h‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
2+
#ifndef STM32_HARDWARE_ENCODER_H
3+
#define STM32_HARDWARE_ENCODER_H
4+
5+
#include <Arduino.h>
6+
7+
8+
#if defined(_STM32_DEF_)
9+
10+
#include <HardwareTimer.h>
11+
#include "common/base_classes/Sensor.h"
12+
#include "common/foc_utils.h"
13+
#include "common/time_utils.h"
14+
15+
class STM32HWEncoder : public Sensor {
16+
public:
17+
/**
18+
Encoder class constructor
19+
@param ppr impulses per rotation (cpr=ppr*4)
20+
*/
21+
explicit STM32HWEncoder(unsigned int ppr, int8_t pinA, int8_t pinB, int8_t pinI=-1);
22+
23+
/** encoder initialise pins */
24+
void init() override;
25+
26+
// Encoder configuration
27+
unsigned int cpr; //!< encoder cpr number
28+
29+
// Abstract functions of the Sensor class implementation
30+
/** get current angle (rad) */
31+
float getSensorAngle() override;
32+
float getMechanicalAngle() override;
33+
/** get current angular velocity (rad/s) */
34+
float getVelocity() override;
35+
float getAngle() override;
36+
double getPreciseAngle() override;
37+
int32_t getFullRotations() override;
38+
void update() override;
39+
40+
/**
41+
* returns 0 if it does need search for absolute zero
42+
* 0 - encoder without index
43+
* 1 - ecoder with index
44+
*/
45+
int needsSearch() override;
46+
47+
private:
48+
int hasIndex(); // !< function returning 1 if encoder has index pin and 0 if not.
49+
50+
void handleOverflow();
51+
52+
TIM_HandleTypeDef encoder_handle;
53+
54+
static constexpr u_int16_t overflow_margin = 20000;
55+
u_int16_t rotations_per_overflow;
56+
u_int16_t ticks_per_overflow;
57+
58+
volatile int32_t overflow_count;
59+
volatile u_int16_t count; //!< current pulse counter
60+
volatile u_int16_t prev_count;
61+
volatile int32_t prev_overflow_count;
62+
63+
// velocity calculation variables
64+
volatile int32_t pulse_timestamp, prev_timestamp;
65+
66+
int8_t _pinA, _pinB, _pinI;
67+
};
68+
69+
#endif
70+
#endif

0 commit comments

Comments
 (0)