pid: use Option instead of NaN

This commit is contained in:
Sebastien Bourdeauducq 2017-05-06 20:43:33 +08:00
parent a39c95e276
commit 0cb8d0c53d
2 changed files with 8 additions and 12 deletions

View File

@ -1,4 +1,4 @@
#![feature(used, const_fn, core_float)] #![feature(used, const_fn)]
#![no_std] #![no_std]
#[macro_use] #[macro_use]

View File

@ -1,6 +1,3 @@
use core::f32;
use core::num::Float;
#[derive(Clone, Copy)] #[derive(Clone, Copy)]
pub struct Parameters { pub struct Parameters {
pub kp: f32, pub kp: f32,
@ -16,7 +13,7 @@ pub struct Controller {
parameters: Parameters, parameters: Parameters,
target: f32, target: f32,
integral: f32, integral: f32,
last_input: f32 last_input: Option<f32>
} }
impl Controller { impl Controller {
@ -24,7 +21,7 @@ impl Controller {
Controller { Controller {
parameters: parameters, parameters: parameters,
target: 0.0, target: 0.0,
last_input: f32::NAN, last_input: None,
integral: 0.0 integral: 0.0
} }
} }
@ -43,12 +40,11 @@ impl Controller {
} }
let i = self.parameters.ki * self.integral; let i = self.parameters.ki * self.integral;
let d = if self.last_input.is_nan() { let d = match self.last_input {
0.0 None => 0.0,
} else { Some(last_input) => self.parameters.kd * (last_input - input)
self.parameters.kd * (self.last_input - input)
}; };
self.last_input = input; self.last_input = Some(input);
let mut output = p + i + d; let mut output = p + i + d;
if output < self.parameters.output_min { if output < self.parameters.output_min {
@ -67,6 +63,6 @@ impl Controller {
#[allow(dead_code)] #[allow(dead_code)]
pub fn reset(&mut self) { pub fn reset(&mut self) {
self.integral = 0.0; self.integral = 0.0;
self.last_input = f32::NAN; self.last_input = None;
} }
} }