diff --git a/src/mavlink/manager.rs b/src/mavlink/manager.rs index 25a829be..998bb262 100644 --- a/src/mavlink/manager.rs +++ b/src/mavlink/manager.rs @@ -75,39 +75,48 @@ impl Manager { #[instrument(level = "debug", skip(inner))] fn receiver_loop(inner: Arc>) { loop { - std::thread::sleep(std::time::Duration::from_millis(10)); - - // Receive from the Mavlink network - let (header, message) = match inner.read().unwrap().connection.recv() { - Ok(message) => message, - Err(error) => { - trace!("Failed receiving from mavlink: {error:?}"); + loop { + std::thread::sleep(std::time::Duration::from_millis(10)); - // The mavlink connection is handled by the sender_loop, so we can just silently skip the WouldBlocks - if let mavlink::error::MessageReadError::Io(io_error) = &error { - if io_error.kind() == std::io::ErrorKind::WouldBlock { - continue; + // Receive from the Mavlink network + let (header, message) = match inner.read().unwrap().connection.recv() { + Ok(message) => message, + Err(error) => { + trace!("Failed receiving from mavlink: {error:?}"); + + // The mavlink connection is handled by the sender_loop, so we can just silently skip the WouldBlocks + if let mavlink::error::MessageReadError::Io(io_error) = &error { + 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:?}"); - // Okay, this is probably an untreated error, so we log it - error!("Failed receiving message from Mavlink Connection: {error:?}"); + // Send the received message to the cameras + if let Err(error) = inner + .read() + .unwrap() + .sender + .send(Message::Received((header, message))) + { + error!("Failed handling message: {error:?}"); continue; } - }; - - trace!("Message received: {header:?}, {message:?}"); + } - // Send the received message to the cameras - if let Err(error) = inner - .read() - .unwrap() - .sender - .send(Message::Received((header, message))) + // Reconnects { - error!("Failed handling message: {error:?}"); - continue; + let mut inner = inner.write().unwrap(); + inner.connection = Connection::connect(&inner.address); } + + std::thread::sleep(std::time::Duration::from_millis(500)); } }