>From 8ad6a53ee05529280cb31e2914f49c02416d56ff Mon Sep 17 00:00:00 2001
From: Tim Clephas <t.t.g.clephas@student.tue.nl>
Date: Tue, 15 Feb 2011 18:01:36 +0100
Subject: [PATCH] Fix and re-enable el5101 driver.

Restores basic implementation for reading out the raw values

Signed-off-by: Tim Clephas <t.t.g.clephas@student.tue.nl>
---
 soem_beckhoff_drivers/CMakeLists.txt      |    2 +-
 soem_beckhoff_drivers/src/soem_el5101.cpp |   46 +++++++--------
 soem_beckhoff_drivers/src/soem_el5101.h   |   87 +++++++++++++----------------
 soem_master/src/soem_master_component.cpp |    2 +-
 4 files changed, 62 insertions(+), 75 deletions(-)

diff --git a/soem_beckhoff_drivers/CMakeLists.txt b/soem_beckhoff_drivers/CMakeLists.txt
index c796045..7fd53b9 100644
--- a/soem_beckhoff_drivers/CMakeLists.txt
+++ b/soem_beckhoff_drivers/CMakeLists.txt
@@ -40,6 +40,6 @@ orocos_plugin(soem_el1xxx src/soem_beckhoff_drivers.cpp src/soem_el1xxx.cpp)
 orocos_plugin(soem_el2xxx src/soem_beckhoff_drivers.cpp src/soem_el2xxx.cpp)
 orocos_plugin(soem_el4xxx src/soem_beckhoff_drivers.cpp src/soem_el4xxx.cpp)
 orocos_plugin(soem_el3062 src/soem_beckhoff_drivers.cpp src/soem_el3062.cpp)
-#orocos_plugin(soem_el5101 src/soem_beckhoff_drivers.cpp src/soem_el5101.cpp)
+orocos_plugin(soem_el5101 src/soem_beckhoff_drivers.cpp src/soem_el5101.cpp)
 
 
diff --git a/soem_beckhoff_drivers/src/soem_el5101.cpp b/soem_beckhoff_drivers/src/soem_el5101.cpp
index 734ffbd..daf65ad 100644
--- a/soem_beckhoff_drivers/src/soem_el5101.cpp
+++ b/soem_beckhoff_drivers/src/soem_el5101.cpp
@@ -34,25 +34,20 @@ namespace soem_beckhoff_drivers
 SoemEL5101::SoemEL5101(ec_slavet* mem_loc) :
     soem_master::SoemDriver(mem_loc), values_port_(this->getName() + "_value")
 {
-    service_->doc(std::string("Services for Beckhoff ") + std::string(
-            datap_->name) + std::string(" Encoder module"));
-    service_->addOperation("read", &SoemEL5101::read, this).doc(
+    m_service->doc(std::string("Services for Beckhoff ") + std::string(
+            m_datap->name) + std::string(" Encoder module"));
+    m_service->addOperation("read", &SoemEL5101::read, this).doc(
             "Read in value of the encoder");
-    //service_->addOperation("read_out",&SoemEL5101::read_out,this).doc("Read out value of the encoder");
-    //service_->addOperation("write_out",&SoemEL5101::write_out,this).doc("Write the initial value").arg("value","Value");
-    //service_->addOperation("control",&SoemEL5101::control,this).doc("Read control of the encoder");
-    //service_->addOperation("status",&SoemEL5101::status,this).doc("Read status of the encoder");
-}
+    //m_service->addOperation("read_out",&SoemEL5101::read_out,this).doc("Read out value of the encoder");
+    //m_service->addOperation("write_out",&SoemEL5101::write_out,this).doc("Write the initial value").arg("value","Value");
+    //m_service->addOperation("control",&SoemEL5101::control,this).doc("Read control of the encoder");
+    //m_service->addOperation("status",&SoemEL5101::status,this).doc("Read status of the encoder");
 
-// void SoemEL5101::addConfigureToTaskContext(RTT::TaskContext* tc){
-//tc->ports()->addPort(values_port_).doc("Uint msg containing the values of the encoder");
-//tc->ports()->addPort(values_port_out_).doc("AnalogMsg containing the values of the encoder");
-// tc->addProperty("",);
-//}
+    //tc->ports()->addPort(values_port_).doc("Uint msg containing the values of the encoder");
+    //tc->ports()->addPort(values_port_out_).doc("AnalogMsg containing the values of the encoder");
+    // tc->addProperty("",);
 
-void SoemEL5101::addPortsToTaskContext(RTT::TaskContext* tc)
-{
-    tc->ports()->addPort(values_port_).doc(
+    m_service->addPort(values_port_).doc(
             "Uint msg containing the value of the encoder");
 
     parameter temp;
@@ -74,7 +69,7 @@ void SoemEL5101::addPortsToTaskContext(RTT::TaskContext* tc)
     {
         for (unsigned int i(0); i < params.size(); ++i) // On parcourt le tableau.
         {
-            tc->addProperty(params[i].name, params[i].param).doc(
+            m_service->addProperty(params[i].name, params[i].param).doc(
                     params[i].description);
         }
     }
@@ -94,33 +89,34 @@ void SoemEL5101::addPortsToTaskContext(RTT::TaskContext* tc)
 
 }
 
-void SoemEL5101::updatePorts()
+void SoemEL5101::update()
 {
+	// publish encoder values
     msg_.value = read();
     values_port_.write(msg_);
 }
 
-double SoemEL5101::read(void)
+uint32_t SoemEL5101::read(void)
 {
-    return ((in_el5101t*) (datap_->inputs))->invalue;
+    return ((in_el5101t*) (m_datap->inputs))->invalue;
 }
 
 /*double SoemEL5101::read_out( void){
- return ((out_el5101t*)(datap_->outputs))->outvalue;
+ return ((out_el5101t*)(m_datap->outputs))->outvalue;
  }
 
  int SoemEL5101::write_out( uint value){
- ((out_el5101t*)(datap_->outputs))->outvalue=value;
+ ((out_el5101t*)(m_datap->outputs))->outvalue=value;
  return 1;
- //return ((out_el5101t*)(datap_->outputs))->outvalue;
+ //return ((out_el5101t*)(m_datap->outputs))->outvalue;
  }
 
  unsigned int SoemEL5101::status( void){
- return ((in_el5101t*)(datap_->inputs))->status;
+ return ((in_el5101t*)(m_datap->inputs))->status;
  }
 
  unsigned int SoemEL5101::control( void){
- return ((out_el5101t*)(datap_->outputs))->control;
+ return ((out_el5101t*)(m_datap->outputs))->control;
  }
  */
 namespace
diff --git a/soem_beckhoff_drivers/src/soem_el5101.h b/soem_beckhoff_drivers/src/soem_el5101.h
index 38b7a59..f31b147 100644
--- a/soem_beckhoff_drivers/src/soem_el5101.h
+++ b/soem_beckhoff_drivers/src/soem_el5101.h
@@ -40,60 +40,51 @@ namespace soem_beckhoff_drivers
 
 class SoemEL5101: public soem_master::SoemDriver
 {
+    typedef struct PACKED {
+		uint8 control;
+		uint16 outvalue;
+	} out_el5101t;
 
-    typedef struct
-    PACKED
-    {
-            uint8 control;
-            uint16 outvalue;
-        } out_el5101t;
+	typedef struct PACKED {
+		uint8 status;
+		uint16 invalue;
+		uint16 latch;
+		uint32 frequency;
+		uint16 period;
+		uint16 window;
+	} in_el5101t;
 
-        typedef struct
-        PACKED
-        {
-                uint8 status;
-                uint16 invalue;
-                uint16 latch;
-                uint32 frequency;
-                uint16 period;
-                uint16 window;
-            } in_el5101t;
+public:
+	SoemEL5101(ec_slavet* mem_loc);
+	virtual ~SoemEL5101() {
+	}
 
-            typedef struct
-            {
-                uint16 index;
-                uint8 subindex;
-                uint8 size;
-                int param;
-                string name;
-                string description;
-            } parameter;
+	/*double read_out(void);
+	 int write_out(uint);
+	 unsigned int control(void);
+	 unsigned int status(void);*/
 
-        public:
-            SoemEL5101(ec_slavet* mem_loc);
-            ~SoemEL5101()
-            {
-            }
-            ;
+	virtual void update();
 
-            //bool write( unsigned int chan, double value );
-            double read(void);
-            /*double read_out(void);
-             int write_out(uint);
-             unsigned int control(void);
-             unsigned int status(void);*/
+	// Returns the encoder value as a double.
+	uint32_t read();
+private:
 
-            void addPortsToTaskContext(RTT::TaskContext* tc);
-            void updatePorts();
+	typedef struct {
+		uint16 index;
+		uint8 subindex;
+		uint8 size;
+		int param;
+		string name;
+		string description;
+	} parameter;
 
-        private:
+	EncoderMsg msg_;
+	RTT::OutputPort<EncoderMsg> values_port_;
+	std::vector<double> values_in_;
+	RTT::Property<std::string> propriete;
+	std::vector<parameter> params;
+};
 
-            EncoderMsg msg_;
-            std::vector<double> values_in_;
-            RTT::OutputPort<EncoderMsg> values_port_;
-            RTT::Property<std::string> propriete;
-            std::vector<parameter> params;
-        };
-
-        }
+}
 #endif
diff --git a/soem_master/src/soem_master_component.cpp b/soem_master/src/soem_master_component.cpp
index 8146b5f..5fad0da 100644
--- a/soem_master/src/soem_master_component.cpp
+++ b/soem_master/src/soem_master_component.cpp
@@ -30,7 +30,7 @@ extern "C"
 }
 
 #include <cstdio>
-#include "soem_master.h"
+//#include "soem_master.h"
 #include <soem_master/soem_driver_factory.h>
 
 #include "soem_master_component.h"
-- 
1.7.0.4

