forked from Candas1/Arduino-FOC
-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathMagneticSensorPWM.cpp
More file actions
117 lines (90 loc) · 4.15 KB
/
Copy pathMagneticSensorPWM.cpp
File metadata and controls
117 lines (90 loc) · 4.15 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include "MagneticSensorPWM.h"
#include "Arduino.h"
/** MagneticSensorPWM(uint8_t _pinPWM, int _min, int _max)
* @param _pinPWM the pin that is reading the pwm from magnetic sensor
* @param _min_raw_count the smallest expected reading
* @param _max_raw_count the largest expected reading
*/
MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _max_raw_count){
pinPWM = _pinPWM;
cpr = _max_raw_count - _min_raw_count + 1;
min_raw_count = _min_raw_count;
max_raw_count = _max_raw_count;
// define if the sensor uses interrupts
is_interrupt_based = false;
// define as not set
last_call_us = _micros();
}
/** MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm_clocks, int _min_pwm_clocks, int _max_pwm_clocks)
*
* Constructor that computes the min and max raw counts based on the PWM frequency and the number of PWM clocks in one period
*
* @param _pinPWM the pin that is reading the pwm from magnetic sensor
* @param freqHz the frequency of the PWM signal, in Hz, e.g. 115, 230, 460 or 920 for the AS5600, depending on the PWM frequency setting
* @param _total_pwm_clocks the total number of PWM clocks in one period, e.g. 4351 for the AS5600
* @param _min_pwm_clocks the 0 value returned by the sensor, in PWM clocks, e.g. 128 for the AS5600
* @param _max_pwm_clocks the largest value returned by the sensor, in PWM clocks, e.g. 4223 for the AS5600
*/
MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm_clocks, int _min_pwm_clocks, int _max_pwm_clocks){
pinPWM = _pinPWM;
min_raw_count = lroundf(1000000.0f/freqHz/_total_pwm_clocks*_min_pwm_clocks);
max_raw_count = lroundf(1000000.0f/freqHz/_total_pwm_clocks*_max_pwm_clocks);
cpr = max_raw_count - min_raw_count + 1;
// define if the sensor uses interrupts
is_interrupt_based = false;
min_elapsed_time = 1.0f/freqHz; // set the minimum time between two readings
// define as not set
last_call_us = _micros();
}
void MagneticSensorPWM::init(){
// initial hardware
pinMode(pinPWM, INPUT);
raw_count = getRawCount();
pulse_timestamp = _micros();
this->Sensor::init(); // call base class init
}
// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
void MagneticSensorPWM::update() {
if (is_interrupt_based)
noInterrupts();
Sensor::update();
angle_prev_ts = pulse_timestamp; // Timestamp of actual sample, before the time-consuming PWM communication
if (is_interrupt_based)
interrupts();
}
// get current angle (rad)
float MagneticSensorPWM::getSensorAngle(){
// raw data from sensor
raw_count = getRawCount();
if (raw_count > max_raw_count) raw_count = max_raw_count;
if (raw_count < min_raw_count) raw_count = min_raw_count;
return( (float) (raw_count - min_raw_count) / (float)cpr) * _2PI;
}
// read the raw counter of the magnetic sensor
int MagneticSensorPWM::getRawCount(){
if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way
pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse
pulse_length_us = pulseIn(pinPWM, HIGH, 1200); // 1200us timeout, should this be configurable?
}
return pulse_length_us;
}
void MagneticSensorPWM::handlePWM() {
// unsigned long now_us = ticks();
unsigned long now_us = _micros();
// if falling edge, calculate the pulse length
if (!digitalRead(pinPWM)) {
pulse_length_us = now_us - last_call_us;
pulse_timestamp = last_call_us; // angle was sampled at the rising edge of the pulse, so use that timestamp
}
// save the currrent timestamp for the next call
last_call_us = now_us;
is_interrupt_based = true; // set the flag to true
}
// function enabling hardware interrupts of the for the callback provided
// if callback is not provided then the interrupt is not enabled
void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){
// declare it's interrupt based
is_interrupt_based = true;
// enable interrupts on pwm input pin
attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE);
}