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.
mjbots · Jan 13, 2021 · 261b9c3 · 261b9c3
1 parent 4f50c7f
commit 261b9c3
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 5 deletions.
diff --git 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
@@ -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));