Index: trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp	(revision 214)
@@ -22,5 +22,5 @@
 #include <SpinBox.h>
 #include <Label.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <string.h>
 
Index: trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp	(revision 214)
@@ -23,5 +23,5 @@
 #include <SpinBox.h>
 #include <Label.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <string.h>
 
@@ -96,5 +96,5 @@
 
   // on prend une fois pour toute le mutex et on fait des accès directs
-  cvmatrix *output = self->output;
+  Matrix *output = self->output;
   output->GetMutex();
   for (int i = 0; i < nb_mot; i++)
Index: trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp	(revision 214)
@@ -25,5 +25,5 @@
 #include <ComboBox.h>
 #include <PushButton.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Mutex.h>
 #include <FrameworkManager.h>
@@ -119,5 +119,5 @@
   pas->AddItem("inverse");
 
-  input = new cvmatrix((IODevice *)this, 8, 1, floatType);
+  input = new Matrix((IODevice *)this, 8, 1, floatType);
 
   cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 2);
@@ -131,5 +131,5 @@
   desc->SetElementName(2, 1, "cons avant droite");
   desc->SetElementName(3, 1, "cons arriere gauche");
-  output = new cvmatrix((IODevice *)this, desc, floatType);
+  output = new Matrix((IODevice *)this, desc, floatType);
   delete desc;
   
Index: trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.h	(revision 214)
@@ -24,5 +24,5 @@
 namespace flair {
 namespace core {
-class cvmatrix;
+class Matrix;
 class I2cPort;
 }
@@ -97,6 +97,6 @@
 
   // matrix
-  core::cvmatrix *input;
-  core::cvmatrix *output;
+  core::Matrix *input;
+  core::Matrix *output;
 
   int tested_motor;
Index: trunk/lib/FlairSensorActuator/src/Bldc.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Bldc.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/Bldc.cpp	(revision 214)
@@ -18,5 +18,5 @@
 #include "Bldc.h"
 #include "Bldc_impl.h"
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <DoubleSpinBox.h>
 #include <sstream>
@@ -53,5 +53,5 @@
   }
 
-  output = new cvmatrix(this, desc, floatType);
+  output = new Matrix(this, desc, floatType);
   delete desc;
   AddDataToLog(output);
@@ -81,5 +81,5 @@
 uint8_t Bldc::MotorsCount(void) const { return pimpl_->motors_count; }
 
-cvmatrix *Bldc::Output(void) const { return output; }
+Matrix *Bldc::Output(void) const { return output; }
 
 bool Bldc::AreEnabled(void) const { return pimpl_->are_enabled; }
Index: trunk/lib/FlairSensorActuator/src/Bldc.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/Bldc.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/Bldc.h	(revision 214)
@@ -19,5 +19,5 @@
 namespace flair {
 namespace core {
-class cvmatrix;
+class Matrix;
 }
 namespace gui {
@@ -96,5 +96,5 @@
   *
   */
-  core::cvmatrix *Output(void) const;
+  core::Matrix *Output(void) const;
 
   /*!
@@ -161,5 +161,5 @@
 
 protected:
-  core::cvmatrix *output;
+  core::Matrix *output;
 
 private:
Index: trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp	(revision 214)
@@ -20,5 +20,5 @@
 #include <GroupBox.h>
 #include <DoubleSpinBox.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Label.h>
 #include <PushButton.h>
@@ -110,5 +110,5 @@
 
 void Bldc_impl::UpdateFrom(const io_data *data) {
-  cvmatrix *input = (cvmatrix *)data;
+  Matrix *input = (Matrix *)data;
   bool is_motor_running = false;
 
Index: trunk/lib/FlairSensorActuator/src/Camera.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Camera.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/Camera.cpp	(revision 214)
@@ -119,5 +119,12 @@
 		if(logFormat==LogFormat::JPG) {
 			data->GetMutex();
-			IplImage *img=((cvimage*)data)->img;
+      //IplImage *img=((cvimage*)data)->img;
+      const cvimage* input = dynamic_cast<const cvimage*>(data);
+      if (!input) {
+          Warn("casting %s to cvimage failed\n",data->ObjectName().c_str());
+          return;
+      }
+			IplImage *img=input->img;
+     
 			string filename=getFrameworkManager()->GetLogPath()+"/"+ObjectName()+"_"+std::to_string(data->DataTime())+".jpg";
 			switch(((cvimage*)data)->GetDataType().GetFormat()) {
Index: trunk/lib/FlairSensorActuator/src/EmulatedController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/EmulatedController.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/EmulatedController.cpp	(revision 214)
@@ -17,5 +17,5 @@
 #include <cstring>
 #include <string>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <stdexcept>
 
@@ -46,5 +46,5 @@
 }
 
-template<typename T> void EmulatedController::fillVectorNoMutex(cvmatrix &vector,T data[],unsigned int size) {
+template<typename T> void EmulatedController::fillVectorNoMutex(Matrix &vector,T data[],unsigned int size) {
     for (unsigned int i=0; i<size; i++) {
         vector.SetValueNoMutex(i,0,data[i]);
@@ -52,5 +52,5 @@
 }
 
-void EmulatedController::fillVectorNoMutex(cvmatrix &destination,cvmatrix &source,unsigned int size) {
+void EmulatedController::fillVectorNoMutex(Matrix &destination,Matrix &source,unsigned int size) {
     for (unsigned int i=0; i<size; i++) {
         destination.SetValueNoMutex(i,0,source.Value(i,0));
@@ -69,5 +69,5 @@
 
 void EmulatedController::AddStep(unsigned int durationMs,string description,uint16_t buttonPressed, float leftAxisX, float leftAxisY, float rightAxisX, float rightAxisY) {
-    cvmatrix *axisMatrix=new cvmatrix((IODevice *)this,4,1,floatType);
+    Matrix *axisMatrix=new Matrix((IODevice *)this,4,1,floatType);
     axisMatrix->SetValueNoMutex(0,0,leftAxisX);
     axisMatrix->SetValueNoMutex(1,0,leftAxisY);
@@ -75,5 +75,5 @@
     axisMatrix->SetValueNoMutex(3,0,rightAxisY);
 
-    cvmatrix *buttonMatrix=new cvmatrix((IODevice *)this,16,1,SignedIntegerType(8));
+    Matrix *buttonMatrix=new Matrix((IODevice *)this,16,1,SignedIntegerType(8));
     if (buttonPressed&(uint16_t)ButtonType::start) buttonMatrix->SetValueNoMutex(0,0,1);
     if (buttonPressed&(uint16_t)ButtonType::select) buttonMatrix->SetValueNoMutex(1,0,1);
@@ -98,5 +98,5 @@
 }
 
-void EmulatedController::ComputeControllerData(DataType dataType, cvmatrix &data) {
+void EmulatedController::ComputeControllerData(DataType dataType, Matrix &data) {
     static Time startStepTime=GetTime();
 
@@ -130,9 +130,9 @@
 }
 
-void EmulatedController::AcquireAxisData(core::cvmatrix &axis) {
+void EmulatedController::AcquireAxisData(core::Matrix &axis) {
     ComputeControllerData(DataType::axis,axis);
 }
 
-void EmulatedController::AcquireButtonData(core::cvmatrix &button) {
+void EmulatedController::AcquireButtonData(core::Matrix &button) {
     ComputeControllerData(DataType::button,button);
 }
Index: trunk/lib/FlairSensorActuator/src/EmulatedController.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/EmulatedController.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/EmulatedController.h	(revision 214)
@@ -21,5 +21,5 @@
 namespace flair {
     namespace core {
-        class cvmatrix;
+        class Matrix;
     }
     namespace gui {
@@ -52,17 +52,17 @@
         bool ProcessMessage(core::Message *msg);
         bool IsDataFrameReady();
-        void AcquireAxisData(core::cvmatrix &axis); //responsible for getting the axis data from the hardware
-        void AcquireButtonData(core::cvmatrix &button); //responsible for getting the button data from the hardware
+        void AcquireAxisData(core::Matrix &axis); //responsible for getting the axis data from the hardware
+        void AcquireButtonData(core::Matrix &button); //responsible for getting the button data from the hardware
         bool ControllerInitialization();
 
     private:
         enum class DataType { axis,button };
-        void ComputeControllerData(DataType dataType, core::cvmatrix &data);
-        template<typename T> void fillVectorNoMutex(core::cvmatrix &vector,T data[],unsigned int size);
-        void fillVectorNoMutex(core::cvmatrix &destination,core::cvmatrix &source,unsigned int size);
+        void ComputeControllerData(DataType dataType, core::Matrix &data);
+        template<typename T> void fillVectorNoMutex(core::Matrix &vector,T data[],unsigned int size);
+        void fillVectorNoMutex(core::Matrix &destination,core::Matrix &source,unsigned int size);
         struct StepData {
             unsigned int durationMs; //milliseconds
-            core::cvmatrix *axisData;
-            core::cvmatrix *buttonData;
+            core::Matrix *axisData;
+            core::Matrix *buttonData;
             std::string description;
             void Print();
Index: trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp	(revision 214)
@@ -21,5 +21,5 @@
 #include <FrameworkManager.h>
 #include <RangeFinderPlot.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Tab.h>
 #include <sstream>
@@ -40,5 +40,5 @@
   main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
   cvmatrix_descriptor *desc = new cvmatrix_descriptor(1081, 1);
-  output = new cvmatrix((IODevice *)this, desc, SignedIntegerType(16));
+  output = new Matrix((IODevice *)this, desc, SignedIntegerType(16));
   delete desc;
 
@@ -246,5 +246,5 @@
 #endif
 }
-cvmatrix *HokuyoUTM30Lx::getDatas() { return output; }
+Matrix *HokuyoUTM30Lx::getDatas() { return output; }
 
 void HokuyoUTM30Lx::UseDefaultPlot(void) {
Index: trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.h	(revision 214)
@@ -23,5 +23,5 @@
 namespace flair {
 namespace core {
-class cvmatrix;
+class Matrix;
 class SerialPort;
 class Mutex;
@@ -56,5 +56,5 @@
   void getMesure(int startStep, int endStep, int clusterCount, int interval,
                  int scanNumber = 0);
-  core::cvmatrix *getDatas(void);
+  core::Matrix *getDatas(void);
 
   /*!
@@ -78,5 +78,5 @@
 
   // matrix
-  core::cvmatrix *output;
+  core::Matrix *output;
 
   std::queue<std::string> bufRet;
Index: trunk/lib/FlairSensorActuator/src/HostEthController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/HostEthController.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/HostEthController.cpp	(revision 214)
@@ -18,5 +18,5 @@
 #include "HostEthController.h"
 #include <Controller.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Tab.h>
 #include <TabWidget.h>
Index: trunk/lib/FlairSensorActuator/src/HostEthController.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/HostEthController.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/HostEthController.h	(revision 214)
@@ -26,5 +26,5 @@
 namespace flair {
 namespace core {
-class cvmatrix;
+class Matrix;
 class TcpSocket;
 class UdpSocket;
@@ -81,5 +81,5 @@
   GetAxisData() = 0; // responsible for getting the axis data from the hardware
   unsigned int axisNumber;
-  core::cvmatrix *axis;
+  core::Matrix *axis;
   gui::DataPlot1D **axisPlot;
   uint32_t bitsPerAxis;
@@ -90,5 +90,5 @@
                                     // from the hardware
   unsigned int buttonNumber;
-  core::cvmatrix *button;
+  core::Matrix *button;
   uint8_t buttonOffset;
   bool meaningfulDataAvailable;
Index: trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/LaserRangeFinder.cpp	(revision 214)
@@ -17,5 +17,5 @@
 
 #include "LaserRangeFinder.h"
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Tab.h>
 #include <TabWidget.h>
@@ -38,5 +38,5 @@
     : IODevice(getFrameworkManager(), name) {
   cvmatrix_descriptor *desc = new cvmatrix_descriptor(360, 1);
-  output = new cvmatrix(this, desc, floatType);
+  output = new Matrix(this, desc, floatType);
   delete desc;
   AddDataToLog(output);
Index: trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/LaserRangeFinder.h	(revision 214)
@@ -19,5 +19,5 @@
 namespace core {
 class FrameworkManager;
-class cvmatrix;
+class Matrix;
 }
 namespace gui {
@@ -102,5 +102,5 @@
       * \return output matrix
       */
-  core::cvmatrix *output;
+  core::Matrix *output;
 
   /*!
Index: trunk/lib/FlairSensorActuator/src/NmeaGps.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/NmeaGps.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/NmeaGps.cpp	(revision 214)
@@ -176,20 +176,15 @@
 
   int result;
-Printf("%s\n",frame);
-Printf("%x %x\n",&parser,parser.buffer);
   result = nmea_parse(&parser, frame, frame_size, &info);
-  Printf("%x %x\n",&parser,parser.buffer);
+  
   if (result != 1) {
     Warn("unrecognized nmea sentence: %s\n",frame);
     return;
   }
-Printf("3\n");
-Printf("%x %x\n",&parser,parser.buffer);
+
   result = nmea_parse_GPGGA(frame, frame_size, &pack);
-Printf("%x %x\n",&parser,parser.buffer);
+
   if (result == 1) {
-    Printf("%x %x\n",&parser,parser.buffer);
     nmea_info2pos(&info, &pos);
-     Printf("%x %x\n",&parser,parser.buffer);
    //  Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
 //Printf("lat:%f long:%f\n",pos.lat,pos.lon);
Index: trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/RadioReceiver.cpp	(revision 214)
@@ -20,5 +20,5 @@
 #include <TabWidget.h>
 #include <FrameworkManager.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <sstream>
 
@@ -44,5 +44,5 @@
     desc->SetElementName(i, 0, channel_name.str());
   }
-  output = new cvmatrix(this, desc, floatType, name);
+  output = new Matrix(this, desc, floatType, name);
   delete desc;
 
Index: trunk/lib/FlairSensorActuator/src/RadioReceiver.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/RadioReceiver.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/RadioReceiver.h	(revision 214)
@@ -19,5 +19,5 @@
 namespace flair {
 namespace core {
-class cvmatrix;
+class Matrix;
 }
 namespace gui {
@@ -87,5 +87,5 @@
   void UpdateFrom(const core::io_data *data){};
 
-  core::cvmatrix *output;
+  core::Matrix *output;
   bool is_connected;
   unsigned int nb_channels;
Index: trunk/lib/FlairSensorActuator/src/SimuBldc.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuBldc.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/SimuBldc.cpp	(revision 214)
@@ -21,6 +21,7 @@
 #include <GroupBox.h>
 #include <SharedMem.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <sstream>
+#include <string.h>
 
 using std::string;
@@ -36,9 +37,14 @@
     : Bldc(parent, layout, name, motors_count) {
   shmem =
-      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float));
+      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float)+sizeof(Time));
 
   GroupBox *groupbox = new GroupBox(layout->NewRow(), "simubldc");
   k = new DoubleSpinBox(groupbox->NewRow(), "k driver:", 0, 10000, 1);
   
+  buf=(char*)malloc(motors_count * sizeof(float)+sizeof(Time));
+  if(buf==NULL) {
+    Err("error creating buffer\n");
+    return;
+  }
   SetIsReady(true);
 }
@@ -48,17 +54,26 @@
     : Bldc(parent, name, motors_count) {
   shmem =
-      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float));
+      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float)+sizeof(Time));
+      
+  buf=(char*)malloc(motors_count * sizeof(float)+sizeof(Time));
+  if(buf==NULL) {
+    Err("error creating buffer\n");
+    return;
+  } 
+  
+  // reset values
+  float *values=(float*)buf;
+  for (int i = 0; i < motors_count; i++) values[i] = 0;
+  Time time=GetTime();
+  memcpy(buf+motors_count * sizeof(float),&time,sizeof(Time));
 
-  // reset values
-  float values[motors_count];
-  for (int i = 0; i < motors_count; i++)
-    values[i] = 0;
-
-  shmem->Write((char *)&values, motors_count * sizeof(float));
+  shmem->Write(buf, motors_count * sizeof(float)+sizeof(Time));
   
   SetIsReady(true);
 }
 
-SimuBldc::~SimuBldc() {}
+SimuBldc::~SimuBldc() {
+  if(buf!=NULL) free(buf);
+}
 
 string SimuBldc::ShMemName(uint32_t modelId,uint32_t deviceId) {
@@ -69,24 +84,27 @@
 
 void SimuBldc::SetMotors(float *value) {
-  float values[MotorsCount()];
+  float *values=(float*)buf;
+  for (int i = 0; i < MotorsCount(); i++) values[i] = k->Value() * value[i];
+  Time time=GetTime();
+  memcpy(buf+MotorsCount() * sizeof(float),&time,sizeof(Time));
 
-  for (int i = 0; i < MotorsCount(); i++)
-    values[i] = k->Value() * value[i];
-
-  shmem->Write((char *)&values, MotorsCount() * sizeof(float));
+  shmem->Write(buf, MotorsCount() * sizeof(float)+sizeof(Time));
 
   // on prend une fois pour toute le mutex et on fait des accès directs
   output->GetMutex();
-  for (int i = 0; i < MotorsCount(); i++)
+  for (int i = 0; i < MotorsCount(); i++) {
     output->SetValueNoMutex(i, 0, values[i]);
+  }
   output->ReleaseMutex();
 }
 
-void SimuBldc::GetSpeeds(float *value) const {
-  float values[MotorsCount()];
-  shmem->Read((char *)&values, MotorsCount() * sizeof(float));
+void SimuBldc::GetSpeeds(float *value,Time* time) const {
+  float *values=(float*)buf;
+  shmem->Read(buf, MotorsCount() * sizeof(float)+sizeof(Time));
+  memcpy(time,buf+MotorsCount() * sizeof(float),sizeof(Time));
 
-  for (int i = 0; i < MotorsCount(); i++)
+  for (int i = 0; i < MotorsCount(); i++) {
     value[i] = values[i];
+  }
 }
 
Index: trunk/lib/FlairSensorActuator/src/SimuBldc.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuBldc.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/SimuBldc.h	(revision 214)
@@ -20,5 +20,5 @@
 class SharedMem;
 class IODevice;
-class cvmatrix;
+class Matrix;
 }
 namespace gui {
@@ -78,6 +78,7 @@
   *
   * \param value array to store motors speeds
+  * \param time time when shared memory was written
   */
-  void GetSpeeds(float *value) const;
+  void GetSpeeds(float *value,core::Time* time) const;
 
   /*!
@@ -113,4 +114,6 @@
   core::SharedMem *shmem;
   gui::DoubleSpinBox *k;
+  char *buf;
+
 };
 } // end namespace actuator
Index: trunk/lib/FlairSensorActuator/src/SimuGps.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuGps.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/SimuGps.cpp	(revision 214)
@@ -25,5 +25,5 @@
 #include <GroupBox.h>
 #include <Euler.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <sstream>
 #include "geodesie.h"
@@ -80,5 +80,5 @@
 void SimuGps::UpdateFrom(const io_data *data) {
   if (data != NULL) {
-    cvmatrix *input = (cvmatrix *)data;
+    Matrix *input = (Matrix *)data;
     gps_states_t state;
 
@@ -109,4 +109,6 @@
   vtg.spk_k='K';
   vtg.spn_n='N';
+  gga.elv_units='M';
+  gga.diff_units='M';
 
   if (dataRate == NULL) {
@@ -152,13 +154,11 @@
     gga.satinuse=numberOfSatellites->Value();
 
-    nmea_gen_GPGGA(buf,sizeof(buf),&gga);
-    Printf("1\n");
-    parseFrame(buf,sizeof(buf));
-
+    int size=nmea_gen_GPGGA(buf,sizeof(buf),&gga);
+    parseFrame(buf,size);
+    
     vtg.dir=90-Euler::ToDegree(atan2f(state.vn,state.ve));
     vtg.spk=sqrtf(state.ve*state.ve+state.vn*state.vn)*3600./1000.;
-    nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
-    Printf("2\n");
-    parseFrame(buf,sizeof(buf));
+    size=nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
+    parseFrame(buf,size);
   }
 
Index: trunk/lib/FlairSensorActuator/src/SimuImu.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/SimuImu.cpp	(revision 214)
@@ -21,5 +21,5 @@
 #include <SpinBox.h>
 #include <GroupBox.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <SharedMem.h>
 #include <AhrsData.h>
@@ -70,5 +70,5 @@
 void SimuImu::UpdateFrom(const io_data *data) {
   if (data != NULL) {
-    cvmatrix *input = (cvmatrix *)data;
+    Matrix *input = (Matrix *)data;
     imu_states_t state;
 
Index: trunk/lib/FlairSensorActuator/src/SimuLaser.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuLaser.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/SimuLaser.cpp	(revision 214)
@@ -20,5 +20,5 @@
 #include <SpinBox.h>
 #include <GroupBox.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <SharedMem.h>
 #include <sstream>
Index: trunk/lib/FlairSensorActuator/src/SimuUs.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/SimuUs.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/SimuUs.cpp	(revision 214)
@@ -20,5 +20,5 @@
 #include <SpinBox.h>
 #include <GroupBox.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <SharedMem.h>
 #include <sstream>
Index: trunk/lib/FlairSensorActuator/src/Srf08.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/Srf08.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/Srf08.cpp	(revision 214)
@@ -21,5 +21,5 @@
 #include <GroupBox.h>
 #include <SpinBox.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <string.h>
 #include <errno.h>
Index: trunk/lib/FlairSensorActuator/src/TargetController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/TargetController.cpp	(revision 214)
@@ -20,5 +20,5 @@
 #include <Tab.h>
 #include <FrameworkManager.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 
 #include <cstring>
@@ -122,7 +122,7 @@
   } else {
 
-    axis = new cvmatrix((IODevice *)this, axisNumber, 1, floatType);
+    axis = new Matrix((IODevice *)this, axisNumber, 1, floatType);
     button =
-        new cvmatrix((IODevice *)this, buttonNumber, 1, SignedIntegerType(8));
+        new Matrix((IODevice *)this, buttonNumber, 1, SignedIntegerType(8));
 
     while (!ToBeStopped()) {
Index: trunk/lib/FlairSensorActuator/src/TargetController.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetController.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/TargetController.h	(revision 214)
@@ -27,5 +27,5 @@
 namespace core {
 class FrameworkManager;
-class cvmatrix;
+class Matrix;
 class Socket;
 class io_data;
@@ -82,6 +82,6 @@
   // axis stuff
   unsigned int axisNumber;
-  core::cvmatrix *axis = NULL;
-  virtual void AcquireAxisData(core::cvmatrix &axis) = 0; // responsible for
+  core::Matrix *axis = NULL;
+  virtual void AcquireAxisData(core::Matrix &axis) = 0; // responsible for
                                                           // getting the axis
                                                           // data from the
@@ -90,6 +90,6 @@
   // button stuff
   unsigned int buttonNumber;
-  core::cvmatrix *button = NULL;
-  virtual void AcquireButtonData(core::cvmatrix &button) = 0; // responsible for
+  core::Matrix *button = NULL;
+  virtual void AcquireButtonData(core::Matrix &button) = 0; // responsible for
                                                               // getting the
                                                               // button data
Index: trunk/lib/FlairSensorActuator/src/TargetEthController.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetEthController.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/TargetEthController.cpp	(revision 214)
@@ -24,5 +24,5 @@
 #include <cstring>
 #include <string>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <stdexcept>
 
@@ -150,5 +150,5 @@
 }
 
-void TargetEthController::AcquireAxisData(core::cvmatrix &axis) {
+void TargetEthController::AcquireAxisData(core::Matrix &axis) {
   axis.GetMutex();
   // char testFrameBuffer[3]={(char)0x09,(char)0x59,(char)0xB8};
@@ -166,5 +166,5 @@
 }
 
-void TargetEthController::AcquireButtonData(core::cvmatrix &button) {
+void TargetEthController::AcquireButtonData(core::Matrix &button) {
   uint8_t buttonValue;
   int currentButton = 0;
Index: trunk/lib/FlairSensorActuator/src/TargetEthController.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/TargetEthController.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/TargetEthController.h	(revision 214)
@@ -27,5 +27,5 @@
 namespace core {
 class FrameworkManager;
-class cvmatrix;
+class Matrix;
 class TcpSocket;
 class UdpSocket;
@@ -62,7 +62,7 @@
 
   bool IsDataFrameReady();
-  void AcquireAxisData(core::cvmatrix &axis);     // responsible for getting the
+  void AcquireAxisData(core::Matrix &axis);     // responsible for getting the
                                                   // axis data from the hardware
-  void AcquireButtonData(core::cvmatrix &button); // responsible for getting the
+  void AcquireButtonData(core::Matrix &button); // responsible for getting the
                                                   // button data from the
                                                   // hardware
Index: trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/UsRangeFinder.cpp	(revision 214)
@@ -23,5 +23,5 @@
 #include <Layout.h>
 #include <DataPlot1D.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 
 using std::string;
@@ -38,5 +38,5 @@
   cvmatrix_descriptor *desc = new cvmatrix_descriptor(1, 1);
   desc->SetElementName(0, 0, name);
-  output = new cvmatrix(this, desc, floatType);
+  output = new Matrix(this, desc, floatType);
   delete desc;
   AddDataToLog(output);
Index: trunk/lib/FlairSensorActuator/src/UsRangeFinder.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/UsRangeFinder.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/UsRangeFinder.h	(revision 214)
@@ -17,14 +17,14 @@
 
 namespace flair {
-namespace core {
-class cvmatrix;
-}
-namespace gui {
-class Tab;
-class TabWidget;
-class GroupBox;
-class Layout;
-class DataPlot1D;
-}
+  namespace core {
+    class Matrix;
+  }
+  namespace gui {
+    class Tab;
+    class TabWidget;
+    class GroupBox;
+    class Layout;
+    class DataPlot1D;
+  }
 }
 
@@ -119,5 +119,5 @@
   * \return output matrix
   */
-  core::cvmatrix *output;
+  core::Matrix *output;
 
   /*!
Index: trunk/lib/FlairSensorActuator/src/VrpnObject.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/VrpnObject.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/VrpnObject.cpp	(revision 214)
@@ -20,6 +20,5 @@
 #include "VrpnClient.h"
 #include <string.h>
-
-#include <cvmatrix.h>
+#include <Matrix.h>
 
 using std::string;
@@ -53,7 +52,7 @@
 }
 
-cvmatrix *VrpnObject::Output(void) const { return pimpl_->output; }
+Matrix *VrpnObject::Output(void) const { return pimpl_->output; }
 
-cvmatrix *VrpnObject::State(void) const { return pimpl_->state; }
+Matrix *VrpnObject::State(void) const { return pimpl_->state; }
 
 Tab *VrpnObject::GetPlotTab(void) const { return pimpl_->plot_tab; }
Index: trunk/lib/FlairSensorActuator/src/VrpnObject.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/VrpnObject.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/VrpnObject.h	(revision 214)
@@ -21,5 +21,5 @@
 namespace flair {
 	namespace core {
-		class cvmatrix;
+		class Matrix;
 		class Quaternion;
 	}
@@ -126,5 +126,5 @@
   * \return Output matrix
   */
-  core::cvmatrix *Output(void) const;
+  core::Matrix *Output(void) const;
 
   /*!
@@ -139,5 +139,5 @@
   * \return State matrix
   */
-  core::cvmatrix *State(void) const;
+  core::Matrix *State(void) const;
 
   /*!
Index: trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp	(revision 214)
@@ -25,5 +25,5 @@
 #include <unistd.h>
 #include <vrpn_Connection.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Tab.h>
 #include <TabWidget.h>
@@ -65,5 +65,5 @@
   desc->SetElementName(5, 0, "y");
   desc->SetElementName(6, 0, "z");
-  output = new cvmatrix(self, desc, floatType);
+  output = new Matrix(self, desc, floatType);
   delete desc;
 
@@ -72,5 +72,5 @@
   desc->SetElementName(1, 0, "pitch");
   desc->SetElementName(2, 0, "yaw");
-  state = new cvmatrix(self, desc, floatType);
+  state = new Matrix(self, desc, floatType);
   delete desc;
 
Index: trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp	(revision 214)
@@ -19,5 +19,5 @@
 #include "XBldc.h"
 #include <I2cPort.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <string.h>
 
Index: trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h	(revision 206)
+++ trunk/lib/FlairSensorActuator/src/unexported/VrpnObject_impl.h	(revision 214)
@@ -28,5 +28,5 @@
 namespace flair {
 	namespace core {
-		class cvmatrix;
+		class Matrix;
 	}
 	namespace gui {
@@ -58,5 +58,5 @@
   flair::gui::DataPlot1D *y_plot;
   flair::gui::DataPlot1D *z_plot;
-  flair::core::cvmatrix *output, *state;
+  flair::core::Matrix *output, *state;
 
   static void VRPN_CALLBACK handle_pos(void *userdata, const vrpn_TRACKERCB t);
