/
ThroughBore.cpp
91 lines (74 loc) · 2.23 KB
/
ThroughBore.cpp
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
#include <BizarroHomer/Hardware/ThroughBore.hpp>
#include <BizarroHomer/Hardware/HardwareManager.hpp>
#include <BizarroHomer/Util/LinearFilter.hpp>
#include <fmt/core.h>
#include <chrono>
#include <string>
#include <sys/ioctl.h>
#include <unistd.h>
#include <fcntl.h>
using namespace std::literals::chrono_literals;
#define TIMEOUT_US 102500us
#define PULSE_MAX 1025.0
thunder::ThroughBore::ThroughBore(int channel)
: m_channel_input(channel) {
fmt::print("Initializing Through Bore at channel {}\n", channel);
// Start the read thread.
m_read_thread = std::thread([this]() {
this->read_thread();
});
}
thunder::ThroughBore::~ThroughBore() {
{
std::lock_guard<std::mutex> lk(m_read_mutex);
m_should_term = true;
}
m_read_thread.join();
}
double thunder::ThroughBore::get_angle() {
std::lock_guard<std::mutex> lk(m_read_mutex);
return m_angle;
}
void thunder::ThroughBore::read_thread() {
LinearFilter<double> filter = LinearFilter<double>::single_pole_iir(0.5, 0.00102);
while (true) {
{
std::lock_guard<std::mutex> lk(m_read_mutex);
if (m_should_term) {
break;
}
}
auto timeout = std::chrono::high_resolution_clock::now();
bool timed_out = false;
// Rising edge.
while (m_channel_input.get() == 0) {
if (std::chrono::high_resolution_clock::now() - timeout > TIMEOUT_US) {
timed_out = true;
break;
}
}
if (timed_out) {
continue;
}
auto start = std::chrono::high_resolution_clock::now();
// Falling edge.
while (m_channel_input.get() == 1) {
if (std::chrono::high_resolution_clock::now() - start > TIMEOUT_US) {
timed_out = true;
break;
}
}
if (timed_out) {
continue;
}
auto end = std::chrono::high_resolution_clock::now();
// Get time high (pulse width).
int duty_cycle = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
// Convert to 0-360 value.
double read_angle = std::clamp(360.0 * (duty_cycle / PULSE_MAX), 0.0, 360.0);
// Filter out bad values.
double good_angle = filter.calculate(read_angle);
std::lock_guard<std::mutex> lk(m_read_mutex);
m_angle = good_angle;
}
}