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
use super::Operation;
use crate::{
Amp, CPUControlFlags, DriverError, TxDatagram, MOD_BUF_SIZE_MAX, MOD_HEADER_INITIAL_DATA_SIZE,
MOD_HEADER_SUBSEQUENT_DATA_SIZE, MOD_SAMPLING_FREQ_DIV_MIN,
};
use anyhow::Result;
pub struct Modulation {
mod_data: Vec<u8>,
sent: usize,
freq_div: u32,
}
impl Modulation {
pub fn new(mod_data: Vec<Amp>, freq_div: u32) -> Self {
Self {
mod_data: mod_data.into_iter().map(Self::to_duty).collect(),
sent: 0,
freq_div,
}
}
pub fn to_duty(amp: Amp) -> u8 {
(amp.value().asin() * 2.0 / std::f64::consts::PI * 255.0) as u8
}
}
impl Operation for Modulation {
fn pack(&mut self, tx: &mut TxDatagram) -> Result<()> {
if self.mod_data.len() > MOD_BUF_SIZE_MAX {
return Err(DriverError::ModulationSizeOutOfRange(self.mod_data.len()).into());
}
let is_first_frame = self.sent == 0;
let max_size = if is_first_frame {
MOD_HEADER_INITIAL_DATA_SIZE
} else {
MOD_HEADER_SUBSEQUENT_DATA_SIZE
};
let mod_size = (self.mod_data.len() - self.sent).min(max_size);
if mod_size == 0 {
tx.header_mut().cpu_flag.remove(CPUControlFlags::MOD);
return Ok(());
}
let is_last_frame = self.sent + mod_size == self.mod_data.len();
tx.header_mut().cpu_flag.set(CPUControlFlags::MOD, true);
tx.header_mut().cpu_flag.remove(CPUControlFlags::MOD_BEGIN);
tx.header_mut().cpu_flag.remove(CPUControlFlags::MOD_END);
tx.header_mut().size = mod_size as _;
if is_first_frame {
if self.freq_div < MOD_SAMPLING_FREQ_DIV_MIN {
return Err(DriverError::ModFreqDivOutOfRange(self.freq_div).into());
}
tx.header_mut()
.cpu_flag
.set(CPUControlFlags::MOD_BEGIN, true);
tx.header_mut().mod_initial_mut().freq_div = self.freq_div;
tx.header_mut().mod_initial_mut().data[0..mod_size]
.copy_from_slice(&self.mod_data[self.sent..]);
} else {
tx.header_mut().mod_subsequent_mut().data[0..mod_size]
.copy_from_slice(&self.mod_data[self.sent..]);
}
if is_last_frame {
tx.header_mut().cpu_flag.set(CPUControlFlags::MOD_END, true);
}
self.sent += mod_size;
Ok(())
}
fn init(&mut self) {
self.sent = 0;
}
fn is_finished(&self) -> bool {
self.sent == self.mod_data.len()
}
}