From fd7c273fabc3d8aa3ff11fe90a92885bfcd6f807 Mon Sep 17 00:00:00 2001
From: german77 <juangerman-13@hotmail.com>
Date: Thu, 13 May 2021 13:26:07 -0500
Subject: input_common: Sanitize motion data

---
 src/input_common/udp/client.cpp | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

(limited to 'src/input_common/udp/client.cpp')

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;
-- 
cgit v1.2.3