88 lines
2.0 KiB
Rust
88 lines
2.0 KiB
Rust
use embedded_hal::blocking::spi::Transfer;
|
|
use cortex_m::asm::nop;
|
|
use cortex_m_semihosting::hprintln;
|
|
use core::assert;
|
|
|
|
use crate::Error;
|
|
|
|
pub struct Attenuator<SPI> {
|
|
spi: SPI,
|
|
data: [u8; 4],
|
|
}
|
|
|
|
impl<SPI, E> Attenuator<SPI>
|
|
where
|
|
SPI: Transfer<u8, Error = E>
|
|
{
|
|
pub fn new(spi: SPI) -> Self {
|
|
Attenuator {
|
|
spi,
|
|
data: [0, 0, 0, 0],
|
|
}
|
|
}
|
|
|
|
pub fn set_attenuation(&mut self, att: [f32; 4]) -> Result<[u8; 4], Error<E>> {
|
|
for i in 0..4 {
|
|
let mut atten = att[i];
|
|
if att[i] > 31.5 {
|
|
atten = 31.5;
|
|
}
|
|
if att[i] < 0.0 {
|
|
atten = 0.0;
|
|
}
|
|
self.data[i] = (atten * 2.0) as u8;
|
|
self.data[i] = self.data[i] << 2;
|
|
}
|
|
let mut clone = self.data.clone();
|
|
// Transmit SPI once to set attenuation
|
|
if self.spi.transfer(&mut clone).map_err(Error::SPI).is_err() {
|
|
return Err(Error::AttenuatorError);
|
|
}
|
|
|
|
// Transmit the same data again, to get the return value
|
|
// Report the data returned by SPI, or an error
|
|
clone = self.data.clone();
|
|
match self.spi.transfer(&mut clone).map_err(Error::SPI) {
|
|
Ok(arr) => {
|
|
Ok([arr[0], arr[1], arr[2], arr[3]])
|
|
},
|
|
err => Err(Error::AttenuatorError)
|
|
}
|
|
}
|
|
|
|
pub fn set_channel_attenuation(&mut self, channel: u8, attenuation: f32) -> Result<u8, Error<E>> {
|
|
assert!(channel < 4);
|
|
let mut arr: [f32; 4] = self.get_attenuation();
|
|
arr[channel as usize] = attenuation;
|
|
match self.set_attenuation(arr) {
|
|
Ok(v) => Ok(v[channel as usize]),
|
|
Err(e) => Err(e)
|
|
}
|
|
}
|
|
|
|
pub fn get_channel_attenuation(&mut self, channel: u8) -> f32 {
|
|
assert!(channel < 4);
|
|
(self.data[channel as usize] as f32)/8.0
|
|
}
|
|
|
|
pub fn get_attenuation(&mut self) -> [f32; 4] {
|
|
[
|
|
self.get_channel_attenuation(0),
|
|
self.get_channel_attenuation(1),
|
|
self.get_channel_attenuation(2),
|
|
self.get_channel_attenuation(3),
|
|
]
|
|
}
|
|
}
|
|
|
|
impl<SPI, E> Transfer<u8> for Attenuator<SPI>
|
|
where
|
|
SPI: Transfer<u8, Error = E>
|
|
{
|
|
type Error = Error<E>;
|
|
|
|
fn transfer<'w>(&mut self, words: &'w mut [u8]) -> Result<&'w [u8], Self::Error> {
|
|
self.spi.transfer(words).map_err(Error::SPI)
|
|
}
|
|
}
|