|
|
|
@ -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;
|
|
|
|
|