From da4aaf4ff6d6fee5517b8aabf8756b9fcf8f9ea1 Mon Sep 17 00:00:00 2001 From: Astro Date: Thu, 19 Mar 2020 18:34:57 +0100 Subject: [PATCH] implement all the tcp commands --- src/ad5680.rs | 2 + src/command_parser.rs | 175 ++++++++++---------- src/main.rs | 359 ++++++++++++++++++++++++------------------ src/pid.rs | 14 ++ src/pins.rs | 12 +- 5 files changed, 310 insertions(+), 252 deletions(-) diff --git a/src/ad5680.rs b/src/ad5680.rs index 7573d8b..53936ff 100644 --- a/src/ad5680.rs +++ b/src/ad5680.rs @@ -14,6 +14,8 @@ pub const SPI_MODE: spi::Mode = spi::Mode { /// 30 MHz pub const SPI_CLOCK: MegaHertz = MegaHertz(30); +pub const MAX_VALUE: u32 = 0x20000; + /// [AD5680](https://www.analog.com/media/en/technical-documentation/data-sheets/AD5680.pdf) DAC pub struct Dac, S: OutputPin> { spi: SPI, diff --git a/src/command_parser.rs b/src/command_parser.rs index 4366ad3..d075faa 100644 --- a/src/command_parser.rs +++ b/src/command_parser.rs @@ -85,30 +85,28 @@ pub enum PidParameter { /// Steinhart-Hart equation parameter #[derive(Debug, Clone, PartialEq)] pub enum ShParameter { - A, - B, - C, - ParallelR, + T0, + R, + R0, } -#[derive(Debug, Clone, PartialEq)] -pub struct PwmConfig { - pub width: u16, - pub total: u16, +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum PwmPin { + ISet, + MaxIPos, + MaxINeg, + MaxV, } -#[derive(Debug, Clone, PartialEq)] -pub enum PwmMode { - Manual(PwmConfig), - Pid, -} - -#[derive(Debug, Clone, PartialEq)] -pub enum PwmSetup { - ISet(PwmMode), - MaxIPos(PwmConfig), - MaxINeg(PwmConfig), - MaxV(PwmConfig), +impl PwmPin { + pub fn name(&self) -> &'static str { + match self { + PwmPin::ISet => "i_set", + PwmPin::MaxIPos => "max_i_pos", + PwmPin::MaxINeg => "max_i_neg", + PwmPin::MaxV => "max_v", + } + } } #[derive(Debug, Clone, PartialEq)] @@ -116,10 +114,17 @@ pub enum Command { Quit, Show(ShowCommand), Reporting(bool), + /// PWM parameter setting Pwm { channel: usize, - setup: PwmSetup, + pin: PwmPin, + duty: u32, }, + /// Enable PID control for `i_set` + PwmPid { + channel: usize, + }, + /// PID parameter setting Pid { channel: usize, parameter: PidParameter, @@ -149,7 +154,7 @@ fn whitespace(input: &[u8]) -> IResult<&[u8], ()> { fold_many1(char(' '), (), |(), _| ())(input) } -fn unsigned(input: &[u8]) -> IResult<&[u8], Result> { +fn unsigned(input: &[u8]) -> IResult<&[u8], Result> { take_while1(is_digit)(input) .map(|(input, digits)| { let result = lexical::parse(digits) @@ -202,82 +207,77 @@ fn report(input: &[u8]) -> IResult<&[u8], Command> { )(input) } -/// `pwm ... ` - Set pwm duty cycle -fn pwm_config(input: &[u8]) -> IResult<&[u8], Result> { - let (input, width) = unsigned(input)?; - let width = match width { - Ok(width) => width, - Err(e) => return Ok((input, Err(e.into()))), - }; - let (input, _) = whitespace(input)?; - let (input, total) = unsigned(input)?; - let total = match total { - Ok(total) => total, - Err(e) => return Ok((input, Err(e.into()))), - }; - Ok((input, Ok(PwmConfig { width, total }))) -} +fn pwm_setup(input: &[u8]) -> IResult<&[u8], Result<(PwmPin, u32), Error>> { + let result_with_pin = |pin: PwmPin| + move |result: Result| + result.map(|duty| (pin, duty)); -fn pwm_setup(input: &[u8]) -> IResult<&[u8], Result> { alt(( map( preceded( tag("max_i_pos"), preceded( whitespace, - pwm_config + unsigned ) ), - |result| result.map(PwmSetup::MaxIPos) + result_with_pin(PwmPin::MaxIPos) ), map( preceded( tag("max_i_neg"), preceded( whitespace, - pwm_config + unsigned ) ), - |result| result.map(PwmSetup::MaxINeg) + result_with_pin(PwmPin::MaxINeg) ), map( preceded( tag("max_v"), preceded( whitespace, - pwm_config + unsigned ) ), - |result| result.map(PwmSetup::MaxV) + result_with_pin(PwmPin::MaxV) ), - map(pwm_config, |result| result.map(|config| { - PwmSetup::ISet(PwmMode::Manual(config)) - })) - ))(input) + map(unsigned, result_with_pin(PwmPin::ISet) + )) + )(input) } /// `pwm <0-1> pid` - Set PWM to be controlled by PID -fn pwm_pid(input: &[u8]) -> IResult<&[u8], Result> { - value(Ok(PwmSetup::ISet(PwmMode::Pid)), tag("pid"))(input) +fn pwm_pid(input: &[u8]) -> IResult<&[u8], ()> { + value((), tag("pid"))(input) } fn pwm(input: &[u8]) -> IResult<&[u8], Result> { let (input, _) = tag("pwm")(input)?; alt(( - preceded( - whitespace, - map( - separated_pair( - channel, - whitespace, - alt(( - pwm_pid, - pwm_setup - )) - ), - |(channel, setup)| setup.map(|setup| Command::Pwm { channel, setup }) - ) - ), + |input| { + let (input, _) = whitespace(input)?; + let (input, channel) = channel(input)?; + let (input, _) = whitespace(input)?; + let (input, result) = alt(( + |input| { + let (input, ()) = pwm_pid(input)?; + Ok((input, Ok(Command::PwmPid { channel }))) + }, + |input| { + let (input, config) = pwm_setup(input)?; + match config { + Ok((pin, duty)) => + Ok((input, Ok(Command::Pwm { channel, pin, duty }))), + Err(e) => + Ok((input, Err(e))), + } + }, + ))(input)?; + end(input)?; + Ok((input, result)) + }, value(Ok(Command::Show(ShowCommand::Pwm)), end) ))(input) } @@ -320,10 +320,9 @@ fn steinhart_hart_parameter(input: &[u8]) -> IResult<&[u8], Result, adc_time: Instant, + dac_value: u32, + pid_enabled: bool, + pid: pid::Controller, + sh: sh::Parameters, } impl Default for ChannelState { @@ -53,11 +61,16 @@ impl Default for ChannelState { ChannelState { adc_data: None, adc_time: Instant::from_secs(0), + dac_value: 0, + pid_enabled: false, + pid: pid::Controller::new(pid::Parameters::default()), + sh: sh::Parameters::default(), } } } +const HSE: MegaHertz = MegaHertz(8); #[cfg(not(feature = "semihosting"))] const WATCHDOG_INTERVAL: u32 = 100; #[cfg(feature = "semihosting")] @@ -65,12 +78,9 @@ const WATCHDOG_INTERVAL: u32 = 10_000; #[cfg(not(feature = "generate-hwaddr"))] const NET_HWADDR: [u8; 6] = [0x02, 0x00, 0xDE, 0xAD, 0xBE, 0xEF]; - const TCP_PORT: u16 = 23; -const HSE: MegaHertz = MegaHertz(8); - /// Initialization and main loop #[entry] fn main() -> ! { @@ -105,9 +115,10 @@ fn main() -> ! { let mut adc = ad7172::Adc::new(pins.adc_spi, pins.adc_nss).unwrap(); let mut dac0 = ad5680::Dac::new(pins.dac0_spi, pins.dac0_sync); - dac0.set(0); + dac0.set(0).unwrap(); let mut dac1 = ad5680::Dac::new(pins.dac1_spi, pins.dac1_sync); - dac1.set(0); + dac1.set(0).unwrap(); + let mut pwm = pins.pwm; timer::setup(cp.SYST, clocks); @@ -120,7 +131,9 @@ fn main() -> ! { }; info!("Net hwaddr: {}", hwaddr); - let mut channel_states = [ChannelState::default(); CHANNELS]; + let mut channel_states: [ChannelState; CHANNELS] = [ + ChannelState::default(), ChannelState::default() + ]; net::run(dp.ETHERNET_MAC, dp.ETHERNET_DMA, hwaddr, |iface| { Server::::run(iface, |server| { @@ -167,175 +180,213 @@ fn main() -> ! { socket, "t={} raw{}=0x{:06X}", state.adc_time, channel, adc_data ); - // TODO: show pwm status et al } } } Command::Show(ShowCommand::Pid) => { - // for (channel, state) in states.iter().enumerate() { - // let _ = writeln!(socket, "PID settings for channel {}", channel); - // let pid = &state.pid; - // let _ = writeln!(socket, "- target={:.4}", pid.get_target()); - // let p = pid.get_parameters(); - // macro_rules! out { - // ($p: tt) => { - // let _ = writeln!(socket, "- {}={:.4}", stringify!($p), p.$p); - // }; - // } - // out!(kp); - // out!(ki); - // out!(kd); - // out!(output_min); - // out!(output_max); - // out!(integral_min); - // out!(integral_max); - // let _ = writeln!(socket, ""); - // } + for (channel, state) in channel_states.iter().enumerate() { + let _ = writeln!(socket, "PID settings for channel {}", channel); + let pid = &state.pid; + let _ = writeln!(socket, "- target={:.4}", pid.get_target()); + let p = pid.get_parameters(); + macro_rules! out { + ($p: tt) => { + let _ = writeln!(socket, "- {}={:.4}", stringify!($p), p.$p); + }; + } + out!(kp); + out!(ki); + out!(kd); + out!(output_min); + out!(output_max); + out!(integral_min); + out!(integral_max); + let _ = writeln!(socket, ""); + } } Command::Show(ShowCommand::Pwm) => { - // for (channel, state) in states.iter().enumerate() { - // let _ = writeln!( - // socket, "channel {}: PID={}", - // channel, - // if state.pid_enabled { "engaged" } else { "disengaged" } - // ); - // for pin in TecPin::VALID_VALUES { - // let (width, total) = match channel { - // 0 => tec0.get(*pin), - // 1 => tec1.get(*pin), - // _ => unreachable!(), - // }; - // let _ = writeln!(socket, "- {}={}/{}", pin, width, total); - // } - // let _ = writeln!(socket, ""); - // } + for (channel, state) in channel_states.iter().enumerate() { + let _ = writeln!( + socket, "channel {}: PID={}", + channel, + if state.pid_enabled { "engaged" } else { "disengaged" } + ); + let _ = writeln!(socket, "- i_set={}/{}", state.dac_value, ad5680::MAX_VALUE); + fn show_pwm_channel(mut socket: S, name: &str, pin: &P) + where + S: core::fmt::Write, + P: hal::PwmPin, + { + let _ = writeln!( + socket, + "- {}={}/{}", + name, pin.get_duty(), pin.get_max_duty() + ); + } + match channel { + 0 => { + show_pwm_channel(socket.deref_mut(), "max_v", &pwm.max_v0); + show_pwm_channel(socket.deref_mut(), "max_i_pos", &pwm.max_i_pos0); + show_pwm_channel(socket.deref_mut(), "max_i_neg", &pwm.max_i_neg0); + } + 1 => { + show_pwm_channel(socket.deref_mut(), "max_v", &pwm.max_v1); + show_pwm_channel(socket.deref_mut(), "max_i_pos", &pwm.max_i_pos1); + show_pwm_channel(socket.deref_mut(), "max_i_neg", &pwm.max_i_neg1); + } + _ => unreachable!(), + } + let _ = writeln!(socket, ""); + } } Command::Show(ShowCommand::SteinhartHart) => { - // for (channel, state) in states.iter().enumerate() { - // let _ = writeln!( - // socket, "channel {}: Steinhart-Hart equation parameters", - // channel, - // ); - // let _ = writeln!(socket, "- a={}", state.sh.a); - // let _ = writeln!(socket, "- b={}", state.sh.b); - // let _ = writeln!(socket, "- c={}", state.sh.c); - // let _ = writeln!(socket, "- parallel_r={}", state.sh.parallel_r); - // let _ = writeln!(socket, ""); - // } + for (channel, state) in channel_states.iter().enumerate() { + let _ = writeln!( + socket, "channel {}: Steinhart-Hart equation parameters", + channel, + ); + let _ = writeln!(socket, "- t0={}", state.sh.t0); + let _ = writeln!(socket, "- r={}", state.sh.r); + let _ = writeln!(socket, "- r0={}", state.sh.r0); + let _ = writeln!(socket, ""); + } } 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 - // ); - // } - // } - // } + for (channel, _) in channel_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, setup: PwmSetup::ISet(PwmMode::Pid) } => { - // states[channel].pid_enabled = true; - // let _ = writeln!(socket, "channel {}: PID enabled to control PWM", channel - // ); + Command::PwmPid { channel } => { + channel_states[channel].pid_enabled = true; + let _ = writeln!(socket, "channel {}: PID enabled to control PWM", channel + ); } - Command::Pwm { channel, setup: PwmSetup::ISet(PwmMode::Manual(config))} => { - // states[channel].pid_enabled = false; - // let PwmConfig { width, total } = config; - // match channel { - // 0 => tec0.set(TecPin::ISet, width, total), - // 1 => tec1.set(TecPin::ISet, width, total), - // _ => unreachable!(), - // } - // let _ = writeln!( - // socket, "channel {}: PWM duty cycle manually set to {}/{}", - // channel, config.width, config.total - // ); + Command::Pwm { channel, pin: PwmPin::ISet, duty } if duty <= ad5680::MAX_VALUE => { + channel_states[channel].pid_enabled = false; + match channel { + 0 => dac0.set(duty).unwrap(), + 1 => dac1.set(duty).unwrap(), + _ => unreachable!(), + } + channel_states[channel].dac_value = duty; + let _ = writeln!( + socket, "channel {}: PWM duty cycle manually set to {}/{}", + channel, duty, ad5680::MAX_VALUE + ); } - Command::Pwm { channel, setup } => { - // let (pin, config) = match setup { - // PwmSetup::ISet(_) => - // // Handled above - // unreachable!(), - // PwmSetup::MaxIPos(config) => - // (TecPin::MaxIPos, config), - // PwmSetup::MaxINeg(config) => - // (TecPin::MaxINeg, config), - // PwmSetup::MaxV(config) => - // (TecPin::MaxV, config), - // }; - // let PwmConfig { width, total } = config; - // match channel { - // 0 => tec0.set(pin, width, total), - // 1 => tec1.set(pin, width, total), - // _ => unreachable!(), - // } - // let _ = writeln!( - // socket, "channel {}: PWM {} reconfigured to {}/{}", - // channel, pin, width, total - // ); + Command::Pwm { pin: PwmPin::ISet, duty, .. } if duty > ad5680::MAX_VALUE => { + let _ = writeln!( + socket, "error: PWM duty range must not exceed {}", + ad5680::MAX_VALUE + ); + } + Command::Pwm { channel, pin, duty } if duty <= 0xFFFF => { + let duty = duty as u16; + + fn set_pwm_channel>(pin: &mut P, duty: u16) -> u16 { + pin.set_duty(duty); + pin.get_max_duty() + } + let max = match (channel, pin) { + (_, PwmPin::ISet) => + // Handled above + unreachable!(), + (0, PwmPin::MaxIPos) => + set_pwm_channel(&mut pwm.max_i_pos0, duty), + (0, PwmPin::MaxINeg) => + set_pwm_channel(&mut pwm.max_i_neg0, duty), + (0, PwmPin::MaxV) => + set_pwm_channel(&mut pwm.max_v0, duty), + (1, PwmPin::MaxIPos) => + set_pwm_channel(&mut pwm.max_i_pos1, duty), + (1, PwmPin::MaxINeg) => + set_pwm_channel(&mut pwm.max_i_neg1, duty), + (1, PwmPin::MaxV) => + set_pwm_channel(&mut pwm.max_v1, duty), + _ => + unreachable!(), + }; + let _ = writeln!( + socket, "channel {}: PWM {} reconfigured to {}/{}", + channel, pin.name(), duty, max + ); + } + Command::Pwm { duty, .. } if duty > 0xFFFF => { + let _ = writeln!(socket, "error: PWM duty range must fit 16 bits"); } Command::Pid { channel, parameter, value } => { - // let pid = &mut states[channel].pid; - // use command_parser::PidParameter::*; - // match parameter { - // Target => - // pid.set_target(value), - // KP => - // pid.update_parameters(|parameters| parameters.kp = value), - // KI => - // pid.update_parameters(|parameters| parameters.ki = value), - // KD => - // pid.update_parameters(|parameters| parameters.kd = value), - // OutputMin => - // pid.update_parameters(|parameters| parameters.output_min = value), - // OutputMax => - // pid.update_parameters(|parameters| parameters.output_max = value), - // IntegralMin => - // pid.update_parameters(|parameters| parameters.integral_min = value), - // IntegralMax => - // pid.update_parameters(|parameters| parameters.integral_max = value), - // } - // pid.reset(); - // let _ = writeln!(socket, "PID parameter updated"); + let pid = &mut channel_states[channel].pid; + use command_parser::PidParameter::*; + match parameter { + Target => + pid.set_target(value), + KP => + pid.update_parameters(|parameters| parameters.kp = value), + KI => + pid.update_parameters(|parameters| parameters.ki = value), + KD => + pid.update_parameters(|parameters| parameters.kd = value), + OutputMin => + pid.update_parameters(|parameters| parameters.output_min = value), + OutputMax => + pid.update_parameters(|parameters| parameters.output_max = value), + IntegralMin => + pid.update_parameters(|parameters| parameters.integral_min = value), + IntegralMax => + pid.update_parameters(|parameters| parameters.integral_max = value), + } + pid.reset(); + let _ = writeln!(socket, "PID parameter updated"); } Command::SteinhartHart { channel, parameter, value } => { - // let sh = &mut states[channel].sh; - // use command_parser::ShParameter::*; - // match parameter { - // A => sh.a = value, - // B => sh.b = value, - // C => sh.c = value, - // ParallelR => sh.parallel_r = value, - // } - // let _ = writeln!(socket, "Steinhart-Hart equation parameter updated"); + let sh = &mut channel_states[channel].sh; + use command_parser::ShParameter::*; + match parameter { + T0 => sh.t0 = value, + R => sh.r = value, + R0 => sh.r0 = value, + } + sh.update(); + let _ = writeln!(socket, "Steinhart-Hart equation 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"); - // } - // } + 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"); + } + } + } + cmd => { + let _ = writeln!(socket, "Not yet implemented: {:?}", cmd); } } Ok(SessionOutput::Error(e)) => { let _ = writeln!(socket, "Command error: {:?}", e); } + Ok(o) => { + let _ = writeln!(socket, "Not yet implemented"); + } Err(_) => socket.close(), } diff --git a/src/pid.rs b/src/pid.rs index efdd730..04451ef 100644 --- a/src/pid.rs +++ b/src/pid.rs @@ -9,6 +9,20 @@ pub struct Parameters { pub integral_max: f64 } +impl Default for Parameters { + fn default() -> Self { + Parameters { + kp: 0.5, + ki: 0.05, + kd: 0.45, + output_min: 0.0, + output_max: 1.0, + integral_min: 0.0, + integral_max: 1.0, + } + } +} + #[derive(Clone)] pub struct Controller { parameters: Parameters, diff --git a/src/pins.rs b/src/pins.rs index b59fb4f..cfe5f90 100644 --- a/src/pins.rs +++ b/src/pins.rs @@ -171,12 +171,12 @@ impl Pins { } pub struct PwmPins { - max_v0: PwmChannels, - max_v1: PwmChannels, - max_i_pos0: PwmChannels, - max_i_pos1: PwmChannels, - max_i_neg0: PwmChannels, - max_i_neg1: PwmChannels, + pub max_v0: PwmChannels, + pub max_v1: PwmChannels, + pub max_i_pos0: PwmChannels, + pub max_i_pos1: PwmChannels, + pub max_i_neg0: PwmChannels, + pub max_i_neg1: PwmChannels, } impl PwmPins {