diff --git a/firmware/README.md b/firmware/README.md index c204219..52a28e2 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -66,3 +66,4 @@ The scope of this setting is per TCP session. | `pid <0/1> output_max ` | | | `pid <0/1> integral_min ` | | | `pid <0/1> integral_max ` | | +| `postfilter <0/1> rate ` | Set postfilter output data rate | diff --git a/firmware/src/ad7172/adc.rs b/firmware/src/ad7172/adc.rs index a9d01d5..5c87683 100644 --- a/firmware/src/ad7172/adc.rs +++ b/firmware/src/ad7172/adc.rs @@ -78,6 +78,29 @@ impl, NSS: OutputPin> Adc { Ok(()) } + pub fn get_postfilter(&mut self, index: u8) -> Result, AdcError> { + self.read_reg(®s::FiltCon { index }) + .map(|data| { + if data.enh_filt_en() { + Some(data.enh_filt()) + } else { + None + } + }) + } + + pub fn set_postfilter(&mut self, index: u8, filter: Option) -> Result<(), AdcError> { + self.update_reg(®s::FiltCon { index }, |data| { + match filter { + None => data.set_enh_filt_en(false), + Some(filter) => { + data.set_enh_filt_en(true); + data.set_enh_filt(filter); + } + } + }) + } + /// Returns the channel the data is from pub fn data_ready(&mut self) -> Result, AdcError> { self.read_reg(®s::Status) diff --git a/firmware/src/ad7172/mod.rs b/firmware/src/ad7172/mod.rs index 7543d17..7b35f1b 100644 --- a/firmware/src/ad7172/mod.rs +++ b/firmware/src/ad7172/mod.rs @@ -111,6 +111,7 @@ impl fmt::Display for RefSource { } } +#[derive(Clone, Copy)] #[repr(u8)] pub enum PostFilter { /// 27 SPS, 47 dB rejection, 36.7 ms settling @@ -124,6 +125,49 @@ pub enum PostFilter { Invalid = 0b111, } +impl PostFilter { + pub const VALID_VALUES: &'static [Self] = &[ + PostFilter::F27SPS, + PostFilter::F21SPS, + PostFilter::F20SPS, + PostFilter::F16SPS, + ]; + + pub fn closest(rate: f32) -> Option { + /// (x - y).abs() + fn d(x: f32, y: f32) -> f32 { + if x >= y { + x - y + } else { + y - x + } + } + + let mut best: Option<(f32, Self)> = None; + for value in Self::VALID_VALUES { + let error = d(rate, value.output_rate().unwrap()); + let better = best + .map(|(best_error, _)| error < best_error) + .unwrap_or(true); + if better { + best = Some((error, *value)); + } + } + best.map(|(_, best)| best) + } + + /// Samples per Second + pub fn output_rate(&self) -> Option { + match self { + PostFilter::F27SPS => Some(27.0), + PostFilter::F21SPS => Some(21.25), + PostFilter::F20SPS => Some(20.0), + PostFilter::F16SPS => Some(16.67), + PostFilter::Invalid => None, + } + } +} + impl From for PostFilter { fn from(x: u8) -> Self { match x { diff --git a/firmware/src/command_parser.rs b/firmware/src/command_parser.rs index f27a3d8..3a28ae6 100644 --- a/firmware/src/command_parser.rs +++ b/firmware/src/command_parser.rs @@ -5,7 +5,7 @@ use nom::{ bytes::complete::{is_a, tag, take_while1}, character::{is_digit, complete::{char, one_of}}, combinator::{complete, map, value}, - sequence::preceded, + sequence::{preceded, separated_pair}, multi::{fold_many0, fold_many1}, error::ErrorKind, }; @@ -66,6 +66,7 @@ pub enum ShowCommand { Reporting, Pwm, Pid, + PostFilter, } #[derive(Debug, Clone)] @@ -103,6 +104,10 @@ pub enum Command { parameter: PidParameter, value: f32, }, + PostFilter { + channel: usize, + rate: f32, + }, } fn end(input: &[u8]) -> IResult<&[u8], ()> { @@ -244,11 +249,34 @@ fn pid(input: &[u8]) -> IResult<&[u8], Result> { ))(input) } +fn postfilter(input: &[u8]) -> IResult<&[u8], Result> { + let (input, _) = tag("postfilter")(input)?; + alt(( + preceded( + whitespace, + |input| { + let (input, channel) = channel(input)?; + let (input, _) = whitespace(input)?; + let (input, _) = tag("rate")(input)?; + let (input, _) = whitespace(input)?; + let (input, rate) = float(input)?; + let result = rate + .map(|rate| Command::PostFilter { + channel, rate, + }); + Ok((input, result)) + } + ), + value(Ok(Command::Show(ShowCommand::PostFilter)), end) + ))(input) +} + fn command(input: &[u8]) -> IResult<&[u8], Result> { alt((value(Ok(Command::Quit), tag("quit")), map(report, Ok), pwm, pid, + postfilter, ))(input) } diff --git a/firmware/src/main.rs b/firmware/src/main.rs index b756934..a209073 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -305,6 +305,24 @@ fn main() -> ! { ); } } + Command::Show(ShowCommand::PostFilter) => { + for (channel, _) in states.iter().enumerate() { + match adc.get_postfilter(channel as u8).unwrap() { + Some(filter) => { + let _ = writeln!( + socket, "channel {}: postfilter={:.2} SPS", + channel, filter.output_rate().unwrap() + ); + } + None => { + let _ = writeln!( + socket, "channel {}: no postfilter", + channel + ); + } + } + } + } Command::Pwm { channel, mode: PwmMode::Manual { width, total }} => { states[channel].pid_enabled = false; board::set_timer_pwm(width, total); @@ -339,6 +357,21 @@ fn main() -> ! { } let _ = writeln!(socket, "PID parameter updated"); } + Command::PostFilter { channel, rate } => { + let filter = ad7172::PostFilter::closest(rate); + match filter { + Some(filter) => { + adc.set_postfilter(channel as u8, Some(filter)).unwrap(); + let _ = writeln!( + socket, "channel {}: postfilter set to {:.2} SPS", + channel, filter.output_rate().unwrap() + ); + } + None => { + let _ = writeln!(socket, "Unable to choose postfilter"); + } + } + } } Ok(SessionOutput::Error(e)) => { let _ = writeln!(socket, "Command error: {:?}", e);