diff --git a/src/src_user/Library/SignalProcess/spike_filter.c b/src/src_user/Library/SignalProcess/spike_filter.c index 0b1c08ea..ca6ac16b 100644 --- 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 index 153c6b1f..be876577 100644 --- a/src/src_user/Library/SignalProcess/z_filter.c +++ b/src/src_user/Library/SignalProcess/z_filter.c @@ -8,7 +8,7 @@ #include #include - +#include //!< 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 index 57747ad9..8269fe96 100644 --- a/src/src_user/Library/pid_control.c +++ b/src/src_user/Library/pid_control.c @@ -9,6 +9,7 @@ #include #include +#include #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; }