From 6caf0d114d873e67f5be02eaf4a86db2ebb64244 Mon Sep 17 00:00:00 2001
From: Lukas Woodtli <woodtli.lukas@gmail.com>
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 <stdlib.h>
#include <string.h>
#include <math.h>
+#include <float.h>
#include <unistd.h>
#include <px4_getopt.h>
#include <errno.h>
@@ -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;
--
GitLab