void Xsens::getNewData() {
// We want the most recent data. No idea, how to do that properly...
//mtcomm.flush();
//------------------------------------------------------------------
int readReturnValue = mtcomm.readDataMessage(data, datalen);
if (readReturnValue != MTRV_OK) {
//std::cout << "Problem while reading from device\n";
std::cout << "x" << std::flush;
_readError = true;
return;
}
// Get sample count for timestamp calculation
mtcomm.getValue(VALUE_SAMPLECNT, samplecounter, data, BID_MASTER);
if (_lastSampleCounter<0) samplecounter = 0; // bug workaround
if (_lastSampleCounter > samplecounter) { // Overflow of the unsigned short?
struct timeval overflow;
overflow.tv_sec = 655;
overflow.tv_usec = 360000;
tvu_add(&startTime, &startTime, &overflow);
//startTime = startTime + 655.36;///
}
_lastSampleCounter = samplecounter;
//lastDataTime = startTime + 0.01 * samplecounter;
tvu_add_ms(&lastDataTime,&startTime, 10 * samplecounter);
_readError = false;
struct timeval now;
struct timeval diff;
gettimeofday(&now, NULL);
tvu_subtract(&diff,&now,&lastDataTime);
if (tvu_timeval_to_double(&diff) > 0.1) { // Data too old (buffered data is read -> flush and read again
//std::cout << "Flushing!\n";
mtcomm.flush(); // Flush old data
getNewData();
}
// debug timstamp
if (DEBUG_TIMESTAMP) {
static unsigned int numRead = 0;
if (numRead % 50 == 0){
struct timeval debug_tv;
fprintf(stderr,"now= %d (sec) %d (usec)\n",(int)now.tv_sec, (int)now.tv_usec);
fprintf(stderr,"xsens= %d (sec) %d (usec)\n",(int)lastDataTime.tv_sec, (int)lastDataTime.tv_usec);
fprintf(stderr,"diff= %d (sec) %d (usec)\n",(int)diff.tv_sec, (int)diff.tv_usec);
fprintf(stderr,"samplecounter= %d\n",samplecounter);
fprintf(stderr,"start= %d (sec) %d (usec)\n",(int)startTime.tv_sec, (int)startTime.tv_usec);
tvu_subtract(&debug_tv, &now, &startTime);
fprintf(stderr,"startToNow= %d (sec) %d (usec)\n",(int)debug_tv.tv_sec, (int)debug_tv.tv_usec);
fprintf(stderr,"\n\n");
}
++numRead;
}
}
bool APIDataReceiver::waitForDataReady(int timeout, char *data) {
if(initialized || startup())
{
// just to be sure, check before we sleep
if(getNewData(data))
return true;
// sleep till signaled
WaitForSingleObject(dataValidEventHandle, static_cast<DWORD>(timeout));
// we woke up, so check for data
return getNewData(data);
}
// sleep if error
if(timeout > 0)
Sleep(static_cast<DWORD>(timeout));
return false;
}
void ArJoyHandler::getData(void)
{
if (myUseOld && !myInitialized)
return;
if (!myFirstData && myLastDataGathered.mSecSince() < 5)
return;
myFirstData = false;
myLastDataGathered.setToNow();
if (myUseOld)
getOldData();
else
getNewData();
}
/************************************************************************
**Function name: getNackSn()
**Descriptions: Get NackSn field
**Input: unsigned int * pduLen,unsigned short *amHdr,FSM_PKT * pkt,unsigned short * ptr
**Output: unsigned int * pduLen,unsigned short *amHdr,unsigned short * ptr
**Returns: the number of the nackSn field
**Created by: ZhangYi
**Created Date: 2014
**---------------------------------------------------------------------
**Modified by: ZhangYi
**Modified Date: 2014
************************************************************************/
u16 getNackSn(u32 * pduLen,u16 *amHdr,FSM_PKT * pkt,u16 * ptr)
{
u16 nackSn = 0;
u16 sptr = 0;
if( *ptr == 0 ) //如果剩余位数为0就再从pkt数据部分取2字节数据
{
getNewData(ptr,pduLen,pkt,amHdr);
}
if( *ptr >= 10 )
{
nackSn += (*amHdr&(0xffc0>>(*ptr-10)))>>(16-*ptr); //取10位NACK_SN
//nackSn += ((*amHdr)&0xffc0) >> 6; //取10位NACK_SN
*ptr -= 10;
}
// Auslesen der empfangenen Semesterveranstaltungsnamen
void Browser::coursesRecieved(QNetworkReply *reply)
{
// Erst mit Daten im TreeView lässt sich der Header gestalten.
QStringList headerLabels;
headerLabels << "Name" << QString::fromUtf8("Größe") << "Datum";
itemModel->setHorizontalHeaderLabels(headerLabels);
ui->dataTreeView->header()->setSectionResizeMode(0, QHeaderView::Stretch);
ui->dataTreeView->header()->setSectionResizeMode(1, QHeaderView::ResizeToContents);
ui->dataTreeView->header()->setSectionResizeMode(2, QHeaderView::ResizeToContents);
// Prüfen auf Fehler beim Abruf
if (!reply->error())
Parser::parseCourses(reply, itemModel);
else
Utils::errorMessageBox("Beim Abruf der Veranstaltungen ist ein Fehler aufgetreten", reply->errorString());
// Löschen der Antwort aus der Queue
replies.remove(reply);
// Antwort für das spätere Löschen markieren
reply->deleteLater();
// Prüfen, ob noch Antworten ausstehen und ggf. Reaktiveren der
// Benutzeroberfläche
if (replies.isEmpty())
{
// Veranstaltungen alphabetisch sortieren
itemModel->sort(0);
QObject::disconnect(manager,
SIGNAL(finished(QNetworkReply *)),
this,
SLOT(coursesRecieved(QNetworkReply *)));
// Aufruf der Funktion zur Aktualisierung der Dateien
getNewData();
}
/////////FIXME//////////
void Xsens::fillMessage(qc_imu_imu_message& msg, bool updateData)
{
if (updateData) getNewData();
msg.timestamp_sec = lastDataTime.tv_sec;
msg.timestamp_usec = lastDataTime.tv_usec;
if ( (_outputMode & OUTPUTMODE_ORIENT)==0
|| (_outputFormat & OUTPUTSETTINGS_ORIENTMODE_MASK)!=OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
{
std::cout << className << "::" << __func__ << ": Not initialized for quaternions!\n";
exit(0);
}
mtcomm.getValue(VALUE_ORIENT_QUAT, fdata, data, BID_MT);
if (DEBUG_XSENS)
printf("Quaternion (QUAD): q0=%0.3f, q1=%0.3f, q2=%0.3f, q3=%0.3f\n",fdata[0], fdata[1], fdata[2], fdata[3]);
msg.q0 = fdata[0];
msg.q1 = fdata[1];
msg.q2 = fdata[2];
msg.q3 = fdata[3];
if (_getAccelerationData) {
if ( (_outputMode & OUTPUTMODE_CALIB)==0
|| (_outputFormat & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) != 0)
{
std::cout << className << "::" << __func__ << ": Not initialized for acceleration data!\n";
exit(0);
}
mtcomm.getValue(VALUE_CALIB_ACC, fdata, data, BID_MT);
if (DEBUG_XSENS)
printf("Acceleration (ACC): accX=%0.2f, accY=%0.2f, accZ=%0.2f\n", fdata[0], fdata[1], fdata[2]);
msg.accX = fdata[0];
msg.accY = fdata[1];
msg.accZ = fdata[2];
} else {
msg.accX = msg.accY = msg.accZ = 0.0;
}
if (_getGyroData) {
if ( (_outputMode & OUTPUTMODE_CALIB)==0
|| (_outputFormat & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) != 0)
{
std::cout << className << "::" << __func__ << ": Not initialized for gyro data!\n";
exit(0);
}
mtcomm.getValue(VALUE_CALIB_GYR, fdata+3, data, BID_MT);
if (DEBUG_XSENS)
printf("Gyroscope (GYR): gyroX=%0.2f, gyroY=%0.2f, gyroZ=%0.2f\n", fdata[3], fdata[4], fdata[5]);
msg.gyroX = fdata[3];
msg.gyroY = fdata[4];
msg.gyroZ = fdata[5];
} else {
msg.gyroX = msg.gyroY = msg.gyroZ = 0.0;
}
if (_getMagneticData) {
mtcomm.getValue(VALUE_CALIB_MAG, fdata+6, data, BID_MT);
if (DEBUG_XSENS)
printf("Compass (MAG): compX=%0.2f, compY=%0.2f, compZ=%0.2f\n", fdata[6], fdata[7], fdata[8]);
msg.magX = fdata[6];
msg.magY = fdata[7];
msg.magZ = fdata[8];
} else {
msg.magX = msg.magY = msg.magZ = 0.0;
}
}
