From 0cb8d0c53d5f4b381b93c0beee45a6233b1fe40c Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Sat, 6 May 2017 20:43:33 +0800
Subject: [PATCH] pid: use Option instead of NaN

---
 firmware/src/main.rs |  2 +-
 firmware/src/pid.rs  | 18 +++++++-----------
 2 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index bf53ea2..e9fd0c8 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -1,4 +1,4 @@
-#![feature(used, const_fn, core_float)]
+#![feature(used, const_fn)]
 #![no_std]
 
 #[macro_use]
diff --git a/firmware/src/pid.rs b/firmware/src/pid.rs
index 2396824..10e15df 100644
--- a/firmware/src/pid.rs
+++ b/firmware/src/pid.rs
@@ -1,6 +1,3 @@
-use core::f32;
-use core::num::Float;
-
 #[derive(Clone, Copy)]
 pub struct Parameters {
     pub kp: f32,
@@ -16,7 +13,7 @@ pub struct Controller {
     parameters: Parameters,
     target: f32,
     integral: f32,
-    last_input: f32
+    last_input: Option<f32>
 }
 
 impl Controller {
@@ -24,7 +21,7 @@ impl Controller {
         Controller {
             parameters: parameters,
             target: 0.0,
-            last_input: f32::NAN,
+            last_input: None,
             integral: 0.0
         }
     }
@@ -43,12 +40,11 @@ impl Controller {
         }
         let i = self.parameters.ki * self.integral;
 
-        let d = if self.last_input.is_nan() {
-            0.0
-        } else {
-            self.parameters.kd * (self.last_input - input)
+        let d = match self.last_input {
+            None => 0.0,
+            Some(last_input) => self.parameters.kd * (last_input - input)
         };
-        self.last_input = input;
+        self.last_input = Some(input);
 
         let mut output = p + i + d;
         if output < self.parameters.output_min {
@@ -67,6 +63,6 @@ impl Controller {
     #[allow(dead_code)]
     pub fn reset(&mut self) {
         self.integral = 0.0;
-        self.last_input = f32::NAN;
+        self.last_input = None;
     }
 }