fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix division by zero and cast of to big floats to int

Browse Source
This commit is contained in:
Lukas Woodtli
2018-10-28 16:54:36 +01:00
committed by Lorenz Meier
parent 6caf0d114d
commit a7d297ed57
3 changed files with 37 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/accelsim/accelsim.cpp
+8 -23
View File
@@ -82,21 +82,6 @@ extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); }
using namespace DriverFramework;
namespace
{
/** Save way to check if float is zero */
inline bool isZero(const float &val)
{
return abs(val - 0.0f) < FLT_EPSILON;
}
inline int16_t constrainToInt(float value)
{
return (int16_t)math::constrain(value, (float)INT16_MIN, (float)INT16_MAX);
}
}
class ACCELSIM_mag;
class ACCELSIM : public VirtDevObj
@@ -864,13 +849,13 @@ ACCELSIM::_measure()
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
if (isZero(_accel_range_scale)) {
if (math::isZero(_accel_range_scale)) {
_accel_range_scale = FLT_EPSILON;
}
accel_report.x_raw = constrainToInt(raw_accel_report.x / _accel_range_scale);
accel_report.y_raw = constrainToInt(raw_accel_report.y / _accel_range_scale);
accel_report.z_raw = constrainToInt(raw_accel_report.z / _accel_range_scale);
accel_report.x_raw = math::constrainFloatToInt16(raw_accel_report.x / _accel_range_scale);
accel_report.y_raw = math::constrainFloatToInt16(raw_accel_report.y / _accel_range_scale);
accel_report.z_raw = math::constrainFloatToInt16(raw_accel_report.z / _accel_range_scale);
accel_report.x = raw_accel_report.x;
accel_report.y = raw_accel_report.y;
@@ -944,13 +929,13 @@ ACCELSIM::mag_measure()
mag_report.device_id = 196616;
mag_report.is_external = false;
if (isZero(_mag_range_scale)) {
if (math::isZero(_mag_range_scale)) {
_mag_range_scale = FLT_EPSILON;
}
float xraw_f = constrainToInt(raw_mag_report.x / _mag_range_scale);
float yraw_f = constrainToInt(raw_mag_report.y / _mag_range_scale);
float zraw_f = constrainToInt(raw_mag_report.z / _mag_range_scale);
float xraw_f = math::constrainFloatToInt16(raw_mag_report.x / _mag_range_scale);
float yraw_f = math::constrainFloatToInt16(raw_mag_report.y / _mag_range_scale);
float zraw_f = math::constrainFloatToInt16(raw_mag_report.z / _mag_range_scale);
mag_report.x_raw = xraw_f;
mag_report.y_raw = yraw_f;
src/modules/simulator/gyrosim/gyrosim.cpp
+8 -22
View File
@@ -109,20 +109,6 @@ using namespace DriverFramework;
#define EXTERNAL_BUS 0
#endif
namespace
{
/** Save way to check if float is zero */
inline bool isZero(const float &val)
{
return abs(val - 0.0f) < FLT_EPSILON;
}
inline int16_t constrainToInt(float value)
{
return (int16_t)math::constrain(value, (float)INT16_MIN, (float)INT16_MAX);
}
}
/*
the GYROSIM can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
@@ -959,13 +945,13 @@ GYROSIM::_measure()
/* NOTE: Axes have been swapped to match the board a few lines above. */
if (isZero(_accel_range_scale)) {
if (math::isZero(_accel_range_scale)) {
_accel_range_scale = FLT_EPSILON;
}
arb.x_raw = constrainToInt(mpu_report.accel_x / _accel_range_scale);
arb.y_raw = constrainToInt(mpu_report.accel_y / _accel_range_scale);
arb.z_raw = constrainToInt(mpu_report.accel_z / _accel_range_scale);
arb.x_raw = math::constrainFloatToInt16(mpu_report.accel_x / _accel_range_scale);
arb.y_raw = math::constrainFloatToInt16(mpu_report.accel_y / _accel_range_scale);
arb.z_raw = math::constrainFloatToInt16(mpu_report.accel_z / _accel_range_scale);
arb.scaling = _accel_range_scale;
@@ -988,13 +974,13 @@ GYROSIM::_measure()
/* fake device ID */
arb.device_id = 1376264;
if (isZero(_gyro_range_scale)) {
if (math::isZero(_gyro_range_scale)) {
_gyro_range_scale = FLT_EPSILON;
}
grb.x_raw = constrainToInt(mpu_report.gyro_x / _gyro_range_scale);
grb.y_raw = constrainToInt(mpu_report.gyro_y / _gyro_range_scale);
grb.z_raw = constrainToInt(mpu_report.gyro_z / _gyro_range_scale);
grb.x_raw = math::constrainFloatToInt16(mpu_report.gyro_x / _gyro_range_scale);
grb.y_raw = math::constrainFloatToInt16(mpu_report.gyro_y / _gyro_range_scale);
grb.z_raw = math::constrainFloatToInt16(mpu_report.gyro_z / _gyro_range_scale);
grb.scaling = _gyro_range_scale;