/************************************************************************************
Filename : OVR_Sensor2Impl.cpp
Content : DK2 sensor device specific implementation.
Created : January 21, 2013
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_Sensor2Impl.h"
#include "OVR_SensorImpl_Common.h"
#include "OVR_Sensor2ImplUtil.h"
#include "Kernel/OVR_Alg.h"
//extern FILE *SF_LOG_fp;
namespace OVR {
//-------------------------------------------------------------------------------------
// ***** Oculus Sensor2-specific packet data structures
enum {
Sensor2_VendorId = Oculus_VendorId,
Sensor2_ProductId = 0x0021,
Sensor2_BootLoader = 0x1001,
Sensor2_DefaultReportRate = 1000, // Hz
};
// Messages we care for
enum Tracker2MessageType
{
Tracker2Message_None = 0,
Tracker2Message_Sensors = 11,
Tracker2Message_Unknown = 0x100,
Tracker2Message_SizeError = 0x101,
};
struct Tracker2Sensors
{
UInt16 LastCommandID;
UByte NumSamples;
UInt16 RunningSampleCount; // Named 'SampleCount' in the firmware docs.
SInt16 Temperature;
UInt32 SampleTimestamp;
TrackerSample Samples[2];
SInt16 MagX, MagY, MagZ;
UInt16 FrameCount;
UInt32 FrameTimestamp;
UByte FrameID;
UByte CameraPattern;
UInt16 CameraFrameCount; // Named 'CameraCount' in the firmware docs.
UInt32 CameraTimestamp;
Tracker2MessageType Decode(const UByte* buffer, int size)
{
if (size < 64)
return Tracker2Message_SizeError;
LastCommandID = DecodeUInt16(buffer + 1);
NumSamples = buffer[3];
RunningSampleCount = DecodeUInt16(buffer + 4);
Temperature = DecodeSInt16(buffer + 6);
SampleTimestamp = DecodeUInt32(buffer + 8);
// Only unpack as many samples as there actually are.
UByte iterationCount = (NumSamples > 1) ? 2 : NumSamples;
for (UByte i = 0; i < iterationCount; i++)
{
UnpackSensor(buffer + 12 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
UnpackSensor(buffer + 20 + 16 * i, &Samples[i].GyroX, &Samples[i].GyroY, &Samples[i].GyroZ);
}
MagX = DecodeSInt16(buffer + 44);
MagY = DecodeSInt16(buffer + 46);
MagZ = DecodeSInt16(buffer + 48);
FrameCount = DecodeUInt16(buffer + 50);
FrameTimestamp = DecodeUInt32(buffer + 52);
FrameID = buffer[56];
CameraPattern = buffer[57];
CameraFrameCount = DecodeUInt16(buffer + 58);
CameraTimestamp = DecodeUInt32(buffer + 60);
return Tracker2Message_Sensors;
}
};
struct Tracker2Message
{
Tracker2MessageType Type;
Tracker2Sensors Sensors;
};
// Sensor reports data in the following coordinate system:
// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
// Gyro: 10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.
// We need to convert it to the following RHS coordinate system:
// X right, Y Up, Z Back (out of screen)
//
Vector3f AccelFromBodyFrameUpdate(const Tracker2Sensors& update, UByte sampleNumber)
{
const TrackerSample& sample = update.Samples[sampleNumber];
float ax = (float)sample.AccelX;
float ay = (float)sample.AccelY;
float az = (float)sample.AccelZ;
return Vector3f(ax, ay, az) * 0.0001f;
}
Vector3f MagFromBodyFrameUpdate(const Tracker2Sensors& update)
{
return Vector3f( (float)update.MagX, (float)update.MagY, (float)update.MagZ) * 0.0001f;
}
Vector3f EulerFromBodyFrameUpdate(const Tracker2Sensors& update, UByte sampleNumber)
{
const TrackerSample& sample = update.Samples[sampleNumber];
float gx = (float)sample.GyroX;
float gy = (float)sample.GyroY;
float gz = (float)sample.GyroZ;
return Vector3f(gx, gy, gz) * 0.0001f;
}
bool Sensor2DeviceImpl::decodeTracker2Message(Tracker2Message* message, UByte* buffer, int size)
{
memset(message, 0, sizeof(Tracker2Message));
if (size < 4)
{
message->Type = Tracker2Message_SizeError;
return false;
}
switch (buffer[0])
{
case Tracker2Message_Sensors:
message->Type = message->Sensors.Decode(buffer, size);
break;
default:
message->Type = Tracker2Message_Unknown;
break;
}
return (message->Type < Tracker2Message_Unknown) && (message->Type != Tracker2Message_None);
}
//-------------------------------------------------------------------------------------
// ***** Sensor2Device
Sensor2DeviceImpl::Sensor2DeviceImpl(SensorDeviceCreateDesc* createDesc)
: SensorDeviceImpl(createDesc),
LastNumSamples(0),
LastRunningSampleCount(0),
FullCameraFrameCount(0),
LastCameraTime("C"),
LastFrameTime("F"),
LastSensorTime("S"),
LastFrameTimestamp(0)
{
// 15 samples ok in min-window for DK2 since it uses microsecond clock.
TimeFilter = SensorTimeFilter(SensorTimeFilter::Settings(15));
pCalibration = new SensorCalibration(this);
}
Sensor2DeviceImpl::~Sensor2DeviceImpl()
{
delete pCalibration;
}
void Sensor2DeviceImpl::openDevice()
{
// Read the currently configured range from sensor.
SensorRangeImpl sr(SensorRange(), 0);
if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
{
sr.Unpack();
sr.GetSensorRange(&CurrentRange);
}
// Read the currently configured calibration from sensor.
SensorFactoryCalibrationImpl sc;
if (GetInternalDevice()->GetFeatureReport(sc.Buffer, SensorFactoryCalibrationImpl::PacketSize))
{
sc.Unpack();
AccelCalibrationOffset = sc.AccelOffset;
GyroCalibrationOffset = sc.GyroOffset;
AccelCalibrationMatrix = sc.AccelMatrix;
GyroCalibrationMatrix = sc.GyroMatrix;
CalibrationTemperature = sc.Temperature;
}
// If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
SensorDisplayInfoImpl displayInfo;
if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
{
displayInfo.Unpack();
Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
Coord_HMD : Coord_Sensor;
}
Coordinates = Coord_HMD; // TODO temporary to force it behave
// Read/Apply sensor config.
setCoordinateFrame(Coordinates);
setReportRate(Sensor2_DefaultReportRate);
setOnboardCalibrationEnabled(false);
// Must send DK2 keep-alive. Set Keep-alive at 10 seconds.
KeepAliveMuxReport keepAlive;
keepAlive.CommandId = 0;
keepAlive.INReport = 11;
keepAlive.Interval = 10 * 1000;
// Device creation is done from background thread so we don't need to add this to the command queue.
KeepAliveMuxImpl keepAliveImpl(keepAlive);
GetInternalDevice()->SetFeatureReport(keepAliveImpl.Buffer, KeepAliveMuxImpl::PacketSize);
// Read the temperature data from the device
pCalibration->Initialize();
}
bool Sensor2DeviceImpl::SetTrackingReport(const TrackingReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setTrackingReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setTrackingReport(const TrackingReport& data)
{
TrackingImpl ci(data);
return GetInternalDevice()->SetFeatureReport(ci.Buffer, TrackingImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetTrackingReport(TrackingReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getTrackingReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getTrackingReport(TrackingReport* data)
{
TrackingImpl ci;
if (GetInternalDevice()->GetFeatureReport(ci.Buffer, TrackingImpl::PacketSize))
{
ci.Unpack();
*data = ci.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetDisplayReport(const DisplayReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setDisplayReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setDisplayReport(const DisplayReport& data)
{
DisplayImpl di(data);
return GetInternalDevice()->SetFeatureReport(di.Buffer, DisplayImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetDisplayReport(DisplayReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getDisplayReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getDisplayReport(DisplayReport* data)
{
DisplayImpl di;
if (GetInternalDevice()->GetFeatureReport(di.Buffer, DisplayImpl::PacketSize))
{
di.Unpack();
*data = di.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetMagCalibrationReport(const MagCalibrationReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setMagCalibrationReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setMagCalibrationReport(const MagCalibrationReport& data)
{
MagCalibrationImpl mci(data);
return GetInternalDevice()->SetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetMagCalibrationReport(MagCalibrationReport* data)
{
// direct call if we are already on the device manager thread
if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
{
return getMagCalibrationReport(data);
}
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getMagCalibrationReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getMagCalibrationReport(MagCalibrationReport* data)
{
MagCalibrationImpl mci;
if (GetInternalDevice()->GetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize))
{
mci.Unpack();
*data = mci.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetPositionCalibrationReport(const PositionCalibrationReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setPositionCalibrationReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setPositionCalibrationReport(const PositionCalibrationReport& data)
{
UByte version = GetDeviceInterfaceVersion();
if (version < 5)
{
PositionCalibrationImpl_Pre5 pci(data);
return GetInternalDevice()->SetFeatureReport(pci.Buffer, PositionCalibrationImpl_Pre5::PacketSize);
}
PositionCalibrationImpl pci(data);
return GetInternalDevice()->SetFeatureReport(pci.Buffer, PositionCalibrationImpl::PacketSize);
}
bool Sensor2DeviceImpl::getPositionCalibrationReport(PositionCalibrationReport* data)
{
UByte version = GetDeviceInterfaceVersion();
if (version < 5)
{
PositionCalibrationImpl_Pre5 pci;
if (GetInternalDevice()->GetFeatureReport(pci.Buffer, PositionCalibrationImpl_Pre5::PacketSize))
{
pci.Unpack();
*data = pci.Settings;
return true;
}
return false;
}
PositionCalibrationImpl pci;
if (GetInternalDevice()->GetFeatureReport(pci.Buffer, PositionCalibrationImpl::PacketSize))
{
pci.Unpack();
*data = pci.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::GetAllPositionCalibrationReports(Array<PositionCalibrationReport>* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getAllPositionCalibrationReports, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getAllPositionCalibrationReports(Array<PositionCalibrationReport>* data)
{
PositionCalibrationReport pc;
bool result = getPositionCalibrationReport(&pc);
if (!result)
return false;
int positions = pc.NumPositions;
data->Clear();
data->Resize(positions);
for (int i = 0; i < positions; i++)
{
result = getPositionCalibrationReport(&pc);
if (!result)
return false;
OVR_ASSERT(pc.NumPositions == positions);
(*data)[pc.PositionIndex] = pc;
// IMU should be the last one
OVR_ASSERT(pc.PositionType == (pc.PositionIndex == positions - 1) ?
PositionCalibrationReport::PositionType_IMU : PositionCalibrationReport::PositionType_LED);
}
return true;
}
bool Sensor2DeviceImpl::SetCustomPatternReport(const CustomPatternReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setCustomPatternReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setCustomPatternReport(const CustomPatternReport& data)
{
CustomPatternImpl cpi(data);
return GetInternalDevice()->SetFeatureReport(cpi.Buffer, CustomPatternImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetCustomPatternReport(CustomPatternReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getCustomPatternReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getCustomPatternReport(CustomPatternReport* data)
{
CustomPatternImpl cpi;
if (GetInternalDevice()->GetFeatureReport(cpi.Buffer, CustomPatternImpl::PacketSize))
{
cpi.Unpack();
*data = cpi.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetManufacturingReport(const ManufacturingReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setManufacturingReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setManufacturingReport(const ManufacturingReport& data)
{
ManufacturingImpl mi(data);
return GetInternalDevice()->SetFeatureReport(mi.Buffer, ManufacturingImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetManufacturingReport(ManufacturingReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getManufacturingReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getManufacturingReport(ManufacturingReport* data)
{
ManufacturingImpl mi;
if (GetInternalDevice()->GetFeatureReport(mi.Buffer, ManufacturingImpl::PacketSize))
{
mi.Unpack();
*data = mi.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetLensDistortionReport(const LensDistortionReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setLensDistortionReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setLensDistortionReport(const LensDistortionReport& data)
{
LensDistortionImpl ui(data);
return GetInternalDevice()->SetFeatureReport(ui.Buffer, LensDistortionImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetLensDistortionReport(LensDistortionReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getLensDistortionReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getLensDistortionReport(LensDistortionReport* data)
{
LensDistortionImpl ui;
if (GetInternalDevice()->GetFeatureReport(ui.Buffer, LensDistortionImpl::PacketSize))
{
ui.Unpack();
*data = ui.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetUUIDReport(const UUIDReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setUUIDReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setUUIDReport(const UUIDReport& data)
{
UUIDImpl ui(data);
return GetInternalDevice()->SetFeatureReport(ui.Buffer, UUIDImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetUUIDReport(UUIDReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getUUIDReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getUUIDReport(UUIDReport* data)
{
UUIDImpl ui;
if (GetInternalDevice()->GetFeatureReport(ui.Buffer, UUIDImpl::PacketSize))
{
ui.Unpack();
*data = ui.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetKeepAliveMuxReport(const KeepAliveMuxReport& data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setKeepAliveMuxReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setKeepAliveMuxReport(const KeepAliveMuxReport& data)
{
KeepAliveMuxImpl kami(data);
return GetInternalDevice()->SetFeatureReport(kami.Buffer, KeepAliveMuxImpl::PacketSize);
}
bool Sensor2DeviceImpl::GetKeepAliveMuxReport(KeepAliveMuxReport* data)
{
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getKeepAliveMuxReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getKeepAliveMuxReport(KeepAliveMuxReport* data)
{
KeepAliveMuxImpl kami;
if (GetInternalDevice()->GetFeatureReport(kami.Buffer, KeepAliveMuxImpl::PacketSize))
{
kami.Unpack();
*data = kami.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::SetTemperatureReport(const TemperatureReport& data)
{
// direct call if we are already on the device manager thread
if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
{
return setTemperatureReport(data);
}
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::setTemperatureReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::setTemperatureReport(const TemperatureReport& data)
{
TemperatureImpl ti(data);
return GetInternalDevice()->SetFeatureReport(ti.Buffer, TemperatureImpl::PacketSize);
}
bool Sensor2DeviceImpl::getTemperatureReport(TemperatureReport* data)
{
TemperatureImpl ti;
if (GetInternalDevice()->GetFeatureReport(ti.Buffer, TemperatureImpl::PacketSize))
{
ti.Unpack();
*data = ti.Settings;
return true;
}
return false;
}
bool Sensor2DeviceImpl::GetAllTemperatureReports(Array<Array<TemperatureReport> >* data)
{
// direct call if we are already on the device manager thread
if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
{
return getAllTemperatureReports(data);
}
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getAllTemperatureReports, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getAllTemperatureReports(Array<Array<TemperatureReport> >* data)
{
TemperatureReport t;
bool result = getTemperatureReport(&t);
if (!result)
return false;
int bins = t.NumBins, samples = t.NumSamples;
data->Clear();
data->Resize(bins);
for (int i = 0; i < bins; i++)
(*data)[i].Resize(samples);
for (int i = 0; i < bins; i++)
for (int j = 0; j < samples; j++)
{
result = getTemperatureReport(&t);
if (!result)
return false;
OVR_ASSERT(t.NumBins == bins && t.NumSamples == samples);
(*data)[t.Bin][t.Sample] = t;
}
return true;
}
bool Sensor2DeviceImpl::GetGyroOffsetReport(GyroOffsetReport* data)
{
// direct call if we are already on the device manager thread
if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
{
return getGyroOffsetReport(data);
}
bool result;
if (!GetManagerImpl()->GetThreadQueue()->
PushCallAndWaitResult(this, &Sensor2DeviceImpl::getGyroOffsetReport, &result, data))
{
return false;
}
return result;
}
bool Sensor2DeviceImpl::getGyroOffsetReport(GyroOffsetReport* data)
{
GyroOffsetImpl goi;
if (GetInternalDevice()->GetFeatureReport(goi.Buffer, GyroOffsetImpl::PacketSize))
{
goi.Unpack();
*data = goi.Settings;
return true;
}
return false;
}
void Sensor2DeviceImpl::onTrackerMessage(Tracker2Message* message)
{
if (message->Type != Tracker2Message_Sensors)
return;
const float sampleIntervalTimeUnit = (1.0f / 1000.f);
double scaledSampleIntervalTimeUnit = sampleIntervalTimeUnit;
Tracker2Sensors& s = message->Sensors;
double absoluteTimeSeconds = 0.0;
if (SequenceValid)
{
UInt32 runningSampleCountDelta;
if (s.RunningSampleCount < LastRunningSampleCount)
{
// The running sample count on the device rolled around the 16 bit counter
// (expect to happen about once per minute), so RunningSampleCount
// needs a high word increment.
runningSampleCountDelta = ((((int)s.RunningSampleCount) + 0x10000) - (int)LastRunningSampleCount);
}
else
{
runningSampleCountDelta = (s.RunningSampleCount - LastRunningSampleCount);
}
absoluteTimeSeconds = LastSensorTime.TimeSeconds;
scaledSampleIntervalTimeUnit = TimeFilter.ScaleTimeUnit(sampleIntervalTimeUnit);
// If we missed a small number of samples, replicate the last sample.
if ((runningSampleCountDelta > LastNumSamples) && (runningSampleCountDelta <= 254))
{
if (HandlerRef.HasHandlers())
{
MessageBodyFrame sensors(this);
sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - s.NumSamples * scaledSampleIntervalTimeUnit;
sensors.TimeDelta = (float) ((runningSampleCountDelta - LastNumSamples) * scaledSampleIntervalTimeUnit);
sensors.Acceleration = LastAcceleration;
sensors.RotationRate = LastRotationRate;
sensors.MagneticField = LastMagneticField;
sensors.Temperature = LastTemperature;
pCalibration->Apply(sensors);
HandlerRef.Call(sensors);
}
}
}
else
{
LastAcceleration = Vector3f(0);
LastRotationRate = Vector3f(0);
LastMagneticField= Vector3f(0);
LastTemperature = 0;
SequenceValid = true;
}
LastNumSamples = s.NumSamples;
LastRunningSampleCount = s.RunningSampleCount;
if (HandlerRef.HasHandlers())
{
MessageBodyFrame sensors(this);
UByte iterations = s.NumSamples;
if (s.NumSamples > 2)
{
iterations = 2;
sensors.TimeDelta = (float) ((s.NumSamples - 1) * scaledSampleIntervalTimeUnit);
}
else
{
sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
}
for (UByte i = 0; i < iterations; i++)
{
sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - ( iterations - 1 - i ) * scaledSampleIntervalTimeUnit;
sensors.Acceleration = AccelFromBodyFrameUpdate(s, i);
sensors.RotationRate = EulerFromBodyFrameUpdate(s, i);
sensors.MagneticField= MagFromBodyFrameUpdate(s);
sensors.Temperature = s.Temperature * 0.01f;
pCalibration->Apply(sensors);
HandlerRef.Call(sensors);
// TimeDelta for the last two sample is always fixed.
sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
}
// Send pixel read only when frame timestamp changes.
if (LastFrameTimestamp != s.FrameTimestamp)
{
MessagePixelRead pixelRead(this);
// Prepare message for pixel read
pixelRead.PixelReadValue = s.FrameID;
pixelRead.RawFrameTime = s.FrameTimestamp;
pixelRead.RawSensorTime = s.SampleTimestamp;
pixelRead.SensorTimeSeconds = LastSensorTime.TimeSeconds;
pixelRead.FrameTimeSeconds = LastFrameTime.TimeSeconds;
HandlerRef.Call(pixelRead);
LastFrameTimestamp = s.FrameTimestamp;
}
UInt16 lowFrameCount = (UInt16) FullCameraFrameCount;
// Send message only when frame counter changes
if (lowFrameCount != s.CameraFrameCount)
{
// check for the rollover in the counter
if (s.CameraFrameCount < lowFrameCount)
FullCameraFrameCount += 0x10000;
// update the low bits
FullCameraFrameCount = (FullCameraFrameCount & ~0xFFFF) | s.CameraFrameCount;
MessageExposureFrame vision(this);
vision.CameraPattern = s.CameraPattern;
vision.CameraFrameCount = FullCameraFrameCount;
vision.CameraTimeSeconds = LastCameraTime.TimeSeconds;
HandlerRef.Call(vision);
}
LastAcceleration = sensors.Acceleration;
LastRotationRate = sensors.RotationRate;
LastMagneticField= sensors.MagneticField;
LastTemperature = sensors.Temperature;
//LastPixelRead = pixelRead.PixelReadValue;
//LastPixelReadTimeStamp = LastFrameTime;
}
else
{
if (s.NumSamples != 0)
{
UByte i = (s.NumSamples > 1) ? 1 : 0;
LastAcceleration = AccelFromBodyFrameUpdate(s, i);
LastRotationRate = EulerFromBodyFrameUpdate(s, i);
LastMagneticField = MagFromBodyFrameUpdate(s);
LastTemperature = s.Temperature * 0.01f;
}
}
}
// Helper function to handle wrap-around of timestamps from Tracker2Message and convert them
// to system time.
// - Any timestamps that didn't increment keep their old system time.
// - This is a bit tricky since we don't know which one of timestamps has most recent time.
// - The first timestamp must be the IMU one; we assume that others can't be too much ahead of it
void UpdateDK2Timestamps(SensorTimeFilter& tf,
SensorTimestampMapping** timestamps, UInt32 *rawValues, int count)
{
int updateIndices[4];
int updateCount = 0;
int i;
double now = Timer::GetSeconds();
OVR_ASSERT(count <= sizeof(updateIndices)/sizeof(int));
// Update timestamp wrapping for any values that changed.
for (i = 0; i < count; i++)
{
UInt32 lowMks = (UInt32)timestamps[i]->TimestampMks; // Low 32-bits are raw old timestamp.
if (rawValues[i] != lowMks)
{
if (i == 0)
{
// Only check for rollover in the IMU timestamp
if (rawValues[i] < lowMks)
{
LogText("Timestamp %d rollover, was: %u, now: %u\n", i, lowMks, rawValues[i]);
timestamps[i]->TimestampMks += 0x100000000;
}
// Update the low bits
timestamps[i]->TimestampMks = (timestamps[i]->TimestampMks & 0xFFFFFFFF00000000) | rawValues[i];
}
else
{
// Take the high bits from the main timestamp first (not a typo in the first argument!)
timestamps[i]->TimestampMks =
(timestamps[0]->TimestampMks & 0xFFFFFFFF00000000) | rawValues[i];
// Now force it into the reasonable range around the expanded main timestamp
if (timestamps[i]->TimestampMks > timestamps[0]->TimestampMks + 0x1000000)
timestamps[i]->TimestampMks -= 0x100000000;
else if (timestamps[i]->TimestampMks + 0x100000000 < timestamps[0]->TimestampMks + 0x1000000)
timestamps[i]->TimestampMks += 0x100000000;
}
updateIndices[updateCount] = i;
updateCount++;
}
}
// TBD: Simplify. Update indices should no longer be needed with new TimeFilter accepting
// previous values.
// We might want to have multi-element checking time roll-over.
static const double mksToSec = 1.0 / 1000000.0;
for (int i = 0; i < updateCount; i++)
{
SensorTimestampMapping& ts = *timestamps[updateIndices[i]];
ts.TimeSeconds = tf.SampleToSystemTime(((double)ts.TimestampMks) * mksToSec,
now, ts.TimeSeconds, ts.DebugTag);
}
}
void Sensor2DeviceImpl::OnInputReport(UByte* pData, UInt32 length)
{
bool processed = false;
if (!processed)
{
Tracker2Message message;
if (decodeTracker2Message(&message, pData, length))
{
processed = true;
// Process microsecond timestamps from DK2 tracker.
// Mapped and raw values must correspond to one another in each array.
// IMU timestamp must be the first one!
SensorTimestampMapping* tsMaps[3] =
{
&LastSensorTime,
&LastCameraTime,
&LastFrameTime
};
UInt32 tsRawMks[3] =
{
message.Sensors.SampleTimestamp,
message.Sensors.CameraTimestamp,
message.Sensors.FrameTimestamp
};
// Handle wrap-around and convert samples to system time for any samples that changed.
UpdateDK2Timestamps(TimeFilter, tsMaps, tsRawMks, sizeof(tsRawMks)/sizeof(tsRawMks[0]));
onTrackerMessage(&message);
/*
if (SF_LOG_fp)
{
static UInt32 lastFrameTs = 0;
static UInt32 lastCameraTs = 0;
if ((lastFrameTs != message.Sensors.FrameTimestamp) ||
(lastCameraTs = message.Sensors.CameraTimestamp))
fprintf(SF_LOG_fp, "msg cameraTs: 0x%X frameTs: 0x%X sensorTs: 0x%X\n",
message.Sensors.CameraTimestamp, message.Sensors.FrameTimestamp,
message.Sensors.SampleTimestamp);
lastFrameTs = message.Sensors.FrameTimestamp;
lastCameraTs = message.Sensors.CameraTimestamp;
}
*/
#if 0
// Checks for DK2 firmware bug.
static unsigned SLastSampleTime = 0;
if ((SLastSampleTime > message.Sensors.SampleTimestamp) && message.Sensors.SampleTimestamp > 1000000 )
{
fprintf(SF_LOG_fp, "*** Sample Timestamp Wrap! ***\n");
OVR_ASSERT (SLastSampleTime <= message.Sensors.SampleTimestamp);
}
SLastSampleTime = message.Sensors.SampleTimestamp;
static unsigned SLastCameraTime = 0;
if ((SLastCameraTime > message.Sensors.CameraTimestamp) && message.Sensors.CameraTimestamp > 1000000 )
{
fprintf(SF_LOG_fp, "*** Camera Timestamp Wrap! ***\n");
OVR_ASSERT (SLastCameraTime <= message.Sensors.CameraTimestamp);
}
SLastCameraTime = message.Sensors.CameraTimestamp;
static unsigned SLastFrameTime = 0;
if ((SLastFrameTime > message.Sensors.FrameTimestamp) && message.Sensors.FrameTimestamp > 1000000 )
{
fprintf(SF_LOG_fp, "*** Frame Timestamp Wrap! ***\n");
OVR_ASSERT (SLastFrameTime <= message.Sensors.FrameTimestamp);
}
SLastFrameTime = message.Sensors.FrameTimestamp;
#endif
}
}
}
double Sensor2DeviceImpl::OnTicks(double tickSeconds)
{
if (tickSeconds >= NextKeepAliveTickSeconds)
{
// Must send DK2 keep-alive. Set Keep-alive at 10 seconds.
KeepAliveMuxReport keepAlive;
keepAlive.CommandId = 0;
keepAlive.INReport = 11;
keepAlive.Interval = 10 * 1000;
// Device creation is done from background thread so we don't need to add this to the command queue.
KeepAliveMuxImpl keepAliveImpl(keepAlive);
GetInternalDevice()->SetFeatureReport(keepAliveImpl.Buffer, KeepAliveMuxImpl::PacketSize);
// Emit keep-alive every few seconds.
double keepAliveDelta = 3.0; // Use 3-second interval.
NextKeepAliveTickSeconds = tickSeconds + keepAliveDelta;
}
return NextKeepAliveTickSeconds - tickSeconds;
}
/*
// TBD: don't report calibration for now, until we figure out the logic between camera and mag yaw correction
bool Sensor2DeviceImpl::IsMagCalibrated()
{
return pCalibration->IsMagCalibrated();
}
*/
} // namespace OVR