diff --git a/libraries/YarpPlugins/SpaceNavigator/DeviceDriverImpl.cpp b/libraries/YarpPlugins/SpaceNavigator/DeviceDriverImpl.cpp index e3f3bdf2c..6f68b6c3b 100644 --- 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 index 15bb5d024..e09c1fc66 100644 --- a/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp +++ b/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp @@ -2,7 +2,9 @@ #include "SpaceNavigator.hpp" -#include // std::clamp +#include // std::abs + +#include // 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; }