input_common: Sanitize motion data

This commit is contained in:
german77 2021-05-13 13:26:07 -05:00
parent bb7d4ec3d3
commit fd7c273fab

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;