94 lines
2.2 KiB
Rust
94 lines
2.2 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[y] refers to the yth byte for SPI communication
|
|
data: [0, 0, 0, 0],
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Set attenuations of all attenuators
|
|
* att[x] refers to the attenuation for channel x
|
|
*/
|
|
pub fn set_attenuation(&mut self, att: [f32; 4]) -> Result<(), 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;
|
|
}
|
|
// Set data as attenuation * 2
|
|
// Flip data using bitwise XOR, active low data
|
|
// Data is most signifant attenuator first
|
|
self.data[3-i] = (((atten * 2.0) as u8) ^ 0xFF) << 2
|
|
}
|
|
let mut clone = self.data.clone();
|
|
// Transmit SPI once to set attenuation
|
|
match self.spi.transfer(&mut clone).map_err(Error::SPI) {
|
|
Err(e) => Err(Error::AttenuatorError),
|
|
Ok(_) => Ok(()),
|
|
}
|
|
}
|
|
|
|
pub fn set_channel_attenuation(&mut self, channel: u8, attenuation: f32) -> Result<(), 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(()),
|
|
Err(e) => Err(e)
|
|
}
|
|
}
|
|
|
|
pub fn get_channel_attenuation(&mut self, channel: u8) -> Result<f32, Error<E>> {
|
|
assert!(channel < 4);
|
|
match self.get_attenuation() {
|
|
Ok(arr) => Ok(arr[channel as usize]),
|
|
Err(e) => Err(e),
|
|
}
|
|
}
|
|
|
|
pub fn get_attenuation(&mut self) -> Result<[f32; 4], Error<E>> {
|
|
let mut clone = self.data.clone();
|
|
match self.spi.transfer(&mut clone).map_err(Error::SPI) {
|
|
Ok(arr) => {
|
|
let mut ret :[f32; 4] = [0.0; 4];
|
|
for index in 0..4 {
|
|
ret[index] = ((arr[3 - index] ^ 0xFC) as f32) / 8.0;
|
|
}
|
|
Ok(ret)
|
|
},
|
|
Err(e) => Err(e),
|
|
}
|
|
}
|
|
}
|
|
|
|
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)
|
|
}
|
|
}
|