Skip to content

Commit b42d4f7

Browse files
beniroquaibeniroquai
committed
Merge remote-tracking branch 'origin/mergemaster'
2 parents 91268d3 + 9b0c2b6 commit b42d4f7

7 files changed

Lines changed: 240 additions & 32 deletions

File tree

‎.DS_Store‎

0 Bytes
Binary file not shown.

‎ESP32/AS5311-PWM-LIB-QUEUE/AS5311-PWM-LIB-QUEUE.ino‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#include "AS5311.h"
2+
3+
AS5311 sensor(GPIO_NUM_26, GPIO_NUM_27);
4+
5+
void setup() {
6+
Serial.begin(115200);
7+
sensor.begin();
8+
}
9+
10+
void loop() {
11+
12+
float position = sensor.readPosition();
13+
int edgeCount = sensor.readEdgeCounter();
14+
if (position != -1.0f) {
15+
Serial.println((edgeCount+position));
16+
Serial.println(edgeCount);
17+
}
18+
delay(10);
19+
}

‎ESP32/AS5311-PWM-LIB-QUEUE/AS5311.cpp‎

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
#include "AS5311.h"
2+
#include "freertos/queue.h"
3+
4+
volatile uint32_t AS5311::_pos_edg_0 = 0;
5+
volatile uint32_t AS5311::_pos_edg_1 = 0;
6+
volatile uint32_t AS5311::_neg_edg_0 = 0;
7+
volatile AS5311::PWM_Params AS5311::_pwm = {0, 0, 0};
8+
volatile int AS5311::_edgeCounter = 0;
9+
volatile float AS5311::_position = 0.f;
10+
QueueHandle_t AS5311::dataQueue = nullptr; // Define the Queue handle globally.
11+
12+
int AS5311::_pwmPin = 0;
13+
int AS5311::_interruptPin = 0;
14+
15+
AS5311::AS5311(int pwmPin, int interruptPin) {
16+
AS5311::_pwmPin = pwmPin;
17+
_interruptPin = interruptPin;
18+
}
19+
20+
void AS5311::begin() {
21+
pinMode(_pwmPin, INPUT_PULLDOWN);
22+
pinMode(_interruptPin, INPUT_PULLDOWN);
23+
24+
attachInterrupt(digitalPinToInterrupt(_pwmPin), _Ext_PWM_ISR_handler, CHANGE);
25+
attachInterrupt(digitalPinToInterrupt(_interruptPin), _handleRisingEdge, RISING);
26+
27+
// Create a queue to handle PWM and EdgeCounter data in a safe context
28+
dataQueue = xQueueCreate(10, sizeof(PWM_Params));
29+
30+
// Task creation should be added here
31+
xTaskCreate(
32+
handleDataTask, /* Task function. */
33+
"HandleData", /* String with name of task. */
34+
10000, /* Stack size in bytes. */
35+
NULL, /* Parameter passed as input to the task */
36+
1, /* Priority at which the task is created. */
37+
NULL); /* Task handle. */
38+
}
39+
40+
float AS5311::readPosition() {
41+
return _position;
42+
}
43+
44+
int AS5311::readEdgeCounter() {
45+
return _edgeCounter;
46+
}
47+
48+
void IRAM_ATTR AS5311::_handleRisingEdge() {
49+
PWM_Params localData;
50+
localData.period = 0;
51+
localData.duty_cycle = 0;
52+
localData.edgeCounter = 1;
53+
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
54+
xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken);
55+
if (xHigherPriorityTaskWoken) {
56+
portYIELD_FROM_ISR();
57+
}
58+
}
59+
60+
void IRAM_ATTR AS5311::_Ext_PWM_ISR_handler() {
61+
uint32_t current_time = micros();
62+
PWM_Params localData;
63+
localData.period = 0;
64+
localData.duty_cycle = 0;
65+
localData.edgeCounter = 0;
66+
67+
if (digitalRead(_pwmPin) == HIGH) {
68+
_pos_edg_1 = current_time;
69+
localData.period = _pos_edg_1 - _pos_edg_0;
70+
_pos_edg_0 = _pos_edg_1;
71+
} else {
72+
_neg_edg_0 = current_time;
73+
localData.duty_cycle = _neg_edg_0 - _pos_edg_0;
74+
}
75+
76+
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
77+
xQueueSendFromISR(dataQueue, &localData, &xHigherPriorityTaskWoken);
78+
if (xHigherPriorityTaskWoken) {
79+
portYIELD_FROM_ISR();
80+
}
81+
}
82+
83+
void AS5311::handleDataTask(void *parameter) {
84+
PWM_Params receivedData;
85+
PWM_Params localData;
86+
localData = {0, 0, 0};
87+
float position = 0.f;
88+
for (;;) {
89+
if (xQueueReceive(dataQueue, &receivedData, portMAX_DELAY)) {
90+
if (receivedData.edgeCounter != 0) {
91+
if (_position>.5){
92+
_edgeCounter = _edgeCounter +1;
93+
}
94+
else{
95+
_edgeCounter = _edgeCounter -1;
96+
}
97+
//Serial.print("Edge Counter: ");
98+
//Serial.println(_edgeCounter);
99+
}
100+
101+
if(receivedData.period != 0) {
102+
//Serial.print("Period: ");
103+
//Serial.println(receivedData.period);
104+
localData.period = receivedData.period;
105+
}
106+
if (receivedData.duty_cycle != 0) {
107+
//Serial.print("Duty Cycle: ");
108+
//Serial.println(receivedData.duty_cycle);
109+
localData.duty_cycle = receivedData.duty_cycle;
110+
}
111+
112+
// Calculate position
113+
if (localData.period !=0 and localData.duty_cycle <= localData.period){
114+
// At the 0/1 change the duty cycle gets noisy, so we need to average it over multiple senses
115+
_position = calculateRollingAverage((float) localData.duty_cycle / (float) localData.period);
116+
}
117+
118+
}
119+
}
120+
}
121+
122+
float AS5311::calculateRollingAverage(float newVal) {
123+
static float values[3] = {0.0, 0.0, 0.0};
124+
static int insertIndex = 0;
125+
static float sum = 0.0;
126+
127+
sum -= values[insertIndex];
128+
values[insertIndex] = newVal;
129+
sum += newVal;
130+
131+
insertIndex = (insertIndex + 1) % 3;
132+
133+
return sum / 3.0;
134+
}

‎ESP32/AS5311-PWM-LIB-QUEUE/AS5311.h‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#ifndef AS5311_H
2+
#define AS5311_H
3+
4+
#include <Arduino.h>
5+
6+
class AS5311 {
7+
public:
8+
AS5311(int pwmPin, int interruptPin);
9+
10+
void begin();
11+
float readPosition();
12+
int readEdgeCounter();
13+
private:
14+
static int _pwmPin, _interruptPin;
15+
static bool writing_counter;
16+
static volatile uint32_t _pos_edg_0, _pos_edg_1, _neg_edg_0;
17+
static volatile int _edgeCounter;
18+
static volatile float _position;
19+
static volatile bool _time_2_print;
20+
static QueueHandle_t dataQueue; // Define the Queue handle globally.
21+
static void handleDataTask(void *parameter);
22+
static float calculateRollingAverage(float newVal);
23+
typedef struct {
24+
uint32_t period;
25+
uint32_t duty_cycle;
26+
int edgeCounter;
27+
} PWM_Params;
28+
29+
static volatile PWM_Params _pwm;
30+
31+
static void IRAM_ATTR _handleRisingEdge();
32+
static void IRAM_ATTR _Ext_PWM_ISR_handler();
33+
static void IRAM_ATTR _print_adcpwm();
34+
};
35+
36+
#endif

‎ESP32/AS5311-PWM-LIB/AS5311-PWM-LIB.ino‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#include "AS5311.h"
22

3-
AS5311 sensor(GPIO_NUM_26, 27);
3+
AS5311 sensor(GPIO_NUM_26, GPIO_NUM_27);
44

55
void setup() {
66
Serial.begin(115200);
@@ -9,8 +9,10 @@ void setup() {
99

1010
void loop() {
1111
float position = sensor.readPosition();
12+
int edgeCount = sensor.readEdgeCounter();
1213
if (position != -1.0f) {
1314
Serial.println(position);
15+
Serial.println(edgeCount);
1416
}
1517
delay(10);
1618
}

‎ESP32/AS5311-PWM-LIB/AS5311.cpp‎

Lines changed: 44 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -4,61 +4,75 @@
44
volatile uint32_t AS5311::_pos_edg_0 = 0;
55
volatile uint32_t AS5311::_pos_edg_1 = 0;
66
volatile uint32_t AS5311::_neg_edg_0 = 0;
7-
volatile uint32_t AS5311::_edgeCounter = 0;
7+
volatile int AS5311::_edgeCounter = 0;
88
volatile bool AS5311::_time_2_print = false;
99
volatile AS5311::PWM_Params AS5311::_pwm = {0, 0};
10+
bool AS5311::writing_counter = false;
1011

1112
int AS5311::_pwmPin = 0;
1213
int AS5311::_interruptPin = 0;
1314

1415
AS5311::AS5311(int pwmPin, int interruptPin) {
15-
_pwmPin = pwmPin; // Set static members in the constructor
16-
_interruptPin = interruptPin;
16+
_pwmPin = pwmPin; // Set static members in the constructor
17+
_interruptPin = interruptPin;
1718
}
1819

20+
static QueueHandle_t gpio_evt_queue = NULL;
21+
22+
1923
void AS5311::begin() {
20-
pinMode(_pwmPin, INPUT_PULLDOWN);
21-
attachInterrupt(digitalPinToInterrupt(_pwmPin), _Ext_PWM_ISR_handler, CHANGE);
24+
pinMode(_pwmPin, INPUT_PULLDOWN);
25+
attachInterrupt(digitalPinToInterrupt(_pwmPin), _Ext_PWM_ISR_handler, CHANGE);
2226

23-
pinMode(_interruptPin, INPUT_PULLDOWN);
24-
attachInterrupt(digitalPinToInterrupt(_interruptPin), _handleRisingEdge, CHANGE);
27+
pinMode(_interruptPin, INPUT_PULLDOWN);
28+
attachInterrupt(digitalPinToInterrupt(_interruptPin), _handleRisingEdge, RISING);
2529

26-
hw_timer_t * timer = timerBegin(0, 80, true);
27-
timerAttachInterrupt(timer, &_print_adcpwm, false);
28-
timerAlarmWrite(timer, 100000, true);
29-
timerAlarmEnable(timer);
30+
hw_timer_t * timer = timerBegin(0, 80, true);
31+
timerAttachInterrupt(timer, &_print_adcpwm, false);
32+
timerAlarmWrite(timer, 100000, true);
33+
timerAlarmEnable(timer);
3034
}
3135

3236
float AS5311::readPosition() {
33-
if (_time_2_print) {
34-
float currentPosition = (float)_pwm.duty_cycle/(float)_pwm.period*2000 + 2000*_edgeCounter;
35-
_time_2_print = false;
36-
return currentPosition;
37-
}
38-
return -1.0f; // indicates no new data
37+
if (_time_2_print and not writing_counter) {
38+
float currentPosition = (float)_pwm.duty_cycle / (float)_pwm.period * 2000.0f + 2000.0f * _edgeCounter;
39+
_time_2_print = false;
40+
return currentPosition;
41+
}
42+
return -1.0f; // indicates no new data
43+
}
44+
45+
46+
int AS5311::readEdgeCounter() {
47+
return _edgeCounter;
3948
}
4049

4150
void IRAM_ATTR AS5311::_handleRisingEdge() {
42-
float currentPosition = (float)_pwm.duty_cycle/(float)_pwm.period*2000 + 2000*_edgeCounter;
43-
if(currentPosition > 1000) {
44-
_edgeCounter++;
45-
} else {
46-
_edgeCounter--;
47-
}
51+
// maybe value is read/written at the same time leading to a reboot?
52+
53+
float currentPosition = (float)_pwm.duty_cycle / (float)_pwm.period;
54+
if (currentPosition < 0.5f) {
55+
_edgeCounter++;
56+
} else {
57+
_edgeCounter--;
58+
}
4859
}
4960

5061
void IRAM_ATTR AS5311::_Ext_PWM_ISR_handler() {
51-
uint32_t current_time = micros();
62+
uint32_t current_time = micros();
63+
if (not writing_counter) {
5264
if (digitalRead(_pwmPin) == HIGH) {
53-
_pos_edg_1 = current_time;
54-
_pwm.period = _pos_edg_1 - _pos_edg_0;
55-
_pos_edg_0 = _pos_edg_1;
65+
_pos_edg_1 = current_time;
66+
_pwm.period = _pos_edg_1 - _pos_edg_0;
67+
_pos_edg_0 = _pos_edg_1;
5668
} else {
57-
_neg_edg_0 = current_time;
58-
_pwm.duty_cycle = _neg_edg_0 - _pos_edg_0;
69+
_neg_edg_0 = current_time;
70+
_pwm.duty_cycle = _neg_edg_0 - _pos_edg_0;
5971
}
72+
xQueueSendFromISR(gpio_evt_queue, &_pwm, NULL);
73+
}
6074
}
6175

6276
void IRAM_ATTR AS5311::_print_adcpwm() {
63-
_time_2_print = true;
77+
_time_2_print = true;
6478
}

‎ESP32/AS5311-PWM-LIB/AS5311.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,12 @@ class AS5311 {
88
AS5311(int pwmPin, int interruptPin);
99
void begin();
1010
float readPosition();
11+
int readEdgeCounter();
1112
private:
13+
static bool writing_counter;
1214
static int _pwmPin, _interruptPin;
13-
static volatile uint32_t _pos_edg_0, _pos_edg_1, _neg_edg_0, _edgeCounter;
15+
static volatile uint32_t _pos_edg_0, _pos_edg_1, _neg_edg_0;
16+
static volatile int _edgeCounter;
1417
static volatile bool _time_2_print;
1518
typedef struct {
1619
uint32_t period;

0 commit comments

Comments
 (0)