diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index cf3e00f3f0e131ca7f5a2e86ba397ac3d9a6400c..4610a01e402eadc2783d9a35f088d74d36fec83f 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -40,6 +40,7 @@
#pragma once
#include <platforms/px4_defines.h>
+#include <float.h>
//this should be defined in stdint.h, but seems to be missing in the ARM toolchain (5.2.0)
#ifndef UINT64_C
@@ -72,6 +73,14 @@ constexpr const _Tp &constrain(const _Tp &val, const _Tp &min_val, const _Tp &ma
return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
}
+/** Constrain float values to valid values for int16_t.
+ * Invalid values are just clipped to be in the range for int16_t. */
+inline int16_t constrainFloatToInt16(float value)
+{
+ return (int16_t)math::constrain(value, (float)INT16_MIN, (float)INT16_MAX);
+}
+
+
template<typename _Tp>
inline constexpr bool isInRange(const _Tp &val, const _Tp &min_val, const _Tp &max_val)
{
@@ -90,4 +99,16 @@ constexpr T degrees(const T radians)
return radians * (static_cast<T>(180) / static_cast<T>(M_PI));
}
+/** Save way to check if float is zero */
+inline bool isZero(const float val)
+{
+ return fabsf(val - 0.0f) < FLT_EPSILON;
+}
+
+/** Save way to check if double is zero */
+inline bool isZero(const double val)
+{
+ return fabs(val - 0.0) < DBL_EPSILON;
+}
+
}
diff --git a/src/modules/simulator/accelsim/accelsim.cpp b/src/modules/simulator/accelsim/accelsim.cpp
index 933b03689bcc00b83547a0376fd57b893f804809..5bf7d37c8aa1d12af9c5bc8e11df0bba30b6b71d 100644
--- a/src/modules/simulator/accelsim/accelsim.cpp
+++ b/src/modules/simulator/accelsim/accelsim.cpp
@@ -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;
diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp
index ae94700bb4edb314f4c33005ce6e489f886ac8ea..af1f6d33a1d6ad059e5aa1bdd0375f3b0e509ac2 100644
--- a/src/modules/simulator/gyrosim/gyrosim.cpp
+++ b/src/modules/simulator/gyrosim/gyrosim.cpp
@@ -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;