From 6caf0d114d873e67f5be02eaf4a86db2ebb64244 Mon Sep 17 00:00:00 2001 From: Lukas Woodtli Date: Wed, 24 Oct 2018 22:38:38 +0200 Subject: [PATCH] Fix division by zero and cast of too big floats to int --- src/modules/simulator/accelsim/accelsim.cpp | 41 ++++++++++++++++----- src/modules/simulator/gyrosim/gyrosim.cpp | 34 ++++++++++++++--- 2 files changed, 60 insertions(+), 15 deletions(-) diff --git a/src/modules/simulator/accelsim/accelsim.cpp b/src/modules/simulator/accelsim/accelsim.cpp index 77fda93d0d..933b03689b 100644 --- a/src/modules/simulator/accelsim/accelsim.cpp +++ b/src/modules/simulator/accelsim/accelsim.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -81,6 +82,21 @@ 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 @@ -848,9 +864,13 @@ ACCELSIM::_measure() // whether it has had failures accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - accel_report.x_raw = (int16_t)(raw_accel_report.x / _accel_range_scale); - accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); - accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); + if (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_accel_report.x; accel_report.y = raw_accel_report.y; @@ -924,14 +944,17 @@ ACCELSIM::mag_measure() mag_report.device_id = 196616; mag_report.is_external = false; - mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale); - mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale); - mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale); + if (isZero(_mag_range_scale)) { + _mag_range_scale = FLT_EPSILON; + } - float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale); - float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale); - float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale); + 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); + mag_report.x_raw = xraw_f; + mag_report.y_raw = yraw_f; + mag_report.z_raw = zraw_f; /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp index 5c274f5e22..ae94700bb4 100644 --- a/src/modules/simulator/gyrosim/gyrosim.cpp +++ b/src/modules/simulator/gyrosim/gyrosim.cpp @@ -109,6 +109,20 @@ 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 @@ -945,9 +959,13 @@ GYROSIM::_measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale); - arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale); - arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale); + if (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.scaling = _accel_range_scale; @@ -970,9 +988,13 @@ GYROSIM::_measure() /* fake device ID */ arb.device_id = 1376264; - grb.x_raw = (int16_t)(mpu_report.gyro_x / _gyro_range_scale); - grb.y_raw = (int16_t)(mpu_report.gyro_y / _gyro_range_scale); - grb.z_raw = (int16_t)(mpu_report.gyro_z / _gyro_range_scale); + if (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.scaling = _gyro_range_scale;