Mise à jour MTI pour ARCS2

+<library>
+    <headers>
+        <header name="mtitracker.h"/>
+        <header name="mtidummy.h"/>
+        <header name="mtilogger.h"/>
+        <header name="mtiemu.h"/>
+    </headers>
+    <components>
+        <component name="MTiTracker"/>
+        <component name="MTiDummy"/>
+        <component name="MTiLogger"/>
+        <component name="MTiEmu"/>
+    </components>
+</library>

+# Project modified by ARCS1to2
+
+unix:  TEMPLATE = lib
+win32: TEMPLATE = vclib
+
+TARGET = mti
+
+HEADERS = MTComm.h
+HEADERS+= mtitracker.h
+HEADERS+= mtidummy.h
+HEADERS+= mtilogger.h
+HEADERS+= mtiemu.h
+HEADRRS+= ../../include/atime.h
+SOURCES = MTComm.cpp
+SOURCES+= mtiemu.cpp
+SOURCES+= mtitracker.cpp
+SOURCES+= mtidummy.cpp
+SOURCES+= 
+
+
+MOC_DIR = ./moc
+OBJECTS_DIR = ./obj
+
+CONFIG += qt thread release
+
+unix {
+INCLUDEPATH+=/usr/local/include
+LIBS+= -L/usr/local/lib
+}
+
+win32 { 
+INCLUDEPATH+=$$(ARCSDIR)/include
+LIBS += -L$$(ARCSDIR)/lib 
+CONFIG += dll exceptions
+DEFINES += _CRT_SECURE_NO_DEPRECATE
+}
+
+
+unix: QMAKE_POST_LINK=mv *.so* ../../../libs
+win32: DLLDESTDIR = ../../../libs
+#The following line was inserted by qt3to4
+#QT +=  qt3support 
+
+
+ALXFILE = libmti.alx
+OTHER_FILES += libmti.alx
+arcslibrary.output = alm_${QMAKE_FILE_BASE}.cpp
+arcslibrary.input = ALXFILE
+arcslibrary.commands = arcslibmaker ${QMAKE_FILE_NAME}
+arcslibrary.variable_out = SOURCES
+QMAKE_EXTRA_COMPILERS += arcslibrary
+INCLUDEPATH += $$(ARCSDIR)/include
+LIBS += -L$$(ARCSDIR)/lib -larcs
+ CONFIG += dll

+#include "mtidummy.h"
+#include <stdlib.h>
+#include <time.h>
+#include <cmath>
+#include <QDateTime>
+#include <QThread>
+
+MTiDummy::MTiDummy(QObject* parent)
+{
+     threaded = false;
+     orientationOutput = true;
+     srand(time(NULL));
+}
+
+
+void MTiDummy::start()
+{
+     std::cout << "Current Thread " << QThread::currentThread() << std::endl;
+
+     std::cout << "[MTi] starting sensor" << std::endl;
+     if (threaded)
+     {
+	  std::cout << "[MTi] in threaded mode " << std::endl;
+	  int msec = 1;
+	  connect(&selfTimer, SIGNAL(timeout()), this, SLOT(sendData()));
+	  selfTimer.start(msec);
+     }
+
+     QTime t = QTime::currentTime();
+     double ts = (double)(t.hour()*3600 + t.minute()*60 + t.second())
+               + (double)(t.msec())/1000.0;
+     emit sendStartTime(ts);
+}
+
+
+void MTiDummy::stop()
+{
+     if (threaded)
+     {
+	  selfTimer.stop();
+	  disconnect(&selfTimer, SIGNAL(timeout()), this, SLOT(sendData()));
+     }
+}
+
+
+void MTiDummy::sendData()
+{
+     float q[4];
+     int x,y,w,z;
+     
+     x = rand()%100;
+     y = rand()%100;
+     z = rand()%100;
+     w = rand()%100;
+     
+     float n = (float)sqrtf(x*x+y*y+z*z+w*w);
+     q[0] = x/n;
+     q[1] = y/n;
+     q[2] = z/n;
+     q[3] = w/n;
+     
+
+     emit sendQuaternion(q);
+}

+#ifndef __MTIDUMMY_H__
+#define __MTIDUMMY_H__
+
+#include <QObject>
+#include <QTimer>
+#include <iostream>
+#include "MTComm.h"
+
+class MTiDummy : public QObject
+{
+Q_OBJECT
+public:
+MTiDummy(QObject* parent=0);
+
+public slots:
+     void setDevice(QString s) {}
+void setOrientationOutput(bool b) { orientationOutput = b;}
+void setQuaternionOutput() {}
+void setThreaded(bool b) { threaded = b; }
+void init() {}
+void start();
+void stop();
+void sendData();
+
+
+signals:
+void sendQuaternion(float*); //!< Sends sensor's orientation using quaternions (4 values).
+void sendStartTime(double);
+
+private:
+bool orientationOutput;
+bool threaded;
+QTimer selfTimer;
+
+
+
+
+};
+
+
+#endif //_MTIDUMMY_H__

+#include "mtiemu.h"
+
+MTiEmu::MTiEmu(QObject* parent) : QObject(parent)
+{
+     autoSend = false;
+     initialized = true;
+     delay = 10;
+     connect(&selfTimer,SIGNAL(timeout()), this, SLOT(sendData()));
+     
+}
+
+
+
+
+void MTiEmu::init()
+{
+     file.open(qPrintable(fileName));
+
+     initialized = file.is_open();
+
+     if (initialized)
+     {
+        double discard;
+        file >> discard;
+	emit sendStartTime(discard);
+     }
+
+
+}
+
+void MTiEmu::start()
+{
+     if (!initialized)
+	  return;
+
+
+     if (autoSend)
+     {
+	     if (delay)
+		  selfTimer.start(delay);
+	     else
+		     while (initialized) 
+			     sendData();
+     }
+}
+
+void MTiEmu::stop()
+{
+     if (autoSend)
+     {
+	  selfTimer.stop();
+     }
+}
+
+
+void MTiEmu::sendData()
+{
+     if (!initialized)
+	  return;
+
+     bool eof;
+
+     int ts;
+
+     float acc[3];
+     float gyr[3];
+     float mag[3];
+     float quat[4];
+
+     // recup temprature ? --> avt quaternion
+     
+     file >> ts >> acc[0] >> acc[1] >> acc[2] >> gyr[0] >> gyr[1] >> gyr[2] >> mag[0] >> mag[1] >> mag[2] >> quat[0] >> quat[1] >> quat[2] >> quat[3] ;
+
+     if (file.eof())
+     {
+	  std::cerr << "[MTiEmu] fin de fichier" << std::endl;
+	  if (autoSend)
+	       stop();
+	  initialized = false;
+	  file.clear();
+	  file.close();
+	  emit sendToken("end");
+	  return;
+     }else{
+     emit sendTimeStamp(ts);
+     emit sendAcceleration(acc);
+     emit sendGyro(gyr);
+     emit sendMag(mag);
+     emit sendQuaternion(quat);
+     }
+
+}

+#ifndef __MTIEMU_H__
+#define __MTIEMU_H__
+
+#include "../../include/atime.h"
+#include <QObject>
+#include <QTimer>
+#include <iostream>
+#include <fstream>
+
+/*! \brief A component to emulate MTI tracker
+ *  \author Jean-Yves Didier
+ *  \date September, 20th 2007
+ *  \ingroup mti
+ */
+class MTiEmu : public QObject
+{
+     Q_OBJECT
+
+	  public:
+     MTiEmu(QObject* parent=0); //!< ARCS Constructor
+
+
+     public slots:
+	  void setFileName(QString s) { fileName =s ;} 
+
+     /*! \brief component initialization
+      * 
+      * setFileName() should be called before.
+      */
+     void init();
+
+/*!\brief Starts the sensor in asynchronous mode.
+ *
+ * Actually does something only if setThreaded() has been called before.
+ */
+void start(); 
+/*! \brief Stops the sensor in asynchronous mode.
+ */
+void stop(); 
+/*! \brief Sends the next available data
+ * 
+ * Depending on the type of desired output, the signal emitted pattern will be different.
+ * If calibrated output is required then it will emit sendAcceleration(), sendGyro(), sendMag() and sendTemperature()
+ * signals. Otherwise it will send one of the sendQuaternion(), sendEulerAngles(), sendRotationMatrix() signal 
+ * according to the selected output type.
+ *
+ * If timestamped output is required, then a timestamp will also be added using signal sendTimestamp().
+ */
+void sendData();
+
+/*! \brief Sets the tracker in continuous mode or not.
+ * 
+ * Once start is called it will send data until it stops. 
+ * \param b Set it to <tt>true</tt> to put it in continuous mode.
+ */ 
+void setThreaded(bool b) { autoSend = b; }
+
+/*! \brief Sets the delay, in millisecondes between two data frames.
+ *
+ * If set to 0, then there will be no pause between two data frames.
+ */
+void setDelay(int i) { delay = i; }
+
+
+ signals:
+     void sendQuaternion(float*); //!< Sends sensor's orientation using quaternions (4 values).
+     void sendRotationMatrix(float*); //!< Sends sensor's orientation using rotation matrix (9 values).
+     void sendEulerAngles(float*); //!< Sends sensor's orientation using Euler angles (3 values).
+     void sendAcceleration(float*); //!< Sends accelaration vector (3 values).
+     void sendGyro(float*); //!< Sends angular turning rates (3 values)
+     void sendMag(float*); //!< Sends the direction of the magnetic North (3 values).
+     
+     void sendTemperature(float); //!< Sends the inside temperature of the sensor.
+     void sendTimeStamp(int); //!< Sends an associated timestamp to data.
+     
+     void sendStartTime(double); //!< Sends the time data started to be acquired.
+	void sendToken(QString s);
+
+     
+
+ private:
+     QTimer selfTimer;
+     bool autoSend;
+     QString fileName;
+
+     ATime startTime;
+     bool initialized;
+     std::ifstream file;
+     int delay;
+
+
+};
+
+
+
+#endif //__MTIEMU_H__

+#ifndef __MTILOGGER_H__
+#define __MTILOGGER_H__
+
+#include <iostream>
+#include <QObject>
+
+/*! \brief This class serializes output from MTiTracker to a Logger class.
+ *
+ * The behaviour of this component can be adjusted. First, it can run in latch mode
+ * (see setLatch()),
+ * where data are only saved when a trigger() is issued. Second, it can run in continuous 
+ * mode (by default). Signals are made to directly connect to a Logger class.
+ * If you intend to log both calibrated data and orientation from the inertial sensor,
+ * then you should tell it using setBoth(). 
+ *
+ * \author Jean-Yves Didier
+ * \date June, the 3rd, 2007
+ * \ingroup mti
+ */
+class MTiLogger : public QObject
+{
+     Q_OBJECT
+	  public:
+     MTiLogger(QObject* parent=0) : QObject(parent) //!< ARCS Constructor
+	  { both = false; latch=false; nolog=false;}
+
+
+     public slots:
+	  void setTimestamp(int i) //!< \see MTiTracker::sendTimestamp()
+	  { 
+	       if (nolog) return; 
+	       emit sendInt(i); 
+	  }
+     void setStartTime(double d)  //!< \see MTiTracker::sendStartTime()
+	  { 
+	       emit sendDouble(d); 
+	       emitend(); 
+	  }
+     void setQuaternion(float* f) //!< \see MTiTracker::sendQuaternion()
+	  { 
+	       if(nolog) return;
+		//std::cout<<"quaternion"<<std::endl; 
+	       for(int i=0; i < 4; i++) 
+		    emit sendFloat(f[i]); 
+	       emitend(); 
+	  }
+     void setRotationMatrix(float* f) //!< \see MTiTracker::sendRotationMatrix()
+	  { 
+	       if(nolog) return; 
+	       for (int i=0; i<9; i++)
+		    emit sendFloat(f[i]);
+	       emitend(); 
+	  }
+     void setEulerAngles(float* f) //!< \see MTiTracker::sendEulerAngles()
+	  { 
+	       if (nolog) return;
+	       for (int i=0; i < 3; i++)
+		    emit sendFloat(f[i]);
+	       emitend(); 
+	  }
+     void setAcceleration(float* f) //!< \see MTiTracker::sendAcceleration()
+	  { 
+	       if (nolog) return; 
+// 		std::cout<<"acceleration"<<std::endl;
+	       for(int i=0; i<3; i++) 
+		    emit sendFloat(f[i]);
+	  }
+     void setGyro(float* f) //!< \see MTiTracker::sendGyro()
+	  { 
+	       if (nolog) return;
+// 		std::cout<<"gyro"<<std::endl;
+	       for(int i=0; i <3; i++) 
+		    emit sendFloat(f[i]);
+	  }
+     void setMag(float* f) //!< \see MTiTracker::sendMag()
+	  { 
+	       if (nolog) return;
+// 		std::cout<<"mag"<<std::endl;
+	       for(int i=0; i<3; i++)
+		    emit sendFloat(f[i]);
+	       if (!both) emitend();
+	  }
+     /*! \brief Tells wether you want to log claibrated output and orientation from the inertial sensor.
+      * \param b Set it to <tt>true</tt> if you want to log both kind of data, <tt>false</tt> otherwise.
+      */
+     void setBoth(bool b) { both = b; }
+     /*! \brief Tells wether you want to activate latch behaviour of this component.
+      * \param b Set it to <tt>true</tt> if you want the latch behaviour, <tt>false</tt> otherwise.
+      */
+     void setLatch(bool b) { latch = b; nolog = b;}
+     /*! \brief Triggers the log of the next data
+      * 
+      * This is ignored when not in latch behaviour.
+      */
+     void trigger() { nolog = false; }
+     
+signals:
+     void sendFloat(float);  //!< \see Logger::log()
+     void sendDouble(double);//!< \see Logger::log()
+     void sendInt(int);//!< \see Logger::log()
+     void end(); //!< \see Logger::endLine()
+
+private:
+     void emitend()
+	  {
+	       emit end();
+	       nolog = latch;
+	  }
+
+     bool both;
+     bool latch;
+     bool nolog;
+
+};
+
+#endif //__MTILOGGER_H__

+#ifndef __MTITRACKER_H__
+#define __MTITRACKER_H__
+
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include "MTComm.h"
+#include <iostream>
+#include "../../include/atime.h"
+
+
+/*! \defgroup mti MTi inertial sensor related classe.
+ *
+ * The MTi is an inertial sensor that produces differents outputs according 
+ * to the user wishes. 
+ * The types of data that could be returned are :
+ * <ul>
+ * <li>Orientation related to a pre-calibrated coordinate frame,</li>
+ * <li>Linear acceleration,</li>
+ * <li>Magnetometer measures,</li>
+ * <li>Angular velocity,</li>
+ * </ul>
+ * \ingroup tracker
+ */
+
+/*! \brief A component embedding mti sensor.
+ *
+ * \author Jean-Yves Didier
+ * \date March, the 29th, 2007
+ * \ingroup mti
+ */
+class MTiTracker: public QObject
+{
+Q_OBJECT
+public:
+MTiTracker(QObject* parent=0); //!< ARCS Constructor
+~MTiTracker(); //!< ARCS Destructor
+
+public slots:
+
+void setTemperatureOutput(bool b) 
+{ 
+     outputMode = (b)? (outputMode | OUTPUTMODE_TEMP):(outputMode & ~OUTPUTMODE_TEMP);
+}
+
+void setMagneticFilter(bool b) { filterMag  = b; }
+
+
+/*! \brief Tells wether the sensor should return calibrated data or not.
+ * 
+ * Calibrated data are accelerometers, gyroscopes and magnetometer values.
+ * \param b Set it to <tt>true</tt> if you want calibrated data, <tt>false</tt> otherwise.
+ */
+void setCalibratedOutput(bool b) 
+{
+     outputMode = (b)? (outputMode | OUTPUTMODE_CALIB):(outputMode & ~OUTPUTMODE_CALIB);
+}
+/*! \brief Tells wether the sensor should return its orientation or not.
+ *
+ * \param b Set it to <tt>true</tt> if you want the sensor's orientation, <tt>false</tt> otherwise.
+ */
+void setOrientationOutput(bool b)
+{
+     outputMode = (b)? (outputMode | OUTPUTMODE_ORIENT):(outputMode & ~OUTPUTMODE_ORIENT);
+}
+
+/*! \brief Sets the sampling rate of the mti sensor.
+ *
+ * Sampling rates are varying from 10 to 512 Hz.
+ * Not every value is available. The initialization step will try to 
+ * grep the available value less or equal to the value you proposed.
+ * \param i Expected sampling rate. 
+ */
+void setSampleFrequency(int i){ frequency=i;} 
+/*! \brief Sets the baudrate of the mti sensor.
+ *
+ * Baudrates are varying from 9600 to 921600 bps.
+ * Not every value is available. The initialization step will try to 
+ * grep the available value less or equal to the value you proposed.
+ * \param i Expected baud rate. 
+ */
+void setBaudrate(int i){ baudrate=i;} 
+/*!\brief Starts the sensor in asynchronous mode.
+ *
+ * Actually does something only if setThreaded() has been called before.
+ */
+void start(); 
+/*! \brief Stops the sensor in asynchronous mode.
+ */
+void stop(); 
+/*! \brief Sends the next available data
+ * 
+ * Depending on the type of desired output, the signal emitted pattern will be different.
+ * If calibrated output is required then it will emit sendAcceleration(), sendGyro(), sendMag() and sendTemperature()
+ * signals. Otherwise it will send one of the sendQuaternion(), sendEulerAngles(), sendRotationMatrix() signal 
+ * according to the selected output type.
+ *
+ * If timestamped output is required, then a timestamp will also be added using signal sendTimestamp().
+ */
+void sendData();
+
+/*! \brief Sets the port on which to contact sensor (Win32 only)
+ *
+ * This is usually the id of the corresponding COM port.
+ * \param i id of the COM port.
+ */
+void setPort(int i) { port = i; } 
+/*!\brief Sets the device filename on which to contact sensor (Unix only)
+ *
+ * \param s filename of the sensor's device (usually something like <tt>/dev/ttyUSB0</tt>)
+ */
+void setDevice(QString s) { device = s;}
+
+/*! \brief Inits the sensor.
+ *
+ * It is a prerequired step before using start(), sendNextData(), sendLastData() or sendRemainingData().
+ */
+void init();
+/*! \brief Sets the tracker in continuous mode or not.
+ * 
+ * Once start is called it will send data until it stops. 
+ * \param b Set it to <tt>true</tt> to put it in continuous mode.
+ */ 
+void setThreaded(bool b) { autoSend = b; }
+/*! \brief Turns the quaternion output on when in orientation mode
+ */
+void setQuaternionOutput() { outputSettings |= OUTPUTSETTINGS_ORIENTMODE_QUATERNION; }
+/*! \brief Turns the Euler angles output on when in orientation mode
+ */
+void setEulerOutput() { outputSettings |= OUTPUTSETTINGS_ORIENTMODE_EULER; }
+/*!\brief Turns the matrix output on when in orientation mode.
+ */
+void setMatrixOutput() {  outputSettings |= OUTPUTSETTINGS_ORIENTMODE_MATRIX; }
+/*! \brief Turns the timestamp output on.
+ */
+void setTimeStampedOutput() { outputSettings |= OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT; }
+
+void setKeepUpdate(bool b) { keepUpdate = b; }
+
+void resetGlobalOrientation(){
+	short a = tracker.writeMessage(MID_RESETORIENTATION,RESETORIENTATION_GLOBAL);
+	if ( a == MTRV_OK)
+		std::cerr<<"Error in Reset Orientation: MTRV_OK"<<std::endl;
+	if ( a == MTRV_RECVERRORMSG )
+		std::cout<<"Error in Reset Orientation: MTRV_RECVERRORMSG"<<std::endl;
+	if ( a == MTRV_TIMEOUT)
+		std::cout<<"Error in Reset Orientation: MTRV_TIMEOUT"<<std::endl;
+	if ( a == MTRV_NOINPUTINITIALIZED) 
+		std::cout<<"Error in Reset Orientation: MTRV_NOINPUTINITIALIZED"<<std::endl;
+	};
+
+void resetObjectOrientation(){
+	if(tracker.writeMessage(MID_RESETORIENTATION,RESETORIENTATION_OBJECT) != MTRV_OK)
+		std::cerr<<"Error in Reset Orientation: OBJECT Mode"<<std::endl;
+	else
+		std::cout<<"Reset Orientation: OBJECT Mode"<<std::endl;
+}
+
+void resetHeadingOrientation(){
+	if(tracker.writeMessage(MID_RESETORIENTATION,RESETORIENTATION_HEADING) != MTRV_OK)
+		std::cerr<<"Error in Reset Orientation: HEADING Mode"<<std::endl;
+	else
+		std::cout<<"Reset Orientation: HEADING Mode"<<std::endl;
+	};
+
+void resetAlignOrientation(){
+	if(tracker.writeMessage(MID_RESETORIENTATION,RESETORIENTATION_ALIGN) != MTRV_OK)
+		std::cerr<<"Error in Reset Orientation: ALIGN Mode"<<std::endl;
+	else
+		std::cout<<"Reset Orientation: ALIGN Mode"<<std::endl;
+	};
+
+signals:
+
+void sendQuaternion(float*); //!< Sends sensor's orientation using quaternions (4 values).
+void sendRotationMatrix(float*); //!< Sends sensor's orientation using rotation matrix (9 values).
+void sendEulerAngles(float*); //!< Sends sensor's orientation using Euler angles (3 values).
+void sendAcceleration(float*); //!< Sends accelaration vector (3 values).
+void sendGyro(float*); //!< Sends angular turning rates (3 values)
+void sendMag(float*); //!< Sends the direction of the magnetic North (3 values).
+
+void sendTemperature(float); //!< Sends the inside temperature of the sensor.
+void sendTimeStamp(int); //!< Sends an associated timestamp to data expressed in number of cycles since the sensor started.
+
+void sendStartTime(double); //!< Sends the time data started to be acquired expressed in seconds since midnight.
+void initFailed(); //!< Sent when the system failed to init inertial tracker. 
+
+private:
+void getFrequency(unsigned long period); 
+void getBaudrate(unsigned int br);
+
+bool displayConfig();
+
+unsigned long computeRealFrequency();
+unsigned long computeRealBaudrate();
+
+//void sendData();
+
+CMTComm tracker;
+unsigned long outputMode;
+unsigned long outputSettings;
+unsigned char data[MAXMSGLEN];
+int frequency;
+int baudrate;
+
+int port;
+QString device;
+
+bool autoSend;
+bool initialized;
+bool filterMag;
+bool keepUpdate;
+QTimer selfTimer;
+static unsigned short frequencies[][2];
+static unsigned int baudrates[][2];
+ATime startTime;
+
+float acc[3];
+float gyr[3];
+float rot[9];
+float qat[4];
+float eul[3];
+float mag[3];
+
+};
+
+
+
+#endif //__MT9TRACKER_H__