diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 6259e7afbb..3820822653 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -498,23 +498,7 @@ void Replay::setup() logreader.set_save_chek_messages(true); } - if (generate_fpe) { - // SITL_State::_parse_command_line sets up an FPE handler. We - // can do better: - feenableexcept(FE_INVALID | FE_OVERFLOW); - signal(SIGFPE, _replay_sig_fpe); - } else { - // disable floating point exception generation: - int exceptions = FE_OVERFLOW | FE_DIVBYZERO; -#ifndef __i386__ - // i386 with gcc doesn't work with FE_INVALID - exceptions |= FE_INVALID; -#endif - if (feclearexcept(exceptions)) { - ::fprintf(stderr, "Failed to disable floating point exceptions: %s", strerror(errno)); - } - signal(SIGFPE, SIG_IGN); - } + set_signal_handlers(); hal.console->printf("Processing log %s\n", filename); @@ -595,6 +579,27 @@ void Replay::set_user_parameters(void) } } +void Replay::set_signal_handlers(void) +{ + if (generate_fpe) { + // SITL_State::_parse_command_line sets up an FPE handler. We + // can do better: + feenableexcept(FE_INVALID | FE_OVERFLOW); + signal(SIGFPE, _replay_sig_fpe); + } else { + // disable floating point exception generation: + int exceptions = FE_OVERFLOW | FE_DIVBYZERO; +#ifndef __i386__ + // i386 with gcc doesn't work with FE_INVALID + exceptions |= FE_INVALID; +#endif + if (feclearexcept(exceptions)) { + ::fprintf(stderr, "Failed to disable floating point exceptions: %s", strerror(errno)); + } + signal(SIGFPE, SIG_IGN); + } +} + /* write out EKF log messages */ diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index ca12354242..ef89c2adec 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -176,6 +176,7 @@ private: const char **parse_list_from_string(const char *str); bool parse_param_line(char *line, char **vname, float &value); void load_param_file(const char *filename); + void set_signal_handlers(void); }; enum {