Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: fixed handling of SIM_FLOAT_EXCEPT in main thread

Browse Source
This commit is contained in:
Andrew Tridgell 2014-11-16 11:04:55 +11:00
parent d9d238cc3c
commit 22237f2530
6 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp
View File

@ -23,9 +23,9 @@
using namespace AVR_SITL;
static SITLScheduler sitlScheduler;
static SITLEEPROMStorage sitlEEPROMStorage;
static SITL_State sitlState;
static SITLScheduler sitlScheduler(&sitlState);
static SITLRCInput sitlRCInput(&sitlState);
static SITLRCOutput sitlRCOutput(&sitlState);
static SITLAnalogIn sitlAnalogIn(&sitlState);

2
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -79,7 +79,7 @@ bool SITL_State::new_rc_input;
// catch floating point exceptions
void SITL_State::_sig_fpe(int signum)
{
fprintf(stderr, "ERROR: Floating point exception\n");
fprintf(stderr, "ERROR: Floating point exception - aborting\n");
abort();
}

1
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -24,6 +24,7 @@
class HAL_AVR_SITL;
class AVR_SITL::SITL_State {
friend class AVR_SITL::SITLScheduler;
public:
void init(int argc, char * const argv[]);

9
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -7,6 +7,7 @@
#include "Scheduler.h"
#include <sys/time.h>
#include <unistd.h>
#include <fenv.h>
#ifdef __CYGWIN__
#include <stdio.h>
@ -43,7 +44,8 @@ double SITLScheduler::_cyg_freq = 0;
long SITLScheduler::_cyg_start = 0;
#endif
SITLScheduler::SITLScheduler()
SITLScheduler::SITLScheduler(SITL_State *sitlState) :
_sitlState(sitlState)
{}
void SITLScheduler::init(void *unused)
@ -207,6 +209,11 @@ void SITLScheduler::system_initialized() {
panic(
PSTR("PANIC: scheduler system initialized called more than once"));
}
if (_sitlState->_sitl->float_exception) {
feenableexcept(FE_INVALID | FE_OVERFLOW | FE_DIVBYZERO);
} else {
feclearexcept(FE_INVALID | FE_OVERFLOW | FE_DIVBYZERO);
}
_initialized = true;
}

3
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -12,7 +12,7 @@
/* Scheduler implementation: */
class AVR_SITL::SITLScheduler : public AP_HAL::Scheduler {
public:
SITLScheduler();
SITLScheduler(SITL_State *sitlState);
/* AP_HAL::Scheduler methods */
void init(void *unused);
@ -49,6 +49,7 @@ public:
static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
private:
SITL_State *_sitlState;
uint8_t _nested_atomic_ctr;
AP_HAL::Proc _delay_cb;
uint16_t _min_delay_cb_ms;

6
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -128,12 +128,6 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
return;
}
if (_sitl->float_exception) {
feenableexcept(FE_INVALID | FE_OVERFLOW);
} else {
feclearexcept(FE_INVALID | FE_OVERFLOW);
}
SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate,
&p, &q, &r);