diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorImpl_Common.cpp')
| -rw-r--r-- | LibOVR/Src/OVR_SensorImpl_Common.cpp | 245 |
1 files changed, 0 insertions, 245 deletions
diff --git a/LibOVR/Src/OVR_SensorImpl_Common.cpp b/LibOVR/Src/OVR_SensorImpl_Common.cpp deleted file mode 100644 index 99febe8..0000000 --- a/LibOVR/Src/OVR_SensorImpl_Common.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/************************************************************************************ - -Filename : OVR_SensorImpl_Common.cpp -Content : Source common to SensorImpl and Sensor2Impl. -Created : January 21, 2014 -Authors : Lee Cooper - -Copyright : Copyright 2014 Oculus VR, Inc. All Rights reserved. - -Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); -you may not use the Oculus VR Rift SDK except in compliance with the License, -which is provided at the time of installation or download, or which -otherwise accompanies this software in either electronic or hard copy form. - -You may obtain a copy of the License at - -http://www.oculusvr.com/licenses/LICENSE-3.1 - -Unless required by applicable law or agreed to in writing, the Oculus VR SDK -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. - -*************************************************************************************/ - -#include "OVR_SensorImpl_Common.h" -#include "Kernel/OVR_Alg.h" - -namespace OVR -{ - -void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z) -{ - // Sign extending trick - // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend - struct {SInt32 x:21;} s; - - *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3); - *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) | - ((buffer[5] & 0xC0) >> 6); - *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1); -} - -void PackSensor(UByte* buffer, SInt32 x, SInt32 y, SInt32 z) -{ - // Pack 3 32 bit integers into 8 bytes - buffer[0] = UByte(x >> 13); - buffer[1] = UByte(x >> 5); - buffer[2] = UByte((x << 3) | ((y >> 18) & 0x07)); - buffer[3] = UByte(y >> 10); - buffer[4] = UByte(y >> 2); - buffer[5] = UByte((y << 6) | ((z >> 15) & 0x3F)); - buffer[6] = UByte(z >> 7); - buffer[7] = UByte(z << 1); -} - -UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count, - float val, float factor, const char* label) -{ - UInt16 threshold = (UInt16)(val * factor); - - for (unsigned i = 0; i<count; i++) - { - if (ramp[i] >= threshold) - return ramp[i]; - } - OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f", - label, float(ramp[count-1]) / factor)); - OVR_UNUSED2(factor, label); - return ramp[count-1]; -} - -SensorRangeImpl::SensorRangeImpl(const SensorRange& r, UInt16 commandId) -{ - SetSensorRange(r, commandId); -} - -void SensorRangeImpl::SetSensorRange(const SensorRange& r, UInt16 commandId) -{ - CommandId = commandId; - AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]), - r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration"); - GyroScale = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]), - r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate"); - MagScale = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]), - r.MaxMagneticField, 1000.0f, "MaxMagneticField"); - Pack(); -} - -void SensorRangeImpl::GetSensorRange(SensorRange* r) -{ - r->MaxAcceleration = AccelScale * 9.81f; - r->MaxRotationRate = DegreeToRad((float)GyroScale); - r->MaxMagneticField= MagScale * 0.001f; -} - -SensorRange SensorRangeImpl::GetMaxSensorRange() -{ - return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f, - GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] * - Math<float>::DegreeToRadFactor, - MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f); -} - -void SensorRangeImpl::Pack() -{ - Buffer[0] = 4; - Buffer[1] = UByte(CommandId & 0xFF); - Buffer[2] = UByte(CommandId >> 8); - Buffer[3] = UByte(AccelScale); - Buffer[4] = UByte(GyroScale & 0xFF); - Buffer[5] = UByte(GyroScale >> 8); - Buffer[6] = UByte(MagScale & 0xFF); - Buffer[7] = UByte(MagScale >> 8); -} - -void SensorRangeImpl::Unpack() -{ - CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); - AccelScale= Buffer[3]; - GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8); - MagScale = Buffer[6] | (UInt16(Buffer[7]) << 8); -} - -SensorConfigImpl::SensorConfigImpl() - : CommandId(0), Flags(0), PacketInterval(0), SampleRate(0) -{ - memset(Buffer, 0, PacketSize); - Buffer[0] = 2; -} - -void SensorConfigImpl::SetSensorCoordinates(bool sensorCoordinates) -{ - Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); -} - -bool SensorConfigImpl::IsUsingSensorCoordinates() const -{ - return (Flags & Flag_SensorCoordinates) != 0; -} - -void SensorConfigImpl::Pack() -{ - Buffer[0] = 2; - Buffer[1] = UByte(CommandId & 0xFF); - Buffer[2] = UByte(CommandId >> 8); - Buffer[3] = Flags; - Buffer[4] = UByte(PacketInterval); - Buffer[5] = UByte(SampleRate & 0xFF); - Buffer[6] = UByte(SampleRate >> 8); -} - -void SensorConfigImpl::Unpack() -{ - CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); - Flags = Buffer[3]; - PacketInterval = Buffer[4]; - SampleRate = Buffer[5] | (UInt16(Buffer[6]) << 8); -} - -SensorFactoryCalibrationImpl::SensorFactoryCalibrationImpl() - : AccelOffset(), GyroOffset(), AccelMatrix(), GyroMatrix(), Temperature(0) -{ - memset(Buffer, 0, PacketSize); - Buffer[0] = 3; -} - -void SensorFactoryCalibrationImpl::Pack() -{ - SInt32 x, y, z; - - Buffer[0] = 3; - - x = SInt32(AccelOffset.x * 1e4f); - y = SInt32(AccelOffset.y * 1e4f); - z = SInt32(AccelOffset.z * 1e4f); - PackSensor(Buffer + 3, x, y, z); - - x = SInt32(GyroOffset.x * 1e4f); - y = SInt32(GyroOffset.y * 1e4f); - z = SInt32(GyroOffset.z * 1e4f); - PackSensor(Buffer + 11, x, y, z); - - // ignore the scale matrices for now -} - -void SensorFactoryCalibrationImpl::Unpack() -{ - static const float sensorMax = (1 << 20) - 1; - SInt32 x, y, z; - - UnpackSensor(Buffer + 3, &x, &y, &z); - AccelOffset.y = (float) y * 1e-4f; - AccelOffset.z = (float) z * 1e-4f; - AccelOffset.x = (float) x * 1e-4f; - - UnpackSensor(Buffer + 11, &x, &y, &z); - GyroOffset.x = (float) x * 1e-4f; - GyroOffset.y = (float) y * 1e-4f; - GyroOffset.z = (float) z * 1e-4f; - - for (int i = 0; i < 3; i++) - { - UnpackSensor(Buffer + 19 + 8 * i, &x, &y, &z); - AccelMatrix.M[i][0] = (float) x / sensorMax; - AccelMatrix.M[i][1] = (float) y / sensorMax; - AccelMatrix.M[i][2] = (float) z / sensorMax; - AccelMatrix.M[i][i] += 1.0f; - } - - for (int i = 0; i < 3; i++) - { - UnpackSensor(Buffer + 43 + 8 * i, &x, &y, &z); - GyroMatrix.M[i][0] = (float) x / sensorMax; - GyroMatrix.M[i][1] = (float) y / sensorMax; - GyroMatrix.M[i][2] = (float) z / sensorMax; - GyroMatrix.M[i][i] += 1.0f; - } - - Temperature = (float) Alg::DecodeSInt16(Buffer + 67) / 100.0f; -} - -SensorKeepAliveImpl::SensorKeepAliveImpl(UInt16 interval, UInt16 commandId) - : CommandId(commandId), KeepAliveIntervalMs(interval) -{ - Pack(); -} - -void SensorKeepAliveImpl::Pack() -{ - Buffer[0] = 8; - Buffer[1] = UByte(CommandId & 0xFF); - Buffer[2] = UByte(CommandId >> 8); - Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF); - Buffer[4] = UByte(KeepAliveIntervalMs >> 8); -} - -void SensorKeepAliveImpl::Unpack() -{ - CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8); - KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8); -} - -} // namespace OVR |