Merge pull request #6310 from german77/nanMotion

input_common: Sanitize motion data

1 files changed, 23 insertions, 0 deletions

diff --git a/src/input_common/udp/client.cpp b/src/input_common/udp/client.cpp
index 8a38a380d..bc1dfab3d 100644
--- a/src/input_common/udp/client.cpp
+++ b/src/input_common/udp/client.cpp
@@ -86,6 +86,7 @@ private:
             case Type::PadData: {
                 Response::PadData pad_data;
                 std::memcpy(&pad_data, &receive_buffer[sizeof(Header)], sizeof(Response::PadData));
+                SanitizeMotion(pad_data);
                 callback.pad_data(std::move(pad_data));
                 break;
             }
@@ -114,6 +115,28 @@ private:
         StartSend(timer.expiry());
     }
 
+    void SanitizeMotion(Response::PadData& data) {
+        // Zero out any non number value
+        if (!std::isnormal(data.gyro.pitch)) {
+            data.gyro.pitch = 0;
+        }
+        if (!std::isnormal(data.gyro.roll)) {
+            data.gyro.roll = 0;
+        }
+        if (!std::isnormal(data.gyro.yaw)) {
+            data.gyro.yaw = 0;
+        }
+        if (!std::isnormal(data.accel.x)) {
+            data.accel.x = 0;
+        }
+        if (!std::isnormal(data.accel.y)) {
+            data.accel.y = 0;
+        }
+        if (!std::isnormal(data.accel.z)) {
+            data.accel.z = 0;
+        }
+    }
+
     SocketCallback callback;
     boost::asio::io_service io_service;
     boost::asio::basic_waitable_timer<clock> timer;