Skip to content

Commit e1f4dad

Browse files
RaulTrombinRaulTrombin
committed
src: device: manager: continuous_mode: Add sector scan for ping360
1 parent 3e1bb98 commit e1f4dad

File tree

1 file changed

+273
-82
lines changed

1 file changed

+273
-82
lines changed

‎src/device/manager/continuous_mode.rs

+273-82
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
use serde_json::json;
2-
use tracing::{error, trace, warn};
2+
use tracing::{error, trace};
33
use uuid::Uuid;
44

55
use crate::device::{
6-
manager::{Answer, DeviceAnswer, ManagerError},
7-
manager::{DeviceManager, DeviceSelection},
6+
devices::DeviceActorHandler,
7+
manager::{Answer, DeviceAnswer, DeviceManager, DeviceSelection, ManagerError},
88
};
99

10+
use super::{DeviceProperties, Ping360Properties};
11+
1012
impl DeviceManager {
1113
// Call the helpers specifically for each device type
1214
pub async fn continuous_mode_start(
@@ -48,75 +50,29 @@ impl DeviceManager {
4850
}
4951
})),
5052
DeviceSelection::Ping360 => {
51-
Some(tokio::spawn(async move {
52-
// Attempt to send the Ping360 request and handle the result
53-
let device_data = match handler
54-
.send(crate::device::devices::PingRequest::Ping360(
55-
crate::device::devices::Ping360Request::DeviceData,
56-
))
57-
.await
58-
{
59-
Ok(response) => match response {
60-
crate::device::devices::PingAnswer::PingMessage(
61-
bluerobotics_ping::Messages::Ping360(
62-
bluerobotics_ping::ping360::Messages::DeviceData(msg),
63-
),
64-
) => msg,
65-
msg => {
66-
error!("Error during start_continuous_mode: unexpected message: {msg:?}");
67-
return;
68-
}
69-
},
70-
Err(err) => {
71-
error!("Error during start_continuous_mode: Device Error: {err:?}");
72-
return;
73-
}
74-
};
53+
let device_properties = self.get_device_properties(device_id).await.ok()?;
54+
let Some(DeviceProperties::Ping360(properties)) = device_properties else {
55+
error!("No properties available for Ping360 device, device: {device_id}");
56+
return None;
57+
};
7558

76-
loop {
77-
for n in 0..=399 {
78-
// Handle timeout and errors
79-
let result = tokio::time::timeout(
80-
std::time::Duration::from_millis(1000),
81-
handler.send(crate::device::devices::PingRequest::Ping360(
82-
crate::device::devices::Ping360Request::Transducer(
83-
bluerobotics_ping::ping360::TransducerStruct {
84-
mode: device_data.mode,
85-
gain_setting: device_data.gain_setting,
86-
transmit_duration: device_data.transmit_duration,
87-
sample_period: device_data.sample_period,
88-
transmit_frequency: device_data.transmit_frequency,
89-
number_of_samples: device_data.number_of_samples,
90-
angle: n,
91-
transmit: 1,
92-
reserved: 0,
93-
},
94-
),
95-
)),
96-
)
97-
.await;
98-
99-
match result {
100-
Ok(Ok(answer)) => match answer {
101-
crate::device::devices::PingAnswer::PingMessage(msg) => {
102-
Self::ping360_continuous_mode_helper(msg, device_id)
103-
}
104-
msg => {
105-
error!("Error during continuous_mode: Unexpected Message: {msg:?}");
106-
return;
107-
}
108-
},
109-
Ok(Err(err)) => {
110-
error!("Error during continuous_mode: Device Error: {err:?}");
111-
return;
112-
}
113-
Err(_err) => {
114-
warn!("Error during continuous_mode: Answer delayed more than 1 s");
115-
}
116-
}
117-
}
118-
}
119-
}))
59+
// Check if firmware supports auto-transmit mode
60+
if properties.common.device_information.firmware_version_major >= 3
61+
&& properties.common.device_information.firmware_version_minor >= 3
62+
{
63+
Some(Self::start_ping360_firmware_mode(
64+
handler,
65+
device_id,
66+
properties.clone(),
67+
subscriber,
68+
))
69+
} else {
70+
Some(Self::start_ping360_software_mode(
71+
handler,
72+
device_id,
73+
properties.clone(),
74+
))
75+
}
12076
}
12177
DeviceSelection::Common | DeviceSelection::Auto => None,
12278
}
@@ -154,17 +110,33 @@ impl DeviceManager {
154110
let handler_request = self.get_device_handler(device_id).await?;
155111
let handler = self.extract_handler(handler_request)?;
156112

157-
if device_type == DeviceSelection::Ping1D {
158-
let id = <bluerobotics_ping::ping1d::ProfileStruct as bluerobotics_ping::message::MessageInfo>::id();
159-
let _ = handler
160-
.send(crate::device::devices::PingRequest::Ping1D(
161-
crate::device::devices::Ping1DRequest::ContinuousStop(
162-
bluerobotics_ping::ping1d::ContinuousStopStruct { id },
163-
),
164-
))
165-
.await
166-
.map_err(|err| {trace!("Something went wrong while executing broadcast_startup_routine, details: {err:?}"); ManagerError::DeviceError(err)})?;
113+
match device_type {
114+
DeviceSelection::Ping1D => {
115+
let id = <bluerobotics_ping::ping1d::ProfileStruct as bluerobotics_ping::message::MessageInfo>::id();
116+
if let Err(err) = handler
117+
.send(crate::device::devices::PingRequest::Ping1D(
118+
crate::device::devices::Ping1DRequest::ContinuousStop(
119+
bluerobotics_ping::ping1d::ContinuousStopStruct { id },
120+
),
121+
))
122+
.await
123+
{
124+
error!("Something went wrong while executing continuous_mode_shutdown_routine, details: {err:?}, device: {device_id}");
125+
}
126+
}
127+
DeviceSelection::Ping360 => {
128+
if let Err(err) = handler
129+
.send(crate::device::devices::PingRequest::Ping360(
130+
crate::device::devices::Ping360Request::MotorOff,
131+
))
132+
.await
133+
{
134+
error!("Something went wrong while executing continuous_mode_shutdown_routine, details: {err:?}, device: {device_id}");
135+
}
136+
}
137+
_ => {}
167138
}
139+
168140
Ok(())
169141
}
170142

@@ -177,7 +149,12 @@ impl DeviceManager {
177149
if let Ok(bluerobotics_ping::Messages::Ping1D(bluerobotics_ping::ping1d::Messages::Profile(_answer))) = bluerobotics_ping::Messages::try_from(&msg) {
178150
let answer = Answer::DeviceMessage(DeviceAnswer {
179151
answer: crate::device::devices::PingAnswer::PingMessage(
180-
bluerobotics_ping::Messages::try_from(&msg).unwrap(),
152+
match bluerobotics_ping::Messages::try_from(&msg){
153+
Ok(msg) => msg,
154+
Err(err) => {
155+
error!("Unexpected message during scan: {err:?}");
156+
return},
157+
}
181158
),
182159
device_id,
183160
});
@@ -186,6 +163,29 @@ impl DeviceManager {
186163
}
187164
}
188165

166+
// An inner helper focused on Ping360 on AutoTransmit mode, which uses AutoDeviceData.
167+
pub fn ping360_continuous_mode_helper_auto(
168+
msg: bluerobotics_ping::message::ProtocolMessage,
169+
device_id: Uuid,
170+
) {
171+
if msg.message_id == <bluerobotics_ping::ping360::AutoDeviceDataStruct as bluerobotics_ping::message::MessageInfo>::id() {
172+
if let Ok(bluerobotics_ping::Messages::Ping360(bluerobotics_ping::ping360::Messages::AutoDeviceData(_answer))) = bluerobotics_ping::Messages::try_from(&msg) {
173+
let answer = Answer::DeviceMessage(DeviceAnswer {
174+
answer: crate::device::devices::PingAnswer::PingMessage(
175+
match bluerobotics_ping::Messages::try_from(&msg){
176+
Ok(msg) => msg,
177+
Err(err) => {
178+
error!("Unexpected message during scan: {err:?}");
179+
return},
180+
}
181+
),
182+
device_id,
183+
});
184+
crate::server::protocols::v1::websocket::send_to_websockets(json!(answer), Some(device_id));
185+
}
186+
}
187+
}
188+
189189
// An inner helper focused on Ping360, which uses DeviceData message to plot graphs
190190
pub fn ping360_continuous_mode_helper(msg: bluerobotics_ping::Messages, device_id: Uuid) {
191191
let answer = Answer::DeviceMessage(DeviceAnswer {
@@ -205,4 +205,195 @@ impl DeviceManager {
205205
));
206206
crate::server::protocols::v1::websocket::send_to_websockets(json!(error), Some(device_id));
207207
}
208+
209+
fn start_ping360_firmware_mode(
210+
handler: DeviceActorHandler,
211+
device_id: Uuid,
212+
properties: Ping360Properties,
213+
mut subscriber: tokio::sync::broadcast::Receiver<
214+
bluerobotics_ping::message::ProtocolMessage,
215+
>,
216+
) -> tokio::task::JoinHandle<()> {
217+
tokio::spawn(async move {
218+
loop {
219+
let config = properties.continuous_mode_settings.clone();
220+
let initial_settings = match config.read() {
221+
Ok(settings) => settings.clone(),
222+
Err(err) => {
223+
error!("Failed to read Ping360Config: {err:?}, device: {device_id}");
224+
break;
225+
}
226+
};
227+
228+
// Stop the motor before starting auto-transmit
229+
if let Err(err) = handler
230+
.send(crate::device::devices::PingRequest::Ping360(
231+
crate::device::devices::Ping360Request::MotorOff,
232+
))
233+
.await
234+
{
235+
error!("Failed to stop motor: {err:?}, device: {device_id}");
236+
break;
237+
}
238+
239+
// Start auto-transmit mode
240+
if let Err(err) = handler
241+
.send(crate::device::devices::PingRequest::Ping360(
242+
crate::device::devices::Ping360Request::AutoTransmit(
243+
bluerobotics_ping::ping360::AutoTransmitStruct {
244+
mode: initial_settings.mode,
245+
gain_setting: initial_settings.gain_setting,
246+
transmit_duration: initial_settings.transmit_duration,
247+
sample_period: initial_settings.sample_period,
248+
transmit_frequency: initial_settings.transmit_frequency,
249+
number_of_samples: initial_settings.number_of_samples,
250+
start_angle: initial_settings.start_angle,
251+
stop_angle: initial_settings.stop_angle,
252+
num_steps: initial_settings.num_steps,
253+
delay: initial_settings.delay,
254+
},
255+
),
256+
))
257+
.await
258+
{
259+
error!("Failed to start auto transmit: {err:?}, device: {device_id}");
260+
break;
261+
}
262+
263+
loop {
264+
let current_settings = match config.read() {
265+
Ok(settings) => settings.clone(),
266+
Err(err) => {
267+
error!("Failed to read Ping360Config: {err:?}, device: {device_id}");
268+
break;
269+
}
270+
};
271+
if initial_settings != current_settings {
272+
break;
273+
}
274+
275+
match subscriber.recv().await {
276+
Ok(msg) => Self::ping360_continuous_mode_helper_auto(msg, device_id),
277+
Err(err) => {
278+
Self::handle_error_continuous_mode(err, device_id);
279+
return;
280+
}
281+
}
282+
}
283+
}
284+
})
285+
}
286+
287+
fn start_ping360_software_mode(
288+
handler: DeviceActorHandler,
289+
device_id: Uuid,
290+
properties: Ping360Properties,
291+
) -> tokio::task::JoinHandle<()> {
292+
tokio::spawn(async move {
293+
loop {
294+
let config = properties.continuous_mode_settings.clone();
295+
let initial_settings = match config.read() {
296+
Ok(settings) => settings.clone(),
297+
Err(err) => {
298+
error!("Failed to read Ping360Config: {err:?}, device: {device_id}");
299+
break;
300+
}
301+
};
302+
303+
let mut angle = initial_settings.start_angle;
304+
let step_size = initial_settings.num_steps as u16;
305+
let is_full_circle =
306+
initial_settings.start_angle == 0 && initial_settings.stop_angle == 399;
307+
let mut direction = 1i16;
308+
309+
loop {
310+
let current_settings = match config.read() {
311+
Ok(settings) => settings.clone(),
312+
Err(err) => {
313+
error!("Failed to read Ping360Config: {err:?}, device: {device_id}");
314+
break;
315+
}
316+
};
317+
if initial_settings != current_settings {
318+
break;
319+
}
320+
321+
match handler
322+
.send(crate::device::devices::PingRequest::Ping360(
323+
crate::device::devices::Ping360Request::Transducer(
324+
bluerobotics_ping::ping360::TransducerStruct {
325+
mode: initial_settings.mode,
326+
gain_setting: initial_settings.gain_setting,
327+
transmit_duration: initial_settings.transmit_duration,
328+
sample_period: initial_settings.sample_period,
329+
transmit_frequency: initial_settings.transmit_frequency,
330+
number_of_samples: initial_settings.number_of_samples,
331+
angle,
332+
transmit: 1,
333+
reserved: 0,
334+
},
335+
),
336+
))
337+
.await
338+
{
339+
Ok(answer) => match answer {
340+
crate::device::devices::PingAnswer::PingMessage(msg) => {
341+
Self::ping360_continuous_mode_helper(msg, device_id)
342+
}
343+
msg => {
344+
error!("Unexpected message during scan: {msg:?}");
345+
return;
346+
}
347+
},
348+
Err(err) => {
349+
error!("Failed to send transducer command: {err:?}");
350+
return;
351+
}
352+
}
353+
354+
angle = Self::calculate_next_angle(
355+
angle,
356+
step_size,
357+
is_full_circle,
358+
&mut direction,
359+
initial_settings.start_angle,
360+
initial_settings.stop_angle,
361+
);
362+
}
363+
}
364+
})
365+
}
366+
367+
fn calculate_next_angle(
368+
current_angle: u16,
369+
step_size: u16,
370+
is_full_circle: bool,
371+
direction: &mut i16,
372+
start_angle: u16,
373+
stop_angle: u16,
374+
) -> u16 {
375+
if is_full_circle {
376+
if current_angle + step_size >= 400 {
377+
0
378+
} else {
379+
current_angle + step_size
380+
}
381+
} else {
382+
if *direction > 0 {
383+
if current_angle + step_size > stop_angle {
384+
*direction = -1;
385+
stop_angle
386+
} else {
387+
current_angle + step_size
388+
}
389+
} else {
390+
if (current_angle as i32 - step_size as i32) <= start_angle as i32 {
391+
*direction = 1;
392+
start_angle
393+
} else {
394+
current_angle.wrapping_sub(step_size)
395+
}
396+
}
397+
}
398+
}
208399
}

0 commit comments

Comments
 (0)