1#![allow(clippy::from_over_into)]
2
3use crate::{ParsingError, SupMCUError};
45use async_graphql::Json;
46use async_scoped::TokioScope;
47
48use futures::Future;
49use i2cdev::core::I2CDevice;
50use i2cdev::linux::LinuxI2CDevice;
51use log::{error, info, trace};
52use parsing::*;
53use regex::Regex;
54use std::{
55 collections::HashMap,
56 fmt::Debug,
57 fs::File,
58 path::{Path, PathBuf},
59 thread,
60 time::Duration,
61};
62use tokio::{runtime, time};
63
64#[cfg(checksum)]
65use crc::{Crc, CRC_32_CKSUM};
66
67#[cfg(not(test))]
68use log::debug; #[cfg(test)]
70use std::println as debug;
71
72mod discovery;
73
74#[cfg(test)]
75mod i2c;
76pub mod parsing;
78
79const HEADER_SIZE: usize = 5;
94const FOOTER_SIZE: usize = 8;
95const DEFAULT_RESPONSE_DELAY: f32 = 0.05;
96const DEFAULT_RETRIES: u8 = 5;
97const RETRY_TIME_INCREMENT: f64 = 0.1;
99#[cfg(checksum)]
100const CRC32: Crc<u32> = Crc::<u32>::new(&CRC_32_CKSUM);
101
102pub struct SupMCUModule<T: I2CDevice + Send + Sync> {
131 i2c_dev: Box<T>,
132 last_cmd: String,
134 definition: Option<SupMCUModuleDefinition>,
135 address: u16,
136 max_retries: Option<u8>,
137}
138
139impl<T> SupMCUModule<T>
140where
141 T: I2CDevice + Send + Sync,
142{
143 pub fn send_command<S: AsRef<str>>(&mut self, cmd: S) -> Result<(), SupMCUError> {
147 let mut cmd = cmd.as_ref().to_string();
148 if !cmd.ends_with('\n') {
149 cmd += "\n";
150 }
151 self.i2c_dev
152 .write(cmd.as_bytes())
153 .map_err(|e| SupMCUError::I2CCommandError(self.address, e.to_string()))?;
154 self.last_cmd = cmd[..cmd.len() - 1].to_string();
155 if let Ok(def) = self.get_definition() {
156 debug!(
157 "{}@{:#04X}: sent command: `{}`",
158 def.name, self.address, self.last_cmd
159 );
160 } else {
161 debug!("{}: sent command: `{}`", self.address, self.last_cmd);
162 }
163 Ok(())
164 }
165
166 pub fn request_telemetry(
168 &mut self,
169 telemetry_type: TelemetryType,
170 idx: usize,
171 ) -> Result<(), SupMCUError> {
172 let mut def = self
173 .get_definition()?
174 .telemetry
175 .iter()
176 .filter(|x| x.idx == idx && x.telemetry_type == telemetry_type);
177 let d = def
178 .next()
179 .ok_or(SupMCUError::TelemetryIndexError(telemetry_type, idx))?
180 .to_owned();
181 self.request_telemetry_by_def(&d)
182 }
183
184 pub fn get_telemetry(
186 &mut self,
187 telemetry_type: TelemetryType,
188 idx: usize,
189 ) -> Result<SupMCUTelemetry, SupMCUError> {
190 let mut def = self
191 .get_definition()?
192 .telemetry
193 .iter()
194 .filter(|x| x.idx == idx && x.telemetry_type == telemetry_type);
195 let d = def
196 .next()
197 .ok_or(SupMCUError::TelemetryIndexError(telemetry_type, idx))?
198 .to_owned();
199 self.get_telemetry_by_def(&d)
200 }
201
202 pub async fn get_telemetry_async(
204 &mut self,
205 telemetry_type: TelemetryType,
206 idx: usize,
207 ) -> Result<SupMCUTelemetry, SupMCUError> {
208 let mut def = self
209 .get_definition()?
210 .telemetry
211 .iter()
212 .filter(|x| x.idx == idx && x.telemetry_type == telemetry_type);
213 let d = def
214 .next()
215 .ok_or(SupMCUError::TelemetryIndexError(telemetry_type, idx))?
216 .to_owned();
217 self.get_telemetry_by_def_async(&d).await
218 }
219
220 pub fn request_telemetry_by_def(
222 &mut self,
223 def: &SupMCUTelemetryDefinition,
224 ) -> Result<(), SupMCUError> {
225 self.send_command(self.create_tlm_command(def)?)
226 }
227
228 pub fn get_telemetry_by_def(
230 &mut self,
231 def: &SupMCUTelemetryDefinition,
232 ) -> Result<SupMCUTelemetry, SupMCUError> {
233 self.request_telemetry_by_def(def)?;
234 self.i2c_delay();
235 self.read_telemetry_response_safe(def)
236 }
237
238 pub async fn get_telemetry_by_def_async(
240 &mut self,
241 def: &SupMCUTelemetryDefinition,
242 ) -> Result<SupMCUTelemetry, SupMCUError> {
243 self.request_telemetry_by_def(def)?;
244 self.i2c_delay_async().await;
245 self.read_telemetry_response_safe_async(def).await
246 }
247
248 pub fn get_all_telemetry(
250 &mut self,
251 ) -> Result<HashMap<String, Json<SupMCUTelemetryData>>, SupMCUError> {
252 let mut telemetry = HashMap::new();
253 self.get_definition()?
254 .telemetry
255 .to_owned()
256 .iter()
257 .for_each(|d| {
258 match self.get_telemetry_by_def(d) {
259 Ok(t) => telemetry.insert(d.name.clone(), Json(t.data)),
260 Err(e) => {
261 let v = Json(vec![SupMCUValue::Str(e.to_string())]);
262 telemetry.insert(d.name.clone(), v)
263 }
264 };
265 });
266 Ok(telemetry)
267 }
268
269 pub fn get_telemetry_by_names(
271 &mut self,
272 names: Vec<String>,
273 ) -> Result<HashMap<String, Json<SupMCUTelemetryData>>, SupMCUError> {
274 let available_names: Vec<&String> = self
275 .get_definition()?
276 .telemetry
277 .iter()
278 .map(|d| &d.name)
279 .collect();
280 for n in &names {
281 if !available_names.contains(&n) {
282 return Err(SupMCUError::UnknownTelemName(n.to_owned()));
283 }
284 }
285 let mut telemetry = HashMap::new();
286 self.get_definition()?
287 .telemetry
288 .to_owned()
289 .iter()
290 .filter(|d| names.contains(&d.name))
291 .for_each(|d| {
292 match self.get_telemetry_by_def(d) {
293 Ok(t) => telemetry.insert(d.name.clone(), Json(t.data)),
294 Err(e) => {
295 let v = Json(vec![SupMCUValue::Str(e.to_string())]);
296 telemetry.insert(d.name.clone(), v)
297 }
298 };
299 });
300 Ok(telemetry)
301 }
302
303 pub async fn get_all_telemetry_async(
305 &mut self,
306 ) -> Result<Vec<Result<SupMCUTelemetry, SupMCUError>>, SupMCUError> {
307 let mut telemetry = vec![];
308 for tlm_def in self.get_definition()?.telemetry.clone() {
309 telemetry.push(self.get_telemetry_by_def_async(&tlm_def).await);
310 }
311 Ok(telemetry)
312 }
313
314 pub fn read_telemetry_response(
316 &mut self,
317 def: &SupMCUTelemetryDefinition,
318 ) -> Result<SupMCUTelemetry, SupMCUError> {
319 let size = SupMCUModule::<T>::telemetry_response_size(def);
320 let mut buff = vec![0u8; size];
321 self.i2c_dev
322 .read(buff.as_mut_slice())
323 .map_err(|e| SupMCUError::I2CTelemetryError(self.address, e.to_string()))?;
324
325 #[cfg(checksum)]
326 {
327 let checksum = buff.split_off(buff.capacity() - FOOTER_SIZE);
328 self.validate(&buff, checksum)?;
329 }
330
331 trace!("Received telemetry response: {:?}", buff);
332 let tel =
333 SupMCUTelemetry::from_bytes(buff, def).map_err(SupMCUError::ParsingError)?;
334 if tel.header.ready {
335 Ok(tel)
336 } else {
337 Err(SupMCUError::NonReadyError(
338 self.address,
339 self.last_cmd.clone(),
340 ))
341 }
342 }
343
344 pub async fn read_telemetry_response_safe_async(
346 &mut self,
347 def: &SupMCUTelemetryDefinition,
348 ) -> Result<SupMCUTelemetry, SupMCUError> {
349 let resp = self.read_telemetry_response(def);
350 if let Err(SupMCUError::NonReadyError(..)) = resp {
351 self.retry_nonready_async(def, resp).await
352 } else {
353 resp
354 }
355 }
356
357 pub fn read_telemetry_response_safe(
359 &mut self,
360 def: &SupMCUTelemetryDefinition,
361 ) -> Result<SupMCUTelemetry, SupMCUError> {
362 let resp = self.read_telemetry_response(def);
363 if let Err(SupMCUError::NonReadyError(..)) = resp {
364 self.retry_nonready(def, resp)
365 } else {
366 resp
367 }
368 }
369
370 fn create_tlm_command(
372 &self,
373 def: &SupMCUTelemetryDefinition,
374 ) -> Result<String, SupMCUError> {
375 let cmd = if def.telemetry_type == TelemetryType::SupMCU {
376 "SUP"
377 } else {
378 &self.get_definition()?.name
379 };
380 Ok(format!("{cmd}:TEL? {}", def.idx))
381 }
382
383 fn response_delay(&self) -> f32 {
385 match &self.definition {
386 Some(def) => def.response_delay,
387 None => DEFAULT_RESPONSE_DELAY,
388 }
389 }
390
391 fn i2c_delay(&self) {
393 thread::sleep(Duration::from_secs_f32(self.response_delay()));
394 }
395
396 async fn i2c_delay_async(&self) {
398 time::sleep(Duration::from_secs_f32(self.response_delay())).await;
399 }
400
401 fn telemetry_response_size(def: &SupMCUTelemetryDefinition) -> usize {
407 def.format
408 .get_byte_length()
409 .unwrap_or_else(|| def.length.unwrap())
410 + HEADER_SIZE
411 + FOOTER_SIZE
412 }
413
414 #[cfg(checksum)]
416 fn validate(&self, data: &Vec<u8>, checksum: Vec<u8>) -> Result<(), SupMCUError> {
417 let mut rdr = Cursor::new(&checksum);
418 if CRC32.checksum(data) != rdr.read_u32::<LE>()? {
419 Err(SupMCUError::ValidationError())
420 } else {
421 Ok(())
422 }
423 }
424
425 async fn discover_cmd_name(&mut self) -> Result<(), SupMCUError> {
427 debug!(
428 "Discovering module command name for address {}",
429 self.address
430 );
431 if let SupMCUValue::Str(version) = &self
432 .get_telemetry_by_def_async(
433 &discovery::PremadeTelemetryDefs::FirmwareVersion.into(),
434 )
435 .await?
436 .data[0]
437 {
438 let v = version.to_string();
439 info!("{:#04X}: {}", self.address, v);
440 let def = self.get_definition_mut()?;
441 let mut cmd_name = v
442 .split(' ')
443 .next()
444 .ok_or_else(|| ParsingError::VersionParsingError(v.clone()))?
445 .split('-')
446 .next()
447 .ok_or_else(|| ParsingError::VersionParsingError(v.clone()))?
448 .to_string();
449 if cmd_name == "GPSRM" {
450 cmd_name = String::from("GPS")
451 } else if cmd_name == "RHM3" {
452 cmd_name = String::from("RHM")
453 }
454 def.name = cmd_name;
455 def.simulatable = v.contains("(on STM)") || v.contains("(on QSM)");
456 debug!("Version: {v}");
457 debug!("CMD Name: {}", self.get_definition()?.name);
458 }
459 Ok(())
460 }
461
462 async fn discover_telemetry_definition(
466 &mut self,
467 telemetry_type: TelemetryType,
468 idx: usize,
469 ) -> Result<SupMCUTelemetryDefinition, SupMCUError> {
470 fn normalize(name: String) -> String {
472 let re = Regex::new(r"[^a-zA-Z0-9]+").unwrap();
473 let mut s = re.replace_all(&name, "_").to_lowercase();
474 if s.ends_with('_') {
475 s = s[..s.len() - 1].to_owned()
476 }
477 s
478 }
479
480 debug!("Discovering {telemetry_type} telemetry item {idx}");
481
482 let mut def = SupMCUTelemetryDefinition {
483 idx,
484 telemetry_type,
485 ..Default::default()
486 };
487
488 trace!("Requesting telemetry name");
489 self.send_command(self.create_tlm_command(&def)? + ",NAME")?;
490 self.i2c_delay_async().await;
491
492 trace!("Parsing telemetry name");
493 let name_resp = self
494 .read_telemetry_response_safe_async(
495 &discovery::PremadeTelemetryDefs::Name.into(),
496 )
497 .await?;
498 if let SupMCUValue::Str(name) = &name_resp.data[0] {
499 def.name = normalize(name.to_string());
500 }
501
502 trace!("Requesting telemetry format");
503 self.send_command(self.create_tlm_command(&def)? + ",FORMAT")?;
504 self.i2c_delay_async().await;
505
506 trace!("Parsing telemetry format");
507 let format_resp = self
508 .read_telemetry_response_safe_async(
509 &discovery::PremadeTelemetryDefs::Format.into(),
510 )
511 .await?;
512 if let SupMCUValue::Str(format) = &format_resp.data[0] {
513 def.format = SupMCUFormat::new(format);
514 }
515
516 if def.format.get_byte_length().is_none() {
517 trace!("Format includes a string. Requesting telemetry length");
518 self.send_command(self.create_tlm_command(&def)? + ",LENGTH")?;
519 self.i2c_delay_async().await;
520
521 trace!("Parsing telemetry length");
522 let length_resp = self
523 .read_telemetry_response_safe_async(
524 &discovery::PremadeTelemetryDefs::Length.into(),
525 )
526 .await?;
527 if let SupMCUValue::U16(length) = length_resp.data[0] {
528 def.length = Some(length.into());
529 }
530 }
531
532 if self.get_definition()?.simulatable {
533 trace!("Checking whether telemetry item is simulatable");
534 self.send_command(self.create_tlm_command(&def)? + ",SIMULATABLE")?;
535 self.i2c_delay_async().await;
536
537 trace!("Parsing simulatability");
538 let simulatable_resp = self
539 .read_telemetry_response_safe_async(
540 &discovery::PremadeTelemetryDefs::Simulatable.into(),
541 )
542 .await?;
543 if let SupMCUValue::U16(simulatable) = simulatable_resp.data[0] {
544 if simulatable == 1 {
545 trace!("Telemetry item is simulatable. Requesting default values.");
546 let defaults = self.get_telemetry_by_def_async(&def).await?;
547 def.default_sim_value = Some(defaults.data);
548 } else {
549 trace!("Telemetry item is not simulatable.");
550 }
551 }
552 }
553 Ok(def)
554 }
555
556 async fn discover_all_telemetry(&mut self) -> Result<(), SupMCUError> {
557 debug!(
558 "Discovering SupMCU telemetry definitions for {}",
559 self.get_definition()?.name
560 );
561 let vals = self
562 .get_telemetry_by_def_async(
563 &discovery::PremadeTelemetryDefs::TlmAmount.into(),
564 )
565 .await?
566 .data;
567 if let SupMCUValue::U16(supmcu_amount) = vals[0] {
568 for i in 0..supmcu_amount {
569 let def = self
570 .discover_telemetry_definition(TelemetryType::SupMCU, i as usize)
571 .await?;
572 self.get_definition_mut()?.telemetry.push(def);
573 }
574 }
575 debug!(
576 "Discovering module telemetry definitions for {}",
577 self.get_definition()?.name
578 );
579 if let SupMCUValue::U16(module_amount) = vals[1] {
580 for i in 0..module_amount {
581 let def = self
582 .discover_telemetry_definition(TelemetryType::Module, i as usize)
583 .await?;
584 self.get_definition_mut()?.telemetry.push(def);
585 }
586 }
587 Ok(())
588 }
589
590 async fn discover_commands(&mut self) -> Result<(), SupMCUError> {
591 debug!("Discovering commands for {}", self.get_definition()?.name);
592 let val = self
593 .get_telemetry_by_def_async(
594 &discovery::PremadeTelemetryDefs::CmdAmount.into(),
595 )
596 .await?
597 .data;
598 if let SupMCUValue::U16(commands_amount) = val[0] {
599 for i in 0..commands_amount {
600 self.send_command(format!("SUP:COM? {i}"))?;
601 self.i2c_delay_async().await;
602 if let SupMCUValue::Str(name) = &self
603 .read_telemetry_response_safe_async(
604 &discovery::PremadeTelemetryDefs::CmdName.into(),
605 )
606 .await?
607 .data[0]
608 {
609 self.get_definition_mut()?.commands.push(SupMCUCommand {
610 name: name.to_string(),
611 idx: i,
612 })
613 }
614 }
615 }
616 Ok(())
617 }
618
619 async fn discover(&mut self) -> Result<(), SupMCUError> {
621 if self.definition.is_none() {
622 self.definition = Some(SupMCUModuleDefinition {
623 address: self.address,
624 ..Default::default()
625 });
626 }
627 self.discover_cmd_name().await?;
628 self.discover_all_telemetry().await?;
629 if self.get_definition()?.name != "DCPS" {
630 self.discover_commands().await?;
631 }
632 Ok(())
633 }
634
635 pub fn get_definition_mut(
637 &mut self,
638 ) -> Result<&mut SupMCUModuleDefinition, SupMCUError> {
639 self.definition
640 .as_mut()
641 .ok_or(SupMCUError::MissingDefinitionError)
642 }
643
644 pub fn get_definition(&self) -> Result<&SupMCUModuleDefinition, SupMCUError> {
646 self.definition
647 .as_ref()
648 .ok_or(SupMCUError::MissingDefinitionError)
649 }
650
651 pub fn set_definition(&mut self, def: SupMCUModuleDefinition) {
653 self.address = def.address;
654 self.definition = Some(def);
655 }
656
657 pub fn matches(&self, other: &SupMCUModuleDefinition) -> bool {
659 match self.get_definition() {
660 Ok(def) => other.address == def.address || other.name == def.name,
661 Err(_) => false,
662 }
663 }
664
665 async fn retry_nonready_async(
669 &mut self,
670 def: &SupMCUTelemetryDefinition,
671 resp: Result<SupMCUTelemetry, SupMCUError>,
672 ) -> Result<SupMCUTelemetry, SupMCUError> {
673 if self.max_retries.is_none() {
674 return resp;
675 }
676 let mut retries = 0;
677 loop {
678 self.send_command(self.last_cmd.clone())?;
679 time::sleep(time::Duration::from_secs_f64(
680 self.response_delay() as f64 + RETRY_TIME_INCREMENT * retries as f64,
681 ))
682 .await;
683 let resp = self.read_telemetry_response(def);
684 if let Err(SupMCUError::NonReadyError(..)) = resp {
685 debug!("{} sent a non-ready response.", self.get_definition()?.name);
686 retries += 1;
687 if retries > self.max_retries.unwrap() {
688 debug!(
689 "Max retries exceeded, returning `SupMCUError::NonReadyError`"
690 );
691 break resp;
692 }
693 debug!("Retrying...");
694 continue;
695 } else {
696 break resp;
697 }
698 }
699 }
700
701 fn retry_nonready(
702 &mut self,
703 def: &SupMCUTelemetryDefinition,
704 resp: Result<SupMCUTelemetry, SupMCUError>,
705 ) -> Result<SupMCUTelemetry, SupMCUError> {
706 if self.max_retries.is_none() {
707 return resp;
708 }
709 let mut retries = 0;
710 loop {
711 self.send_command(self.last_cmd.clone())?;
712 thread::sleep(time::Duration::from_secs_f64(
713 self.response_delay() as f64 + RETRY_TIME_INCREMENT * retries as f64,
714 ));
715 let resp = self.read_telemetry_response(def);
716 if let Err(SupMCUError::NonReadyError(..)) = resp {
717 debug!("{} sent a non-ready response.", self.get_definition()?.name);
718 retries += 1;
719 if retries > self.max_retries.unwrap() {
720 debug!(
721 "Max retries exceeded, returning `SupMCUError::NonReadyError`"
722 );
723 break resp;
724 }
725 debug!("Retrying...");
726 continue;
727 } else {
728 break resp;
729 }
730 }
731 }
732
733 pub fn get_address(&self) -> u16 {
735 self.address
736 }
737}
738
739impl<T> Debug for SupMCUModule<T>
740where
741 T: I2CDevice + Send + Sync,
742{
743 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
744 f.debug_struct("SupMCUModule")
745 .field("address", &self.address)
746 .field("response_delay", &self.response_delay())
747 .field("max_retries", &self.max_retries)
748 .field("last_cmd", &self.last_cmd)
749 .finish()
750 }
751}
752
753impl SupMCUModule<LinuxI2CDevice> {
754 pub fn new(
756 device: &str,
757 address: u16,
758 max_retries: Option<u8>,
759 ) -> Result<Self, SupMCUError> {
760 let dev = LinuxI2CDevice::new(device, address).map_err(|error| {
761 SupMCUError::I2CDevError {
762 device: String::from(device),
763 address,
764 error,
765 }
766 })?;
767 Ok(SupMCUModule {
768 i2c_dev: Box::new(dev),
769 last_cmd: "".into(),
770 definition: None,
771 max_retries,
772 address,
773 })
774 }
775
776 pub fn new_from_def(
778 device: &str,
779 max_retries: Option<u8>,
780 def: SupMCUModuleDefinition,
781 ) -> Result<Self, SupMCUError> {
782 let address = def.address;
783 let dev = LinuxI2CDevice::new(device, def.address).map_err(|error| {
784 SupMCUError::I2CDevError {
785 device: String::from(device),
786 address,
787 error,
788 }
789 })?;
790 Ok(SupMCUModule {
791 i2c_dev: Box::new(dev),
792 definition: Some(def),
793 last_cmd: "".into(),
794 max_retries,
795 address,
796 })
797 }
798}
799
800pub struct SupMCUMaster<I: I2CDevice + Send + Sync> {
840 pub modules: Vec<SupMCUModule<I>>,
842 def_file: Option<PathBuf>,
843 rt: runtime::Runtime,
844}
845
846impl<I> SupMCUMaster<I>
847where
848 I: I2CDevice + Send + Sync,
849{
850
851 pub fn discover_modules(&mut self) -> Result<(), SupMCUError> {
853 log::info!(
854 "Discovering modules: {:?}",
855 self.modules
856 .iter()
857 .map(|m| format!("{:#04X}", m.address))
858 .collect::<Vec<String>>()
859 );
860 self.for_each(|module: &mut SupMCUModule<I>| module.discover())
861 .into_iter()
862 .collect::<Result<Vec<()>, SupMCUError>>()?;
864 Ok(())
865 }
866
867 pub fn discover_module(
869 &mut self,
870 module: &SupMCUModuleDefinition,
871 ) -> Result<(), SupMCUError> {
872 for m in self.modules.iter_mut() {
873 if m.matches(module) {
874 return self.rt.block_on(async { m.discover().await });
875 }
876 }
877 Err(SupMCUError::ModuleNotFound(
878 module.name.clone(),
879 module.address,
880 ))
881 }
882
883 pub fn get_definitions(&self) -> Result<Vec<SupMCUModuleDefinition>, SupMCUError> {
885 self.modules
886 .iter()
887 .map(|module| Ok(module.get_definition()?.clone()))
888 .collect::<Result<Vec<SupMCUModuleDefinition>, SupMCUError>>()
889 }
890
891 pub fn get_all_telemetry(
893 &mut self,
894 ) -> Vec<Vec<Result<SupMCUTelemetry, SupMCUError>>> {
895 self.for_each(|module| async { module.get_all_telemetry_async().await.unwrap() })
896 }
897
898 pub fn with_module<F: FnOnce(&SupMCUModule<I>) -> O, O: Send + 'static>(
900 &self,
901 module: &SupMCUModuleDefinition,
902 f: F,
903 ) -> Result<O, SupMCUError> {
904 self.modules
905 .iter()
906 .find(|m| m.matches(module))
907 .map(f)
908 .ok_or(SupMCUError::ModuleNotFound(
909 module.name.clone(),
910 module.address,
911 ))
912 }
913
914 pub fn with_module_mut<F: FnOnce(&mut SupMCUModule<I>) -> O, O: Send + 'static>(
916 &mut self,
917 module: &SupMCUModuleDefinition,
918 f: F,
919 ) -> Result<O, SupMCUError> {
920 self.modules
921 .iter_mut()
922 .find(|m| m.matches(module))
923 .map(f)
924 .ok_or(SupMCUError::ModuleNotFound(
925 module.name.clone(),
926 module.address,
927 ))
928 }
929
930 pub fn send_command(
932 &mut self,
933 module: &SupMCUModuleDefinition,
934 command: &str,
935 ) -> Result<(), SupMCUError> {
936 let module_command = |module: &mut SupMCUModule<I>| module.send_command(command);
937 self.with_module_mut(module, module_command)?
938 }
939
940 pub fn response_delay(
942 &mut self,
943 module: &SupMCUModuleDefinition,
944 delay: f32,
945 ) -> Result<(), SupMCUError> {
946 self.with_module_mut(module, |m| -> Result<(), SupMCUError> {
947 m.definition
948 .as_mut()
949 .ok_or(SupMCUError::MissingDefinitionError)?
950 .response_delay = delay;
951 Ok(())
952 })??;
953 if let Some(file) = &self.def_file {
954 self.save_def_file(file)?;
955 }
956 Ok(())
957 }
958
959 pub fn for_each<'a, F, T, O>(&'a mut self, f: F) -> Vec<O>
961 where
962 F: Fn(&'a mut SupMCUModule<I>) -> T,
963 T: Future<Output = O> + Send,
964 O: Send + 'static,
965 {
966 self.rt.block_on(async {
968 let (_, outputs) = TokioScope::scope_and_block(|s| {
970 for module in self.modules.iter_mut() {
971 s.spawn(f(module));
973 }
974 });
975 outputs.into_iter().map(|t| t.unwrap()).collect::<Vec<O>>()
977 })
978 }
979
980 pub fn load_def_file(&mut self, file: &Path) -> Result<(), SupMCUError> {
982 let defs: Vec<SupMCUModuleDefinition> = serde_json::from_reader(File::open(file)?)?;
983 for (def, module) in defs.into_iter().zip(self.modules.iter_mut()) {
984 module.set_definition(def);
985 }
986 self.def_file = Some(file.to_path_buf());
987 Ok(())
988 }
989
990 pub fn save_def_file<P: AsRef<Path>>(&self, file: P) -> Result<(), SupMCUError> {
992 let file = File::create(&file)?;
993 serde_json::to_writer(file, &self.get_definitions()?).unwrap();
994 Ok(())
995 }
996}
997
998impl SupMCUMaster<LinuxI2CDevice> {
999 pub fn scan_bus(
1003 device: &str,
1004 blacklist: Option<Vec<u16>>,
1005 ) -> Result<Vec<u16>, SupMCUError> {
1006 debug!("scanning I2C bus");
1007 let address = 0x03;
1008 let mut dev = LinuxI2CDevice::new(device, address).map_err(|error| {
1009 SupMCUError::I2CDevError {
1010 device: String::from(device),
1011 address,
1012 error,
1013 }
1014 })?;
1015 let mut addresses = vec![];
1016
1017 for i in 0x03..0x78 {
1018 trace!("checking address 0x{i:x}");
1019 if dev.set_slave_address(i).is_err() {
1020 error!("failed to set address 0x{i:x}");
1021 continue;
1022 }
1023 if dev.smbus_read_byte().is_ok() {
1024 debug!("found valid address 0x{i:x}");
1025 if let Some(blacklist) = &blacklist {
1026 if let Err(_idx) = blacklist.binary_search(&i) {
1027 addresses.push(i);
1028 } else {
1029 debug!("skipping blacklisted address 0x{i:x}");
1030 }
1031 } else {
1032 addresses.push(i);
1033 }
1034 }
1035 }
1036 Ok(addresses)
1037 }
1038
1039 fn new_ext<S: AsRef<str>>(
1040 device: S,
1041 max_retries: Option<u8>,
1042 addresses: Option<Vec<u16>>,
1043 blacklist: Option<Vec<u16>>,
1044 ) -> Result<Self, SupMCUError> {
1045 let device = device.as_ref();
1046 let addresses = if let Some(addrs) = addresses {
1047 addrs
1048 } else {
1049 SupMCUMaster::scan_bus(device, blacklist)?
1050 };
1051 Ok(SupMCUMaster {
1052 modules: addresses
1053 .into_iter()
1054 .map(|addr| SupMCUModule::new(device, addr, max_retries))
1055 .collect::<Result<Vec<SupMCUModule<LinuxI2CDevice>>, SupMCUError>>()?,
1056 def_file: None,
1057 rt: runtime::Builder::new_multi_thread()
1058 .worker_threads(2)
1059 .enable_all()
1060 .build()?,
1061 })
1062 }
1063
1064 pub fn new<S: AsRef<str>>(
1066 device: S,
1067 blacklist: Option<Vec<u16>>,
1068 ) -> Result<Self, SupMCUError> {
1069 SupMCUMaster::new_ext(device, Some(DEFAULT_RETRIES), None, blacklist)
1070 }
1071
1072 pub fn new_with_addrs<S: AsRef<str>>(
1074 device: S,
1075 addresses: Vec<u16>,
1076 ) -> Result<Self, SupMCUError> {
1077 SupMCUMaster::new_ext(device, Some(DEFAULT_RETRIES), Some(addresses), None)
1078 }
1079
1080 pub fn new_from_file<S: AsRef<str>, P: AsRef<Path>>(
1082 device: S,
1083 file: P,
1084 ) -> Result<Self, SupMCUError> {
1085 let def_file = Some(PathBuf::from(file.as_ref()));
1086 let defs: Vec<SupMCUModuleDefinition> = serde_json::from_reader(File::open(file)?)?;
1087 let modules = defs
1088 .into_iter()
1089 .map(|d| SupMCUModule::new_from_def(device.as_ref(), None, d).unwrap())
1090 .collect();
1091 Ok(SupMCUMaster {
1092 modules,
1093 def_file,
1094 rt: runtime::Builder::new_multi_thread()
1095 .worker_threads(2)
1096 .enable_all()
1097 .build()?,
1098
1099 })
1100 }
1101
1102 pub fn new_no_retries<S: AsRef<str>>(device: S) -> Result<Self, SupMCUError> {
1105 SupMCUMaster::new_ext(device, None, None, None)
1106 }
1107}
1108
1109#[cfg(test)]
1110mod test {
1111
1112 use i2c::TestI2CDevice;
1113 use rand::rngs::SmallRng;
1114 use rand::SeedableRng;
1115
1116 use super::*;
1117
1118 impl SupMCUModule<TestI2CDevice> {
1119 pub fn new_test(
1120 rng: SmallRng,
1121 def: SupMCUModuleDefinition,
1122 nonreadys: bool,
1123 max_retries: Option<u8>,
1124 ) -> Result<Self, SupMCUError> {
1125 Ok(SupMCUModule {
1126 i2c_dev: Box::new(TestI2CDevice::new(rng, def, nonreadys)),
1127 last_cmd: "".into(),
1128 definition: None,
1129 max_retries,
1130 address: 0,
1131 })
1132 }
1133
1134 pub fn update_def(&mut self) {
1135 self.i2c_dev.definition = self.definition.clone().unwrap();
1136 }
1137 }
1138
1139 impl SupMCUMaster<TestI2CDevice> {
1140 pub fn new_test(
1141 rng: SmallRng,
1142 nonreadys: bool,
1143 max_retries: Option<u8>,
1144 ) -> Result<Self, SupMCUError> {
1145 let defs: Vec<SupMCUModuleDefinition> =
1146 serde_json::from_reader(File::open(Path::new("test-definition.json"))?)?;
1147
1148 Ok(SupMCUMaster {
1149 modules: defs
1150 .into_iter()
1151 .map(|def| {
1152 SupMCUModule::new_test(rng.clone(), def, nonreadys, max_retries)
1153 })
1154 .collect::<Result<Vec<SupMCUModule<TestI2CDevice>>, SupMCUError>>()?,
1155 def_file: None,
1156 rt: runtime::Builder::new_multi_thread()
1157 .worker_threads(2)
1158 .enable_all()
1159 .build()?,
1160 })
1161 }
1162 }
1163
1164 #[test]
1165 fn discover_module() {
1166 let rng = SmallRng::from_entropy();
1167
1168 SupMCUMaster::new_test(rng, true, Some(5))
1169 .unwrap()
1170 .discover_modules()
1171 .unwrap();
1172 }
1173
1174 #[test]
1178 #[should_panic]
1179 fn nonready_no_retry() {
1180 let rng = SmallRng::from_entropy();
1181
1182 SupMCUMaster::new_test(rng, true, None)
1183 .unwrap()
1184 .discover_modules()
1185 .unwrap();
1186 }
1187
1188 #[test]
1189 fn get_telemetry_values() {
1190 let rng = SmallRng::from_entropy();
1192 let mut master = SupMCUMaster::new_test(rng.clone(), false, Some(5)).unwrap();
1193 master
1194 .load_def_file(Path::new("test-definition.json"))
1195 .unwrap();
1196 for module in master.modules.iter_mut() {
1197 let mut local_rng = rng.clone();
1199 for tel_def in module
1200 .get_definition_mut()
1201 .unwrap()
1202 .telemetry
1203 .clone()
1204 .iter_mut()
1205 {
1206 if tel_def.telemetry_type == TelemetryType::SupMCU
1208 && (tel_def.idx == 0 || tel_def.idx == 14 || tel_def.idx == 17 || tel_def.idx ==19)
1209 {
1210 continue;
1211 }
1212 assert_eq!(
1213 module.get_telemetry_by_def(tel_def).unwrap().data,
1215 tel_def.format.random_data(&mut local_rng)
1216 );
1217 }
1218 }
1219 }
1220
1221 #[test]
1223 fn save_load_defs() {
1224 let tmp_path = "test-definition.tmp";
1225 let rng = SmallRng::from_entropy();
1226 let mut master = SupMCUMaster::new_test(rng.clone(), false, Some(5)).unwrap();
1227 master
1228 .load_def_file(Path::new("test-definition.json"))
1229 .unwrap();
1230 master.save_def_file(Path::new(tmp_path)).unwrap();
1231 let mut reload_master = SupMCUMaster::new_test(rng, false, Some(5)).unwrap();
1232 match reload_master.load_def_file(Path::new(tmp_path)) {
1233 Ok(m) => m,
1234 Err(e) => {
1235 std::fs::remove_file(tmp_path).unwrap();
1236 panic!("{}", e);
1237 }
1238 };
1239 std::fs::remove_file(tmp_path).unwrap();
1240 assert_eq!(
1241 master.get_definitions().unwrap(),
1242 reload_master.get_definitions().unwrap(),
1243 );
1244 }
1245}