Skip to content

Commit b80f25c

Browse files
H1ronoH1rono
authored
Merge pull request #17 from rogy-AquaLab/fix-20240824
pcのmutexを追加
2 parents 6a8fd28 + 28a992c commit b80f25c

File tree

3 files changed

+78
-47
lines changed

3 files changed

+78
-47
lines changed

Diff for: ‎include/umiusi/outputs.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,9 @@
55
#include <mutex>
66

77
#include <DigitalOut.h>
8+
#include <events/EventQueue.h>
89
#include <Mutex.h>
910
#include <PwmOut.h>
10-
#include <events/EventQueue.h>
1111

1212
#include "umiusi/state.hpp"
1313

Diff for: ‎include/umiusi/trylock_guard.hpp

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#ifndef UMIUSI_TRYLOCK_GUARD_HPP
2+
#define UMIUSI_TRYLOCK_GUARD_HPP
3+
4+
template<typename Mutex> class TrylockGuard {
5+
private:
6+
Mutex& _mutex;
7+
bool _locked;
8+
9+
public:
10+
TrylockGuard(Mutex& mutex) : _mutex(mutex) {
11+
this->_locked = this->_mutex.trylock();
12+
}
13+
14+
~TrylockGuard() {
15+
if (this->_locked) {
16+
this->_mutex.unlock();
17+
}
18+
}
19+
20+
auto locked() const -> bool {
21+
return this->_locked;
22+
}
23+
};
24+
25+
#endif // UMIUSI_TRYLOCK_GUARD_HPP

Diff for: ‎src/main.cpp

+52-46
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,12 @@
44
#include <mutex>
55

66
#include "BufferedSerial.h"
7-
#include "events/EventQueue.h"
87
#include "events/Event.h"
8+
#include "events/EventQueue.h"
99

1010
#include "umiusi/inputs.hpp"
1111
#include "umiusi/outputs.hpp"
12+
#include "umiusi/trylock_guard.hpp"
1213

1314
using namespace std::chrono_literals;
1415

@@ -20,6 +21,7 @@ int main() {
2021
CachedInputs inputs{};
2122
OutputMachine output{};
2223
mbed::BufferedSerial pc(USBTX, USBRX);
24+
rtos::Mutex pc_mutex{};
2325
std::atomic_bool received_order;
2426
pc.set_blocking(false);
2527
inputs.read();
@@ -34,58 +36,62 @@ int main() {
3436
output.suspend();
3537
}));
3638

37-
const auto process_order = [&pc, &inputs, &output, &initialize_event, &suspend_event](std::uint8_t header) {
39+
auto write_order_event = equeue.event(mbed::callback([&pc, &pc_mutex, &output]() {
40+
std::lock_guard<rtos::Mutex> _guard(pc_mutex);
41+
std::array<uint8_t, 16> buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2
42+
pc.read(buffer.data(), 16);
43+
std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM> pulsewidths_us{};
44+
for (size_t i = 0; i < THRUSTER_NUM; ++i) {
45+
// bldc
46+
uint16_t pulsewidth_us_lsb = static_cast<uint16_t>(buffer[i * 2 + 0]);
47+
uint16_t pulsewidth_us_msb = static_cast<uint16_t>(buffer[i * 2 + 1]);
48+
pulsewidths_us[i].first = (pulsewidth_us_lsb << 0) | (pulsewidth_us_msb << 8);
49+
// servo
50+
pulsewidth_us_lsb = static_cast<uint16_t>(buffer[i * 2 + 0 + THRUSTER_NUM * 2]
51+
);
52+
pulsewidth_us_msb = static_cast<uint16_t>(buffer[i * 2 + 1 + THRUSTER_NUM * 2]
53+
);
54+
pulsewidths_us[i].second = (pulsewidth_us_lsb << 0)
55+
| (pulsewidth_us_msb << 8);
56+
}
57+
output.set_powers(pulsewidths_us);
58+
}));
59+
auto read_order_event = equeue.event(mbed::callback([&pc, &pc_mutex, &inputs]() {
60+
std::lock_guard<rtos::Mutex> _guard(pc_mutex);
61+
std::array<uint8_t, 8> buffer = inputs.get().packet_data();
62+
pc.write(buffer.data(), 8);
63+
}));
64+
auto read_state_order_event
65+
= equeue.event(mbed::callback([&pc, &pc_mutex, &output]() {
66+
std::lock_guard<rtos::Mutex> _guard(pc_mutex);
67+
uint8_t state_val = static_cast<uint8_t>(output.state());
68+
pc.write(&state_val, 1);
69+
}));
70+
71+
const auto process_order = [&write_order_event,
72+
&read_order_event,
73+
&read_state_order_event,
74+
&initialize_event,
75+
&suspend_event](std::uint8_t header) {
3876
switch (header) {
39-
case 0: {
40-
// write
41-
std::array<uint8_t, 16> buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2
42-
pc.read(buffer.data(), 16);
43-
std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM> pulsewidths_us{};
44-
for (size_t i = 0; i < THRUSTER_NUM; ++i) {
45-
// bldc
46-
uint16_t pulsewidth_us_lsb = static_cast<uint16_t>(buffer[i * 2 + 0]);
47-
uint16_t pulsewidth_us_msb = static_cast<uint16_t>(buffer[i * 2 + 1]);
48-
pulsewidths_us[i].first = (pulsewidth_us_lsb << 0)
49-
| (pulsewidth_us_msb << 8);
50-
// servo
51-
pulsewidth_us_lsb
52-
= static_cast<uint16_t>(buffer[i * 2 + 0 + THRUSTER_NUM * 2]);
53-
pulsewidth_us_msb
54-
= static_cast<uint16_t>(buffer[i * 2 + 1 + THRUSTER_NUM * 2]);
55-
pulsewidths_us[i].second = (pulsewidth_us_lsb << 0)
56-
| (pulsewidth_us_msb << 8);
57-
}
58-
output.set_powers(pulsewidths_us);
59-
} break;
60-
case 1: {
61-
// read
62-
std::array<uint8_t, 8> buffer = inputs.get().packet_data();
63-
pc.write(buffer.data(), 8);
64-
} break;
65-
case 2: {
66-
// read state
67-
uint8_t state_val = static_cast<uint8_t>(output.state());
68-
pc.write(&state_val, 1);
69-
} break;
70-
case 0xFE: {
71-
// (re)start
72-
if (output.state() == State::INITIALIZING) {
73-
return;
74-
}
75-
initialize_event.call();
76-
} break;
77-
case 0xFF:
78-
// suspend
79-
suspend_event.call();
80-
break;
77+
case 0: write_order_event.call(); break;
78+
case 1: read_order_event.call(); break;
79+
case 2: read_state_order_event.call(); break;
80+
case 0xFE: initialize_event.call(); break;
81+
case 0xFF: suspend_event.call(); break;
8182
default:
8283
// unexpected
8384
return;
8485
}
8586
};
86-
equeue.call_every(30ms, [&equeue, &pc, &received_order, &process_order]() {
87+
// FIXME: なぜか30msより短いと挙動がおかしくなる
88+
equeue.call_every(30ms, [&equeue, &pc, &pc_mutex, &received_order, &process_order]() {
89+
TrylockGuard pc_guard(pc_mutex);
90+
if (!pc_guard.locked()) {
91+
return;
92+
}
8793
std::uint8_t header = 0;
88-
ssize_t res = pc.read(&header, 1);
94+
ssize_t res = pc.read(&header, 1);
8995
if (res < 1) {
9096
return;
9197
}

0 commit comments

Comments
 (0)