switch pid+steinhart_hart parameters from f32 to f64

softspi
Astro 2020-03-18 21:56:52 +01:00
parent 1ad821299b
commit 0f4442b124
4 changed files with 29 additions and 36 deletions

View File

@ -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);

View File

@ -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))
} }

View File

@ -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());

View File

@ -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 +