2020-09-25 05:04:29 +08:00
|
|
|
use serde::{Serialize, Deserialize};
|
2020-09-25 03:10:27 +08:00
|
|
|
use uom::si::{
|
|
|
|
electric_potential::volt,
|
|
|
|
electric_current::ampere,
|
2020-12-12 08:25:07 +08:00
|
|
|
f64::{ElectricCurrent, ElectricPotential},
|
2020-09-25 03:10:27 +08:00
|
|
|
};
|
2020-09-24 07:18:33 +08:00
|
|
|
use crate::{
|
2020-09-26 07:40:01 +08:00
|
|
|
ad7172::PostFilter,
|
2020-12-12 08:25:07 +08:00
|
|
|
channels::Channels,
|
2020-09-24 07:18:33 +08:00
|
|
|
command_parser::CenterPoint,
|
|
|
|
pid,
|
|
|
|
steinhart_hart,
|
|
|
|
};
|
|
|
|
|
2020-09-25 03:35:15 +08:00
|
|
|
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
2020-09-24 07:18:33 +08:00
|
|
|
pub struct ChannelConfig {
|
|
|
|
center: CenterPoint,
|
|
|
|
pid: pid::Parameters,
|
2020-09-25 02:45:07 +08:00
|
|
|
pid_target: f32,
|
2020-12-13 09:33:59 +08:00
|
|
|
pid_engaged: bool,
|
2020-12-12 08:25:07 +08:00
|
|
|
sh: steinhart_hart::Parameters,
|
2020-09-25 03:10:27 +08:00
|
|
|
pwm: PwmLimits,
|
2020-09-26 07:40:01 +08:00
|
|
|
/// uses variant `PostFilter::Invalid` instead of `None` to save space
|
|
|
|
adc_postfilter: PostFilter,
|
2020-09-24 07:18:33 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
impl ChannelConfig {
|
2020-09-25 03:10:27 +08:00
|
|
|
pub fn new(channels: &mut Channels, channel: usize) -> Self {
|
|
|
|
let pwm = PwmLimits::new(channels, channel);
|
2020-09-26 07:40:01 +08:00
|
|
|
|
|
|
|
let adc_postfilter = channels.adc.get_postfilter(channel as u8)
|
|
|
|
.unwrap()
|
|
|
|
.unwrap_or(PostFilter::Invalid);
|
|
|
|
|
2020-09-25 03:10:27 +08:00
|
|
|
let state = channels.channel_state(channel);
|
2020-09-24 07:18:33 +08:00
|
|
|
ChannelConfig {
|
|
|
|
center: state.center.clone(),
|
|
|
|
pid: state.pid.parameters.clone(),
|
2020-09-25 02:45:07 +08:00
|
|
|
pid_target: state.pid.target as f32,
|
2020-12-13 09:33:59 +08:00
|
|
|
pid_engaged: state.pid_engaged,
|
2020-12-12 08:25:07 +08:00
|
|
|
sh: state.sh.clone(),
|
2020-09-25 03:10:27 +08:00
|
|
|
pwm,
|
2020-09-26 07:40:01 +08:00
|
|
|
adc_postfilter,
|
2020-09-24 07:18:33 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-09-25 05:04:29 +08:00
|
|
|
pub fn apply(&self, channels: &mut Channels, channel: usize) {
|
|
|
|
let state = channels.channel_state(channel);
|
|
|
|
state.center = self.center.clone();
|
|
|
|
state.pid.parameters = self.pid.clone();
|
|
|
|
state.pid.target = self.pid_target.into();
|
2020-12-13 09:33:59 +08:00
|
|
|
state.pid_engaged = self.pid_engaged;
|
2020-12-12 08:25:07 +08:00
|
|
|
state.sh = self.sh.clone();
|
2020-09-26 07:40:01 +08:00
|
|
|
|
2020-09-25 05:04:29 +08:00
|
|
|
self.pwm.apply(channels, channel);
|
2020-09-26 07:40:01 +08:00
|
|
|
|
|
|
|
let adc_postfilter = match self.adc_postfilter {
|
|
|
|
PostFilter::Invalid => None,
|
|
|
|
adc_postfilter => Some(adc_postfilter),
|
|
|
|
};
|
|
|
|
let _ = channels.adc.set_postfilter(channel as u8, adc_postfilter);
|
2020-09-25 05:04:29 +08:00
|
|
|
}
|
2020-09-24 07:18:33 +08:00
|
|
|
}
|
2020-09-25 02:59:04 +08:00
|
|
|
|
2020-09-25 03:35:15 +08:00
|
|
|
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
|
2020-09-25 03:10:27 +08:00
|
|
|
struct PwmLimits {
|
2020-12-12 08:25:07 +08:00
|
|
|
max_v: f64,
|
|
|
|
max_i_pos: f64,
|
|
|
|
max_i_neg: f64,
|
2020-09-25 03:10:27 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
impl PwmLimits {
|
|
|
|
pub fn new(channels: &mut Channels, channel: usize) -> Self {
|
2021-01-25 13:22:27 +08:00
|
|
|
let max_v = channels.get_max_v(channel);
|
2020-09-25 03:10:27 +08:00
|
|
|
let (max_i_pos, _) = channels.get_max_i_pos(channel);
|
|
|
|
let (max_i_neg, _) = channels.get_max_i_neg(channel);
|
|
|
|
PwmLimits {
|
2020-12-12 08:25:07 +08:00
|
|
|
max_v: max_v.get::<volt>(),
|
|
|
|
max_i_pos: max_i_pos.get::<ampere>(),
|
|
|
|
max_i_neg: max_i_neg.get::<ampere>(),
|
2020-09-25 03:10:27 +08:00
|
|
|
}
|
|
|
|
}
|
2020-09-25 05:04:29 +08:00
|
|
|
|
|
|
|
pub fn apply(&self, channels: &mut Channels, channel: usize) {
|
2020-12-12 08:25:07 +08:00
|
|
|
channels.set_max_v(channel, ElectricPotential::new::<volt>(self.max_v));
|
|
|
|
channels.set_max_i_pos(channel, ElectricCurrent::new::<ampere>(self.max_i_pos));
|
|
|
|
channels.set_max_i_neg(channel, ElectricCurrent::new::<ampere>(self.max_i_neg));
|
2020-09-25 03:35:15 +08:00
|
|
|
}
|
2020-09-25 02:59:04 +08:00
|
|
|
}
|