Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: enable use of SIM_FLOAT_EXCEPT parameter

Browse Source
This commit is contained in:
Andrew Tridgell 2014-04-21 15:25:07 +10:00
parent e0db7b117f
commit 05bffb5915
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -379,6 +379,13 @@ void SITL_State::_fdm_input(void)
if (_sitl != NULL) {
_sitl->state = d.fg_pkt;
// prevent bad inputs from SIM from corrupting our state
double *v = &_sitl->state.latitude;
for (uint8_t i=0; i<17; i++) {
if (isinf(v[i]) || isnan(v[i])) {
v[i] = 0;
}
}
}
_update_count++;

10
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -23,6 +23,7 @@
#include <AP_Math.h>
#include "../AP_ADC/AP_ADC.h"
#include <SITL_State.h>
#include <fenv.h>
using namespace AVR_SITL;
@ -38,6 +39,9 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed)
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
airspeed_raw = airspeed_pressure + airspeed_offset;
if (airspeed_raw/4 > 0xFFFF) {
return 0xFFFF;
}
return airspeed_raw/4;
}
@ -120,6 +124,12 @@ 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);