switch pid+steinhart_hart parameters from f32 to f64
This commit is contained in:
parent
1ad821299b
commit
0f4442b124
@ -1,4 +1,5 @@
|
|||||||
use core::fmt;
|
use core::fmt;
|
||||||
|
use lexical_core::Float;
|
||||||
use stm32f4xx_hal::{
|
use stm32f4xx_hal::{
|
||||||
time::{MegaHertz, U32Ext},
|
time::{MegaHertz, U32Ext},
|
||||||
spi,
|
spi,
|
||||||
@ -146,18 +147,9 @@ impl PostFilter {
|
|||||||
];
|
];
|
||||||
|
|
||||||
pub fn closest(rate: f32) -> Option<Self> {
|
pub fn closest(rate: f32) -> Option<Self> {
|
||||||
/// (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;
|
let mut best: Option<(f32, Self)> = None;
|
||||||
for value in Self::VALID_VALUES {
|
for value in Self::VALID_VALUES {
|
||||||
let error = d(rate, value.output_rate().unwrap());
|
let error = (rate - value.output_rate().unwrap()).abs();
|
||||||
let better = best
|
let better = best
|
||||||
.map(|(best_error, _)| error < best_error)
|
.map(|(best_error, _)| error < best_error)
|
||||||
.unwrap_or(true);
|
.unwrap_or(true);
|
||||||
|
@ -123,12 +123,12 @@ pub enum Command {
|
|||||||
Pid {
|
Pid {
|
||||||
channel: usize,
|
channel: usize,
|
||||||
parameter: PidParameter,
|
parameter: PidParameter,
|
||||||
value: f32,
|
value: f64,
|
||||||
},
|
},
|
||||||
SteinhartHart {
|
SteinhartHart {
|
||||||
channel: usize,
|
channel: usize,
|
||||||
parameter: ShParameter,
|
parameter: ShParameter,
|
||||||
value: f32,
|
value: f64,
|
||||||
},
|
},
|
||||||
PostFilter {
|
PostFilter {
|
||||||
channel: usize,
|
channel: usize,
|
||||||
@ -158,12 +158,12 @@ fn unsigned(input: &[u8]) -> IResult<&[u8], Result<u16, Error>> {
|
|||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
fn float(input: &[u8]) -> IResult<&[u8], Result<f32, Error>> {
|
fn float(input: &[u8]) -> IResult<&[u8], Result<f64, Error>> {
|
||||||
let (input, sign) = opt(is_a("-"))(input)?;
|
let (input, sign) = opt(is_a("-"))(input)?;
|
||||||
let negative = sign.is_some();
|
let negative = sign.is_some();
|
||||||
let (input, digits) = take_while1(|c| is_digit(c) || c == '.' as u8)(input)?;
|
let (input, digits) = take_while1(|c| is_digit(c) || c == '.' as u8)(input)?;
|
||||||
let result = lexical::parse(digits)
|
let result = lexical::parse(digits)
|
||||||
.map(|result: f32| if negative { -result } else { result })
|
.map(|result: f64| if negative { -result } else { result })
|
||||||
.map_err(|e| e.into());
|
.map_err(|e| e.into());
|
||||||
Ok((input, result))
|
Ok((input, result))
|
||||||
}
|
}
|
||||||
@ -357,7 +357,8 @@ fn postfilter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
|
|||||||
let (input, rate) = float(input)?;
|
let (input, rate) = float(input)?;
|
||||||
let result = rate
|
let result = rate
|
||||||
.map(|rate| Command::PostFilter {
|
.map(|rate| Command::PostFilter {
|
||||||
channel, rate,
|
channel,
|
||||||
|
rate: rate as f32,
|
||||||
});
|
});
|
||||||
Ok((input, result))
|
Ok((input, result))
|
||||||
}
|
}
|
||||||
|
32
src/pid.rs
32
src/pid.rs
@ -1,20 +1,20 @@
|
|||||||
#[derive(Clone, Copy)]
|
#[derive(Clone, Copy)]
|
||||||
pub struct Parameters {
|
pub struct Parameters {
|
||||||
pub kp: f32,
|
pub kp: f64,
|
||||||
pub ki: f32,
|
pub ki: f64,
|
||||||
pub kd: f32,
|
pub kd: f64,
|
||||||
pub output_min: f32,
|
pub output_min: f64,
|
||||||
pub output_max: f32,
|
pub output_max: f64,
|
||||||
pub integral_min: f32,
|
pub integral_min: f64,
|
||||||
pub integral_max: f32
|
pub integral_max: f64
|
||||||
}
|
}
|
||||||
|
|
||||||
#[derive(Clone)]
|
#[derive(Clone)]
|
||||||
pub struct Controller {
|
pub struct Controller {
|
||||||
parameters: Parameters,
|
parameters: Parameters,
|
||||||
target: f32,
|
target: f64,
|
||||||
integral: f32,
|
integral: f64,
|
||||||
last_input: Option<f32>
|
last_input: Option<f64>
|
||||||
}
|
}
|
||||||
|
|
||||||
impl Controller {
|
impl Controller {
|
||||||
@ -27,7 +27,7 @@ impl Controller {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn update(&mut self, input: f32) -> f32 {
|
pub fn update(&mut self, input: f64) -> f64 {
|
||||||
let error = self.target - input;
|
let error = self.target - input;
|
||||||
|
|
||||||
let p = self.parameters.kp * error;
|
let p = self.parameters.kp * error;
|
||||||
@ -57,11 +57,11 @@ impl Controller {
|
|||||||
output
|
output
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn get_target(&self) -> f32 {
|
pub fn get_target(&self) -> f64 {
|
||||||
self.target
|
self.target
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn set_target(&mut self, target: f32) {
|
pub fn set_target(&mut self, target: f64) {
|
||||||
self.target = target;
|
self.target = target;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -96,9 +96,9 @@ mod test {
|
|||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn test_controller() {
|
fn test_controller() {
|
||||||
const DEFAULT: f32 = 0.0;
|
const DEFAULT: f64 = 0.0;
|
||||||
const TARGET: f32 = 1234.56;
|
const TARGET: f64 = 1234.56;
|
||||||
const ERROR: f32 = 0.01;
|
const ERROR: f64 = 0.01;
|
||||||
const DELAY: usize = 10;
|
const DELAY: usize = 10;
|
||||||
|
|
||||||
let mut pid = Controller::new(PARAMETERS.clone());
|
let mut pid = Controller::new(PARAMETERS.clone());
|
||||||
|
@ -3,14 +3,14 @@ use lexical_core::Float;
|
|||||||
/// Steinhart-Hart equation parameters
|
/// Steinhart-Hart equation parameters
|
||||||
#[derive(Clone, Debug)]
|
#[derive(Clone, Debug)]
|
||||||
pub struct Parameters {
|
pub struct Parameters {
|
||||||
pub a: f32,
|
pub a: f64,
|
||||||
pub b: f32,
|
pub b: f64,
|
||||||
pub c: f32,
|
pub c: f64,
|
||||||
/// Parallel resistance
|
/// Parallel resistance
|
||||||
///
|
///
|
||||||
/// Not truly part of the equation but required to calculate
|
/// Not truly part of the equation but required to calculate
|
||||||
/// resistance from voltage.
|
/// resistance from voltage.
|
||||||
pub parallel_r: f32,
|
pub parallel_r: f64,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl Parameters {
|
impl Parameters {
|
||||||
@ -19,7 +19,7 @@ impl Parameters {
|
|||||||
/// Result unit: Kelvin
|
/// Result unit: Kelvin
|
||||||
///
|
///
|
||||||
/// TODO: verify
|
/// TODO: verify
|
||||||
pub fn get_temperature(&self, voltage: f32) -> f32 {
|
pub fn get_temperature(&self, voltage: f64) -> f64 {
|
||||||
let r = self.parallel_r * voltage;
|
let r = self.parallel_r * voltage;
|
||||||
let ln_r = r.abs().ln();
|
let ln_r = r.abs().ln();
|
||||||
let inv_temp = self.a +
|
let inv_temp = self.a +
|
||||||
|
Loading…
Reference in New Issue
Block a user