void DR::EMHandler::getDynamicRecording(pugi::xml_node &frame) { int errorCode; if (!m_connected) return; pugi::xml_node trackSTAR = frame.append_child("TrackSTAR"); trackSTAR.append_attribute("Unit").set_value("millimeter"); m_lock.lock(); // Collect data from all birds // Loop through all sensors and get a data record if the sensor is attached. // Print result to screen // Note: The default data format is DOUBLE_POSITION_ANGLES. We can use this // format without first setting it. // // DOUBLE_POSITION_QUATERNION_RECORD record, *pRecord = &record; // scan the sensors and request a record for (int sensorID = 0; sensorID < ATC3DG.m_config.numberSensors; sensorID++) { // sensor attached so get record errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record)); if (errorCode != BIRD_ERROR_SUCCESS) { errorHandler(errorCode); return; } // get the status of the last data record // only report the data if everything is okay unsigned int status = GetSensorStatus(sensorID); if (status == VALID_STATUS) { pugi::xml_node bird_node = trackSTAR.append_child("BIRD"); pugi::xml_attribute id = bird_node.append_attribute("id"); id.set_value(sensorID); pugi::xml_node position_node = bird_node.append_child(); position_node.set_name("Position"); position_node.append_attribute("X").set_value(record.x*1000.0f); position_node.append_attribute("Y").set_value(record.y*1000.0f); position_node.append_attribute("Z").set_value(record.z*1000.0f); pugi::xml_node rotation_node = bird_node.append_child(); rotation_node.set_name("Rotation"); rotation_node.append_attribute("W").set_value(record.q[0]); rotation_node.append_attribute("X").set_value(record.q[1]); rotation_node.append_attribute("Y").set_value(record.q[2]); rotation_node.append_attribute("Z").set_value(record.q[3]); } } m_lock.unlock(); }
int waitForStatusAndBlink(int statusIndex, int statusValue, int blinkGreen, int blinkRed, long timeout) { int* status; int yesNo = 0; struct timeval startTime; struct timeval testTime; long timeDiff; if (timeout > 0) { gettimeofday(&startTime, NULL); } do { status = GetSensorStatus(); if(status[statusIndex] == statusValue) { SetLedStatus(0, 0); return 1; } else { if(yesNo == 0) { if(blinkGreen) { SetLedStatus(1, 0); } else if (blinkRed) { SetLedStatus(0, 1); } } else { SetLedStatus(0, 0); } yesNo = !yesNo; millisleep(BLINKING_SLEEP_TIME); } free(status); if (timeout > 0) { gettimeofday(&testTime, NULL); timeDiff = (testTime.tv_usec + 1000000 * testTime.tv_sec) - (startTime.tv_usec + 1000000 * startTime.tv_sec); } } while(timeout == 0 || timeDiff < (timeout * 1000)); return 0; }
int waitForStatus(int statusIndex, int statusValue, long timeout) { int* status; struct timeval startTime; struct timeval testTime; long timeDiff; if (timeout > 0) { gettimeofday(&startTime, NULL); } do { status = GetSensorStatus(); if(status[statusIndex] == statusValue) { return 1; } else { millisleep(WAITING_SLEEP_TIME); } free(status); if (timeout > 0) { gettimeofday(&testTime, NULL); timeDiff = (testTime.tv_usec + 1000000 * testTime.tv_sec) - (startTime.tv_usec + 1000000 * startTime.tv_sec); } } while(timeout == 0 || timeDiff < (timeout * 1000)); return 0; }
int main2() { //Tracker tracker(10000); CSystem PCIBird; CSensor *pSensor; CXmtr *pXmtr; int errorCode; int i; int sensorID; short id; int records = 10000; double rate = 255.0f; printf("\n\n"); printf("ATC3DG Timing Test Application\n"); ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // Initialize the PCIBIRD driver and DLL // // It is always necessary to first initialize the PCIBird "system". By // "system" we mean the set of PCIBird cards installed in the PC. All cards // will be initialized by a single call to InitializeBIRDSystem(). This // call will first invoke a hardware reset of each board. If at any time // during operation of the system an unrecoverable error occurs then the // first course of action should be to attempt to Recall InitializeBIRDSystem() // if this doesn't restore normal operating conditions there is probably a // permanent failure - contact tech support. // A call to InitializeBIRDSystem() does not return any information. // printf("Initializing ATC3DG system...\n"); errorCode = InitializeBIRDSystem(); if(errorCode!=BIRD_ERROR_SUCCESS) { errorHandler(errorCode); exit(1); } ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // GET SYSTEM CONFIGURATION // // In order to get information about the system we have to make a call to // GetBIRDSystemConfiguration(). This call will fill a fixed size structure // containing amongst other things the number of boards detected and the // number of sensors and transmitters the system can support (Note: This // does not mean that all sensors and transmitters that can be supported // are physically attached) // errorCode = GetBIRDSystemConfiguration(&PCIBird.m_config); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // GET SENSOR CONFIGURATION // // Having determined how many sensors can be supported we can dynamically // allocate storage for the information about each sensor. // This information is acquired through a call to GetSensorConfiguration() // This call will fill a fixed size structure containing amongst other things // a status which indicates whether a physical sensor is attached to this // sensor port or not. // pSensor = new CSensor[PCIBird.m_config.numberSensors]; for(i=0;i<PCIBird.m_config.numberSensors;i++) { errorCode = GetSensorConfiguration(i, &(pSensor+i)->m_config); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); } ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // GET TRANSMITTER CONFIGURATION // // The call to GetTransmitterConfiguration() performs a similar task to the // GetSensorConfiguration() call. It also returns a status in the filled // structure which indicates whether a transmitter is attached to this // port or not. In a single transmitter system it is only necessary to // find where that transmitter is in order to turn it on and use it. // pXmtr = new CXmtr[PCIBird.m_config.numberTransmitters]; for(i=0;i<PCIBird.m_config.numberTransmitters;i++) { errorCode = GetTransmitterConfiguration(i, &(pXmtr+i)->m_config); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); } // // Set the measurement rate // errorCode = SetSystemParameter(MEASUREMENT_RATE, &rate, sizeof(rate)); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); // ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // Search for the first attached transmitter and turn it on // for(id=0;id<PCIBird.m_config.numberTransmitters;id++) { if((pXmtr+id)->m_config.attached) { // Transmitter selection is a system function. // Using the SELECT_TRANSMITTER parameter we send the id of the // transmitter that we want to run with the SetSystemParameter() call errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id)); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); break; } } ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // Collect data from all birds // Loop through all sensors and get a data record if the sensor is attached. // Print result to screen // Note: The default data format is DOUBLE_POSITION_ANGLES. We can use this // format without first setting it. // // DOUBLE_POSITION_ANGLES_TIME_Q_RECORD record[8*4], *pRecord = record; // Set the data format type for each attached sensor. for(i=0;i<PCIBird.m_config.numberSensors;i++) { DATA_FORMAT_TYPE type = DOUBLE_POSITION_ANGLES_TIME_Q; errorCode = SetSensorParameter(i,DATA_FORMAT,&type,sizeof(type)); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); } // collect as many records as specified in the records var printf("Collecting %d data records at %3.2fhz...\n", records, rate*3.0); for(i=0;i<records;i++) { errorCode = GetSynchronousRecord(ALL_SENSORS, pRecord, sizeof(record[0]) * PCIBird.m_config.numberSensors); if(errorCode!=BIRD_ERROR_SUCCESS) { errorHandler(errorCode); } // Get a snap shot of the PC's time time_t currentTime; time(¤tTime); // Some user IO, every 500 records print a progress indication if( !(i % 500)) printf( "."); // scan the sensors and request a record if the sensor is physically attached for(sensorID=0;sensorID<PCIBird.m_config.numberSensors;sensorID++) { // get the status of the last data record // only report the data if everything is okay unsigned int status = GetSensorStatus( sensorID); if( status == VALID_STATUS) { // save output to buffer sprintf(output[i][sensorID], "[%d] 0x%04x %8.3f %8.3f %8.3f: %8.2f %8.2f %8.2f %8.4f\n", sensorID, status, record[sensorID].x, record[sensorID].y, record[sensorID].z, record[sensorID].a, record[sensorID].e, record[sensorID].r, record[sensorID].time ); } } } printf("\nSaving results to 'GetSynchronousRecord.dat'..\n"); FILE *fp = fopen( "GetSynchronousRecord.dat", "w"); for(i=0;i<records;i++) for(sensorID=0;sensorID<PCIBird.m_config.numberSensors;sensorID++) fprintf( fp, output[i][sensorID]); fclose( fp); printf("Complete...\n"); ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // Turn off the transmitter before exiting // We turn off the transmitter by "selecting" a transmitter with an id of "-1" // id = -1; errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id)); if(errorCode!=BIRD_ERROR_SUCCESS) errorHandler(errorCode); ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // // Free memory allocations before exiting // delete[] pSensor; delete[] pXmtr; return 0; }