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

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);

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;