From a174b979aff9e08fd6117e58a776ace7c41db08f Mon Sep 17 00:00:00 2001
From: Johan Robben <jrobben@induct.be>
Date: Mon, 27 Apr 2015 16:12:57 +0200
Subject: [PATCH] soem_beckhoff_drivers: Add support for EL4022

Signed-off-by: Johan Robben <jrobben@octinion.com>
---
 soem_beckhoff_drivers/src/soem_el4xxx.cpp |  6 +++++
 soem_beckhoff_drivers/src/soem_el4xxx.h   | 28 +++++++++++++++++------
 2 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/soem_beckhoff_drivers/src/soem_el4xxx.cpp b/soem_beckhoff_drivers/src/soem_el4xxx.cpp
index cfc406b..3668087 100644
--- a/soem_beckhoff_drivers/src/soem_el4xxx.cpp
+++ b/soem_beckhoff_drivers/src/soem_el4xxx.cpp
@@ -45,6 +45,10 @@ soem_master::SoemDriver* createSoemEL4008(ec_slavet* mem_loc)
 {
     return new SoemEL4xxx<8> (mem_loc, 32768, 0.0, 10.0);
 }
+soem_master::SoemDriver* createSoemEL4022(ec_slavet* mem_loc)
+{
+    return new SoemEL4xxx<2> (mem_loc, 32768, 4., 20.);
+}
 soem_master::SoemDriver* createSoemEL4032(ec_slavet* mem_loc)
 {
     return new SoemEL4xxx<2> (mem_loc, 65536, -10.0, 10.0);
@@ -62,9 +66,11 @@ soem_master::SoemDriver* createSoemEL4134(ec_slavet* mem_loc) // Added by Bert
     return new SoemEL4xxx<4> (mem_loc, 65536, -10.0, 10.0);
 }
 
+
 REGISTER_SOEM_DRIVER(EL4002, createSoemEL4002)
 REGISTER_SOEM_DRIVER(EL4004, createSoemEL4004)
 REGISTER_SOEM_DRIVER(EL4008, createSoemEL4008)
+REGISTER_SOEM_DRIVER(EL4022, createSoemEL4022)
 REGISTER_SOEM_DRIVER(EL4032, createSoemEL4032)
 REGISTER_SOEM_DRIVER(EL4034, createSoemEL4034)
 REGISTER_SOEM_DRIVER(EL4038, createSoemEL4038)
diff --git a/soem_beckhoff_drivers/src/soem_el4xxx.h b/soem_beckhoff_drivers/src/soem_el4xxx.h
index c300b99..2602a05 100644
--- a/soem_beckhoff_drivers/src/soem_el4xxx.h
+++ b/soem_beckhoff_drivers/src/soem_el4xxx.h
@@ -50,8 +50,10 @@ class SoemEL4xxx: public soem_master::SoemDriver
     const unsigned int m_raw_range;
     const double m_lowest;
     const double m_highest;
+    double m_offset;
     double m_resolution;
 
+
     AnalogMsg m_msg;
     AnalogMsg m_raw_msg;
     std::vector<double> m_values;
@@ -66,7 +68,7 @@ class SoemEL4xxx: public soem_master::SoemDriver
 public:
     SoemEL4xxx(ec_slavet* mem_loc, int range, double lowest, double highest) :
         soem_master::SoemDriver(mem_loc), m_size(N), m_raw_range(range),
-                m_lowest(lowest), m_highest(highest), m_values(m_size),
+                m_lowest(lowest), m_highest(highest), m_offset(0.), m_values(m_size),
                 m_raw_values(m_size), prop_enable_user_scale(false),
                 prop_offset(0), prop_gain(1.0)
     {
@@ -87,8 +89,10 @@ class SoemEL4xxx: public soem_master::SoemDriver
 
 	m_service->addOperation("configure_channel",&SoemEL4xxx::configure_channel,this,RTT::OwnThread).doc("Configure offset and gain of channel i").arg("i","channel").arg("offset","offset").arg("gain","gain");
 
+        m_offset = (m_lowest > 0) ? m_lowest : 0.;
         m_resolution = ((m_highest - m_lowest) / (double) m_raw_range);
 
+
         m_service->addConstant("raw_range", m_raw_range);
         m_service->addConstant("resolution", m_resolution);
         m_service->addConstant("lowest", m_lowest);
@@ -117,7 +121,7 @@ class SoemEL4xxx: public soem_master::SoemDriver
     {
 	return true;
     }
-    
+
     bool configure_channel(unsigned int channel, double offset, double gain){
 	if(channel<m_size){
 	    bool enable_user_scale = true;
@@ -137,6 +141,16 @@ class SoemEL4xxx: public soem_master::SoemDriver
 	return false;
     }
 
+    double convert_to_raw(double value)
+    {
+        return (value - m_offset)/m_resolution;
+    }
+
+    double convert_from_raw(double value)
+    {
+        return m_offset + value*m_resolution;
+    }
+
     void update()
     {
 
@@ -153,11 +167,13 @@ class SoemEL4xxx: public soem_master::SoemDriver
             if (port_values.read(m_msg) == RTT::NewData)
                 if (m_msg.values.size() == m_size)
                     for (unsigned int i = 0; i < m_size; i++)
-                        m_raw_values[i] = m_msg.values[i] / m_resolution;
+                        m_raw_values[i] = convert_to_raw(m_msg.values[i]);
         }
         for (unsigned int i = 0; i < m_size; i++)
+        {
             ((out_el4xxxt*) (m_datap->outputs))->values[i]
                     = ((int) (m_raw_values[i]));
+        }
     }
 
     bool rawWrite(unsigned int chan, int value)
@@ -186,7 +202,7 @@ class SoemEL4xxx: public soem_master::SoemDriver
     {
         if (chan < m_size)
         {
-            m_raw_values[chan] = value / m_resolution;
+            m_raw_values[chan] = convert_to_raw(value);
             return true;
         }
         else
@@ -198,15 +214,13 @@ class SoemEL4xxx: public soem_master::SoemDriver
     double read(unsigned int chan)
     {
         if (chan < m_size)
-            return m_raw_values[chan] * m_resolution;
+            return convert_from_raw(m_raw_values[chan]);
         else
             log(Error) << "Channel " << chan << " is out of the module's range"
                     << endlog();
         return -1;
     }
 
-
-
 };
 
 }