From 261b9c3af5537c5084f03dca0e6987c270d78666 Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Wed, 13 Jan 2021 15:51:04 -0500
Subject: [PATCH] Filter temperature

r4.5 boards, when under extreme loads, can have spurious bogus
temperature readings.  Filtering the temperature before using is a
good idea anyway, as it will make the derating limiter more stable.
---
 fw/bldc_servo.cc | 15 ++++++++++-----
 fw/bldc_servo.h  |  2 ++
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
index 3154f0d0..3aa99334 100644
--- a/fw/bldc_servo.cc
+++ b/fw/bldc_servo.cc
@@ -898,18 +898,23 @@ class BldcServo::Impl {
           (temp2 - temp1) *
           static_cast<float>(status_.adc_fet_temp_raw - this_value) /
           static_cast<float>(next_value - this_value);
+      ISR_UpdateFilteredValue(status_.fet_temp_C, &status_.filt_fet_temp_C, 0.01f);
     }
   }
 
-  void ISR_UpdateFilteredBusV(float* filtered, float period_s) const MOTEUS_CCM_ATTRIBUTE {
+  static void ISR_UpdateFilteredValue(float input, float* filtered, float period_s) MOTEUS_CCM_ATTRIBUTE {
     if (std::isnan(*filtered)) {
-      *filtered = status_.bus_V;
+      *filtered = input;
     } else {
       const float alpha = 1.0f / (kRateHz * period_s);
-      *filtered = alpha * status_.bus_V + (1.0f - alpha) * *filtered;
+      *filtered = alpha * input + (1.0f - alpha) * *filtered;
     }
   }
 
+  void ISR_UpdateFilteredBusV(float* filtered, float period_s) const MOTEUS_CCM_ATTRIBUTE {
+    ISR_UpdateFilteredValue(status_.bus_V, filtered, period_s);
+  }
+
   // This is called from the ISR.
   void ISR_CalculateCurrentState(const SinCos& sin_cos) MOTEUS_CCM_ATTRIBUTE {
     status_.cur1_A = (status_.adc_cur1_raw - status_.adc_cur1_offset) * adc_scale_;
@@ -1179,7 +1184,7 @@ class BldcServo::Impl {
         status_.mode = kFault;
         status_.fault = errc::kUnderVoltage;
       }
-      if (status_.fet_temp_C > config_.fault_temperature) {
+      if (status_.filt_fet_temp_C > config_.fault_temperature) {
         status_.mode = kFault;
         status_.fault = errc::kOverTemperature;
       }
@@ -1388,7 +1393,7 @@ class BldcServo::Impl {
     };
 
     const float derate_fraction = (
-        status_.fet_temp_C - config_.derate_temperature) / (
+        status_.filt_fet_temp_C - config_.derate_temperature) / (
             config_.fault_temperature - config_.derate_temperature);
 
     const float derate_current_A =
diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h
index e85749b7..2da95430 100644
--- a/fw/bldc_servo.h
+++ b/fw/bldc_servo.h
@@ -317,6 +317,7 @@ class BldcServo {
     float filt_1ms_bus_V = std::numeric_limits<float>::quiet_NaN();
     uint16_t position = 0;
     float fet_temp_C = 0.0f;
+    float filt_fet_temp_C = std::numeric_limits<float>::quiet_NaN();
 
     float electrical_theta = 0.0f;
 
@@ -404,6 +405,7 @@ class BldcServo {
       a->Visit(MJ_NVP(filt_1ms_bus_V));
       a->Visit(MJ_NVP(position));
       a->Visit(MJ_NVP(fet_temp_C));
+      a->Visit(MJ_NVP(filt_fet_temp_C));
       a->Visit(MJ_NVP(electrical_theta));
 
       a->Visit(MJ_NVP(d_A));