Merge pull request #323 from ut-issl/feature/add-assertion-nan

Add NaN assertion
ut-issl · May 29, 2024 · 03d653f · 03d653f
2 parents 751d963 + 8c7c308
commit 03d653f
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 1 deletion.
diff --git a/src/src_user/Library/SignalProcess/spike_filter.c b/src/src_user/Library/SignalProcess/spike_filter.c
@@ -66,6 +66,14 @@ C2A_MATH_ERROR SPIKE_FILTER_calc_output_uint32(SpikeFilter* filter, uint32_t* ou
 
 C2A_MATH_ERROR SPIKE_FILTER_calc_output_double(SpikeFilter* filter, double* output, const double input)
 {
+  // NAN確認
+  C2A_MATH_ERROR ret = C2A_MATH_check_nan_inf_double(input);
+  if (ret != C2A_MATH_ERROR_OK)
+  {
+    *output = filter->last_accept_val_;
+    return C2A_MATH_ERROR_NAN;
+  }
+
   C2A_MATH_ERROR judgement_result = C2A_MATH_ERROR_OK;
 
   double diff = input - filter->last_accept_val_;

diff --git a/src/src_user/Library/SignalProcess/z_filter.c b/src/src_user/Library/SignalProcess/z_filter.c
@@ -8,7 +8,7 @@
 
 #include <math.h>
 #include <src_user/Library/math_constants.h>
-
+#include <src_core/System/EventManager/event_logger.h>
 
 //!< bilinear transformation
 static C2A_MATH_ERROR Z_FILTER_bilinear_trans_(ZFilter* filter,
@@ -157,6 +157,15 @@ float Z_FILTER_calc_output(ZFilter* filter, const float input)
 
 double Z_FILTER_calc_output_double(ZFilter* filter, const double input)
 {
+  // NAN確認
+  C2A_MATH_ERROR ret = C2A_MATH_check_nan_inf_double(input);
+  if (ret != C2A_MATH_ERROR_OK)
+  {
+    // TODO: IDは現状テキトウな値なので、修正する(今はアプリIDと被らない大きな値にしている。)
+    EL_record_event(EL_GROUP_CALCULATION_ERROR, 201, EL_ERROR_LEVEL_LOW, (uint32_t)ret);
+    return filter->output_previous[0];
+  }
+
   double output_d = input;
 
   switch (filter->order)

diff --git a/src/src_user/Library/pid_control.c b/src/src_user/Library/pid_control.c
@@ -9,6 +9,7 @@
 #include <math.h>
 
 #include <src_core/System/TimeManager/time_manager.h>
+#include <src_core/System/EventManager/event_logger.h>
 
 #include "math_constants.h"
 
@@ -76,6 +77,17 @@ void PID_CONTROL_calc_output(PidControl* pid_control, const float error)
                                  pid_control->gains.d_gain * pid_control->d_error;
   pid_control->pre_error = pid_control->error;
 
+  // NAN確認
+  C2A_MATH_ERROR ret = C2A_MATH_check_nan_inf(pid_control->control_output);
+  if (ret != C2A_MATH_ERROR_OK)
+  {
+    pid_control->control_output = 0.0f;
+    PID_CONTROL_reset_integral_error(pid_control);
+    pid_control->pre_error = 0.0f;
+    // TODO: IDは現状テキトウな値なので、修正する(今はアプリIDと被らない大きな値にしている。)
+    EL_record_event(EL_GROUP_CALCULATION_ERROR, 200, EL_ERROR_LEVEL_LOW, (uint32_t)ret);
+  }
+
   return;
 }