/**
* Check for an update of new data from the Driver Station
* This will read the new values and update the data structures in this class.
*/
void Kinect::UpdateData()
{
Synchronized sync(m_dataLock);
uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
if (m_recentPacketNumber != packetNumber)
{
m_recentPacketNumber = packetNumber;
int retVal = getDynamicControlData(kHeaderBundleID, m_rawHeader, sizeof(m_rawHeader), 5);
if(retVal == 0)
{
m_numberOfPlayers = (int)m_rawHeader[13];
memcpy(&m_floorClipPlane.x, &m_rawHeader[18], 4);
memcpy(&m_floorClipPlane.y, &m_rawHeader[22], 4);
memcpy(&m_floorClipPlane.z, &m_rawHeader[26], 4);
memcpy(&m_floorClipPlane.w, &m_rawHeader[30], 4);
memcpy(&m_gravityNormal.x, &m_rawHeader[34], 4);
memcpy(&m_gravityNormal.y, &m_rawHeader[38], 4);
memcpy(&m_gravityNormal.z, &m_rawHeader[42], 4);
}
retVal = getDynamicControlData(kSkeletonExtraBundleID, m_rawSkeletonExtra, sizeof(m_rawSkeletonExtra), 5);
if(retVal == 0)
{
memcpy(&m_position[0].x, &m_rawSkeletonExtra[22], 4);
memcpy(&m_position[0].y, &m_rawSkeletonExtra[26], 4);
memcpy(&m_position[0].z, &m_rawSkeletonExtra[30], 4);
memcpy(&m_quality[0], &m_rawSkeletonExtra[34], 4);
memcpy(&m_trackingState[0], &m_rawSkeletonExtra[38], 4);
}
retVal = getDynamicControlData(kSkeletonBundleID, m_rawSkeleton, sizeof(m_rawSkeleton), 5);
if(retVal == 0)
{
for(int i=0; i < Skeleton::JointCount; i++)
{
memcpy(&m_skeletons[0].m_joints[i].x, &m_rawSkeleton[i*12+2], 4);
memcpy(&m_skeletons[0].m_joints[i].y, &m_rawSkeleton[i*12+6], 4);
memcpy(&m_skeletons[0].m_joints[i].z, &m_rawSkeleton[i*12+10], 4);
m_skeletons[0].m_joints[i].trackingState = (Skeleton::JointTrackingState)m_rawSkeletonExtra[i+2];
}
}
// TODO: Read skeleton #2
}
}
/**
* Get dynamic data from the driver station buffer
*/
void KinectStick::GetData()
{
uint32_t packetNumber = DriverStation::GetInstance()->GetPacketNumber();
if (_recentPacketNumber != packetNumber)
{
_recentPacketNumber = packetNumber;
int retVal = getDynamicControlData(kJoystickBundleID, _sticks.data, sizeof(_sticks.data), 5);
if (retVal == 0)
{
wpi_assert(_sticks.formatted.size == sizeof(_sticks.data) - 1);
}
}
}
