Merge pull request #6310 from german77/nanMotion

input_common: Sanitize motion data
This commit is contained in:
bunnei 2021-05-19 15:47:48 -07:00 committed by GitHub
commit 41b1f8d616
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23

View file

@ -86,6 +86,7 @@ private:
case Type::PadData: { case Type::PadData: {
Response::PadData pad_data; Response::PadData pad_data;
std::memcpy(&pad_data, &receive_buffer[sizeof(Header)], sizeof(Response::PadData)); std::memcpy(&pad_data, &receive_buffer[sizeof(Header)], sizeof(Response::PadData));
SanitizeMotion(pad_data);
callback.pad_data(std::move(pad_data)); callback.pad_data(std::move(pad_data));
break; break;
} }
@ -114,6 +115,28 @@ private:
StartSend(timer.expiry()); 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; SocketCallback callback;
boost::asio::io_service io_service; boost::asio::io_service io_service;
boost::asio::basic_waitable_timer<clock> timer; boost::asio::basic_waitable_timer<clock> timer;