/
manager.rs
247 lines (203 loc) · 8.18 KB
/
manager.rs
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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
use std::marker::Send;
use std::sync::{Arc, Mutex, RwLock};
use mavlink::common::MavMessage;
use mavlink::{MavConnection, MavHeader};
use tokio::sync::broadcast;
use tracing::*;
use crate::settings;
lazy_static! {
static ref MANAGER: Arc<Mutex<Manager>> = Default::default();
}
pub struct Manager {
connection: Arc<RwLock<Connection>>,
ids: Arc<RwLock<Vec<u8>>>,
}
struct Connection {
address: String,
connection: Option<Box<dyn MavConnection<MavMessage> + Sync + Send>>,
sender: broadcast::Sender<Message>,
}
#[derive(Debug, Clone)]
pub enum Message {
Received((MavHeader, MavMessage)),
ToBeSent((MavHeader, MavMessage)),
}
impl Default for Manager {
#[instrument(level = "debug")]
fn default() -> Self {
let address =
settings::manager::mavlink_endpoint().expect("No configured mavlink endpoint");
let (sender, _receiver) = broadcast::channel(100);
let this = Self {
connection: Arc::new(RwLock::new(Connection {
address,
connection: None,
sender,
})),
ids: Arc::new(RwLock::new(vec![])),
};
let connection = this.connection.clone();
std::thread::Builder::new()
.name("MavSender".into())
.spawn(move || Manager::sender_loop(connection))
.expect("Failed to spawn MavSender thread");
let connection = this.connection.clone();
std::thread::Builder::new()
.name("MavReceiver".into())
.spawn(move || Manager::receiver_loop(connection))
.expect("Failed to spawn MavReceiver thread");
this
}
}
impl Manager {
// Construct our manager, should be done inside main
#[instrument(level = "debug")]
pub fn init() {
MANAGER.as_ref();
}
#[instrument(level = "debug", skip(inner))]
fn receiver_loop(inner: Arc<RwLock<Connection>>) {
loop {
loop {
let Ok(inner_guard) = inner.read() else {
break; // Break to trigger reconnection
};
let Some(mavlink) = inner_guard.connection.as_deref() else {
break; // Break to trigger reconnection
};
// Receive from the Mavlink network
let (header, message) = match mavlink.recv() {
Ok(message) => message,
Err(error) => {
trace!("Failed receiving from mavlink: {error:?}");
match &error {
mavlink::error::MessageReadError::Parse(_) => continue,
mavlink::error::MessageReadError::Io(io_error) => {
// The mavlink connection is handled by the sender_loop, so we can just silently skip the WouldBlocks
if io_error.kind() == std::io::ErrorKind::WouldBlock {
continue;
}
}
}
error!("Failed receiving message from Mavlink Connection: {error:?}");
break; // Break to trigger reconnection
}
};
trace!("Message received: {header:?}, {message:?}");
// Early filter non-GCS messages to avoid passing unwanted ones to the camera componenets.
let allowed_component_ids = [
mavlink::common::MavComponent::MAV_COMP_ID_ALL as u8,
mavlink::common::MavComponent::MAV_COMP_ID_SYSTEM_CONTROL as u8,
mavlink::common::MavComponent::MAV_COMP_ID_MISSIONPLANNER as u8,
];
if !allowed_component_ids.contains(&header.component_id) {
trace!("Message dropped: {header:?}, {message:?}");
continue;
}
debug!("Message accepted: {header:?}, {message:?}");
// Send the received message to the cameras
if let Err(error) = inner_guard
.sender
.send(Message::Received((header, message)))
{
error!("Failed handling message: {error:?}");
continue;
}
}
// Reconnects
{
let mut inner = inner.write().unwrap();
let address = inner.address.clone();
inner.connection.replace(Connection::connect(&address));
}
std::thread::sleep(std::time::Duration::from_millis(500));
}
}
#[instrument(level = "debug", skip(inner))]
fn sender_loop(inner: Arc<RwLock<Connection>>) {
let mut receiver = { inner.read().unwrap().sender.subscribe() };
loop {
loop {
// Receive answer from the cameras
let (header, message) = match receiver.blocking_recv() {
Ok(Message::ToBeSent(message)) => message,
Ok(Message::Received(_)) => continue,
Err(broadcast::error::RecvError::Closed) => {
unreachable!(
"Closed channel: This should never happen, this channel is static!"
);
}
Err(broadcast::error::RecvError::Lagged(samples)) => {
warn!("Channel is lagged behind by {samples} messages. Expect degraded performance on the mavlink responsiviness.");
continue;
}
};
let Ok(inner_guard) = inner.read() else {
break; // Break to trigger reconnection
};
let Some(mavlink) = inner_guard.connection.as_deref() else {
break; // Break to trigger reconnection
};
// Send the response from the cameras to the Mavlink network
if let Err(error) = mavlink.send(&header, &message) {
error!("Failed sending message to Mavlink Connection: {error:?}");
break; // Break to trigger reconnection
}
debug!("Message sent: {header:?}, {message:?}");
}
// Reconnects
{
let mut inner = inner.write().unwrap();
let address = inner.address.clone();
inner.connection.replace(Connection::connect(&address));
}
std::thread::sleep(std::time::Duration::from_millis(500));
}
}
#[instrument(level = "debug")]
pub fn new_component_id() -> u8 {
let manager = MANAGER.lock().unwrap();
let mut id = mavlink::common::MavComponent::MAV_COMP_ID_CAMERA as u8;
let mut vector = manager.ids.write().unwrap();
// Find the closest ID available
while vector.contains(&id) {
id += 1;
}
vector.push(id);
id
}
#[instrument(level = "debug")]
pub fn drop_id(id: u8) {
let manager = MANAGER.lock().unwrap();
let mut vector = manager.ids.write().unwrap();
if let Some(position) = vector.iter().position(|&vec_id| vec_id == id) {
vector.remove(position);
} else {
error!("Id not found");
}
}
#[instrument(level = "debug")]
pub fn get_sender() -> broadcast::Sender<Message> {
let manager = MANAGER.lock().unwrap();
let connection = manager.connection.read().unwrap();
connection.sender.clone()
}
}
impl Connection {
#[instrument(level = "debug")]
fn connect(address: &str) -> Box<dyn MavConnection<MavMessage> + Sync + Send> {
loop {
std::thread::sleep(std::time::Duration::from_secs(1));
debug!("Connecting...");
match mavlink::connect(address) {
Ok(connection) => {
info!("Successfully connected");
return connection;
}
Err(error) => {
error!("Failed to connect, trying again in one second. Reason: {error:?}.");
}
}
}
}
}