//////////////////////////////////////////////////////////////////////////
    // doMTSettings
    //
    // Set user settings in MTi/MTx
    // Assumes initialized global MTComm class
    void AHRS::doMtSettings(xsens::Cmt3 &cmt3, CmtOutputMode &mode,
		    CmtOutputSettings &settings, CmtDeviceId deviceIds[]) 
    {
	    XsensResultValue res;
	    unsigned long mtCount = cmt3.getMtCount();

	    // set sensor to config sate
	    res = cmt3.gotoConfig();
	    EXIT_ON_ERROR(res,"gotoConfig");

	    unsigned short sampleFreq;
	    sampleFreq = cmt3.getSampleFrequency();

	    // set the device output mode for the device(s)

	    for (unsigned int i = 0; i < mtCount; i++) {
		    CmtDeviceMode deviceMode(mode, settings, sampleFreq);
		    if ((deviceIds[i] & 0xFFF00000) != 0x00500000) {
			    // not an MTi-G, remove all GPS related stuff
			    deviceMode.m_outputMode &= 0xFF0F;
		    }
		    res = cmt3.setDeviceMode(deviceMode, true, deviceIds[i]);
		    EXIT_ON_ERROR(res,"setDeviceMode");
	    }

	    // start receiving data
	    res = cmt3.gotoMeasurement();
	    EXIT_ON_ERROR(res,"gotoMeasurement");
    }
//////////////////////////////////////////////////////////////////////////
// doMTSettings
//
// Set user settings in MTi/MTx
// Assumes initialized global MTComm class
void XSensDriver::doMtSettings( xsens::Cmt3 &cmt3, CmtOutputMode &mode, CmtOutputSettings &settings, CmtDeviceId deviceIds[] )
{
    XsensResultValue res;
    unsigned long mtCount = cmt3.getMtCount();

    // set sensor to config sate
    res = cmt3.gotoConfig();

    unsigned short sampleFreq;
    sampleFreq = 100;//cmt3.getSampleFrequency();

    std::cout << "sampling at " << sampleFreq << std::endl;

    // set the device output mode for the device(s)
    std::cout << "Configuring your mode selection" << std::endl;

    for ( unsigned int i = 0; i < mtCount; i++ )
    {
        CmtDeviceMode deviceMode( mode, settings, sampleFreq );
        if ( ( deviceIds[i] & 0xFFF00000 ) != 0x00500000 )
        {
            // not an MTi-G, remove all GPS related stuff
            deviceMode.m_outputMode &= 0xFF0F;
        }
        res = cmt3.setDeviceMode( deviceMode, true, deviceIds[i] );
    }

    // start receiving data
    res = cmt3.gotoMeasurement();
}
    //////////////////////////////////////////////////////////////////////////
    // doHardwareScan
    //
    // Checks available COM ports and scans for MotionTrackers
    int AHRS::doHardwareScan(xsens::Cmt3 &cmt3, CmtDeviceId deviceIds[])
    {
	    XsensResultValue res;
	    List<CmtPortInfo> portInfo;
	    unsigned long portCount = 0;
	    int mtCount;
	    
	    xsens::cmtScanPorts(portInfo);
	    portCount = portInfo.length();
	    
	    if (portCount == 0) {
		    printf("No MotionTrackers found \n\n");
		    return 0; 
	    }

	    for(int p = 0; p < (int)portCount; p++){
		    res = cmt3.openPort(portInfo[p].m_portName, portInfo[p].m_baudrate);
		    EXIT_ON_ERROR(res,"cmtOpenPort");  
	    }

	    //get the Mt sensor count.
	    mtCount = cmt3.getMtCount();

	    // retrieve the device IDs 
	    for(int j = 0; j < mtCount; j++){
		    res = cmt3.getDeviceId((unsigned char)(j+1), deviceIds[j]);
		    EXIT_ON_ERROR(res,"getDeviceId");
	    }
	    
	    return mtCount;
    }
Exemple #4
0
void exitFunc(void) {
  // Close any open COM ports
  cmt3.closePort();

  // get rid of keystrokes before we post our message

  while (_kbhit()) {
    _getch();
  }

  // wait for a keypress
  if (!g_userQuit) {
    printf("Press a key to exit\n");
    _getch();
  }

  if(g_outputFile != NULL) {
    fclose(g_outputFile);
  }
}
Exemple #5
0
int main(int argc, char **argv)
{
    unsigned long mtCount;

    /* initialize carmen */
    //carmen_randomize(&argc, &argv);
    carmen_ipc_initialize(argc, argv);
    carmen_param_check_version(argv[0]);

    mode = 6;
    settings = 2;
    const char* pName = NULL;

    read_parameters(argc, argv);

    pName = dev;

    packet = new xsens::Packet((unsigned short) mtCount, cmt3.isXm());
    /* Initializing Xsens */
    initializeXsens(cmt3, mode, settings, mtCount, (char*)pName);

    /* Setup exit handler */
    signal(SIGINT, shutdown_xsens);

    /* register localize related messages */
    // register_ipc_messages();
    carmen_xsens_define_messages();

    while(1){
    getDataFromXsens(cmt3, packet, mode, settings, g_data);
    publish_xsens_global(g_data, /*mode,*/ settings);
    sleep(0.1);
    }

    /* Loop forever */
    carmen_ipc_dispatch();

    return 0;
}
//////////////////////////////////////////////////////////////////////////
// doHardwareScan
//
// Checks available COM ports and scans for MotionTrackers
int XSensDriver::doHardwareScan( xsens::Cmt3 &cmt3, CmtDeviceId deviceIds[] )
{
    XsensResultValue res;
    List<CmtPortInfo> portInfo;
    unsigned long portCount = 0;
    int mtCount;

    std::cout << "Scanning for connected Xsens devices..." << std::endl;

//    xsens::cmtScanPorts( portInfo );
    CmtPortInfo target_port = { 0, 0, 0, "" };
    sprintf( target_port.m_portName, "%s", port_.c_str() );

    if( cmtScanPort( target_port, target_port.m_baudrate ) ) portInfo.append( target_port );

    portCount = portInfo.length();
    std::cout << "done" << std::endl;

    if ( portCount == 0 )
    {
        std::cout << "No MotionTrackers found" << std::endl;
        return 0;
    }

    for ( int i = 0; i < (int) portCount; i++ )
    {
        std::cout << "Using COM port at [ " << portInfo[i].m_portName << " ] ";
        std::cout << "baud [ ";

        switch ( portInfo[i].m_baudrate )
        {
        case B9600:
            std::cout << "9k6";
            break;
        case B19200:
            std::cout << "19k2";
            break;
        case B38400:
            std::cout << "38k4";
            break;
        case B57600:
            std::cout << "57k6";
            break;
        case B115200:
            std::cout << "115k2";
            break;
        case B230400:
            std::cout << "230k4";
            break;
        case B460800:
            std::cout << "460k8";
            break;
        case B921600:
            std::cout << "921k6";
            break;
        default:
            std::cout << portInfo[i].m_baudrate;
        }
        std::cout << " ]" << std::endl;
    }

    std::cout << "Opening ports...";
    //open the port which the device is connected to and connect at the device's baudrate.
    for ( int p = 0; p < (int) portCount; p++ )
    {
        res = cmt3.openPort( portInfo[p].m_portName, portInfo[p].m_baudrate );
        //      EXIT_ON_ERROR(res,"cmtOpenPort");
    }
    std::cout << "done" << std::endl;

    //get the Mt sensor count.
    std::cout << "Retrieving MotionTracker count (excluding attached Xbus Master(s))" << std::endl;
    mtCount = cmt3.getMtCount();
    std::cout << "MotionTracker count: " << mtCount << std::endl;

    // retrieve the device IDs
    std::cout << "Retrieving MotionTrackers device ID(s)" << std::endl;
    for ( int j = 0; j < mtCount; j++ )
    {
        res = cmt3.getDeviceId( (unsigned char) ( j + 1 ), deviceIds[j] );
        //      EXIT_ON_ERROR(res,"getDeviceId");
        std::cout << "Device ID at busId " << j + 1 << "," << (long) deviceIds[j] << std::endl;
    }

    return mtCount;
}
Exemple #7
0
int main(int argc, char * argv[]) {
  int			l_retMain = 0;
  XsensResultValue	l_res;
  int			l_nDevices = 0;
  Packet*		l_packet = NULL;
  bool			l_reportRaw = false;
  bool			l_outBinary = false;

  CmtVector		l_acc, l_mag, l_rot;
  CmtShortVector	l_accR, l_magR, l_rotR;
  CmtEuler		l_eul;
  CmtQuat		l_quat;
  CmtRawData		l_raw;
  double                l_timeStamp;
  uint32_t*             l_devIDs;
  int                   l_packetCnt;

  // Set exit function
  atexit(exitFunc);

  if(argc < 2) {
    usage();
    l_retMain = -1;
    goto end;
  }

  init(argc, argv, &g_dataOptions, &g_processOptions);

  report_tasks(&g_dataOptions, &g_processOptions);

  l_outBinary = g_processOptions.get_binaryMode();

  l_res = cmt3.openLogFile(g_processOptions.get_infile().c_str());

  if(l_res != XRV_OK) {
    printf("Error opening file '%s'!\n",g_processOptions.get_infile().c_str());
    l_retMain = -1;
    goto end;
  }

  if(g_processOptions.get_outfile().length() > 0) {

    g_outputFile = fopen(g_processOptions.get_outfile().c_str(),"w");

  } else {

    if(l_outBinary) {
      g_outputFile = fopen("output.bin","w");
    } else {
      g_outputFile = fopen("output.ascii","w");
    }

  }
	
  if(g_outputFile == NULL) {
    printf("Error opening output file '%s'!\n",g_processOptions.get_outfile().c_str());
    l_retMain = -1;
    goto end;
  }

  // see how many devices were present..
  l_nDevices = cmt3.getMtCount();

  // initialize the packet structure
  l_packet = new Packet((unsigned short)l_nDevices,cmt3.isXm());

  // make room for the dev ids
  l_devIDs = (uint32_t*)calloc(l_nDevices,sizeof(uint32_t));

  for(int d = 0; d < l_nDevices; d++) {
    cmt3.getMtDeviceId(d,l_devIDs[d]);    
  }
  
  // check if there is raw data...
  cmt3.readDataPacket(l_packet);

  if(g_dataOptions.use_raw && !l_packet->containsRawData()) {
    warning("[WARNING]: Data does not contain raw data!\n");
    g_dataOptions.use_raw = false;
  }
  cmt3.resetLogFileReadPos();
  
  write_header(g_outputFile,l_nDevices,l_devIDs,g_dataOptions.get_options(), g_processOptions.get_options());

  l_reportRaw = g_dataOptions.use_raw;

  while(l_res == XRV_OK) {

    l_res = cmt3.readDataPacket(l_packet);

    l_packetCnt++;

    for(int dev = 0; dev < l_nDevices; dev++) {

      if(l_reportRaw) {
	l_raw = l_packet->getRawData();
      }

      // report the time stamp
      if(g_dataOptions.use_tim) {
	l_timeStamp = (double)l_packet->getRtc(dev);
	write_1vec(g_outputFile,&l_timeStamp,&g_processOptions);
      }

      if(g_dataOptions.use_acc) {

	l_acc = l_packet->getCalAcc(dev);
	write_3vec(g_outputFile,l_acc.m_data,&g_processOptions);
				
	if(l_reportRaw) {
	  l_accR = l_raw.m_acc;
	  write_3vec(g_outputFile,l_accR.m_data,&g_processOptions);
	}
      }

      if(g_dataOptions.use_gyr) {
	l_rot = l_packet->getCalGyr(dev);

	write_3vec(g_outputFile,l_rot.m_data,&g_processOptions);

	if(l_reportRaw) {
	  l_rotR = l_raw.m_gyr;
	  write_3vec(g_outputFile,l_rotR.m_data,&g_processOptions);
	}
      }

      if(g_dataOptions.use_mag) {
	l_mag = l_packet->getCalMag(dev);	
	write_3vec(g_outputFile,l_mag.m_data,&g_processOptions);

	if(l_reportRaw) {
	  l_magR = l_raw.m_mag;
	  write_3vec(g_outputFile,l_mag.m_data,&g_processOptions);
	}
      }

      if(g_dataOptions.use_eul) {
	l_eul = l_packet->getOriEuler(dev);
	write_3vec(g_outputFile,l_eul.m_roll, l_eul.m_pitch, l_eul.m_yaw,&g_processOptions);
      }

      if(g_dataOptions.use_qua) {
	l_quat = l_packet->getOriQuat(dev);
	write_4vec(g_outputFile,l_quat.m_data,&g_processOptions);
      }
    }

    if(l_outBinary == false) {
      fprintf(g_outputFile,"\n");
    }
  }

  free(l_devIDs);

  fclose(g_outputFile);

  g_outputFile = NULL;

 end:
  return l_retMain;
}