Avoid slopes in deadband calculation

roboticslab-uc3m · Jun 8, 2024 · e6d00b3 · e6d00b3
1 parent ac8c342
commit e6d00b3
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 14 deletions.
diff --git a/libraries/YarpPlugins/SpaceNavigator/DeviceDriverImpl.cpp b/libraries/YarpPlugins/SpaceNavigator/DeviceDriverImpl.cpp
@@ -21,6 +21,12 @@ bool SpaceNavigator::open(yarp::os::Searchable & config)
 {
     deadband = config.check("deadband", yarp::os::Value(DEFAULT_DEADBAND), "deadband [0,1]").asFloat64();
 
+    if (deadband < 0.0 || deadband > 1.0)
+    {
+        yCError(SPNAV) << "Invalid deadband value (must be in [0,1]):" << deadband;
+        return false;
+    }
+
     if (spnav_open() == -1)
     {
         yCError(SPNAV) << "Failed to connect to the space navigator daemon";

diff --git a/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp b/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp
@@ -2,7 +2,9 @@
 
 #include "SpaceNavigator.hpp"
 
-#include <algorithm> // std::clamp
+#include <cmath> // std::abs
+
+#include <algorithm> // std::clamp, std::copysign
 
 using namespace roboticslab;
 
@@ -19,22 +21,13 @@ double SpaceNavigator::enforceRange(double in)
 
 double SpaceNavigator::enforceDeadband(double in)
 {
-    double out;
-
-    if (in > deadband)
+    if (std::abs(in) <= deadband)
     {
-        out = in;
+        return 0.0;
     }
     else
     {
-        if (in < -deadband)
-        {
-            out = in;
-        }
-        else
-        {
-            out = 0.0;
-        }
+        const double slope = RANGE / (RANGE - deadband);
+        return slope * std::copysign(std::abs(in) - deadband, in);
     }
-    return out;
 }