From 87dc660bd6768724c326c92b0b721de382546f67 Mon Sep 17 00:00:00 2001
From: Adelin Dobre <adelin.dobre@rinftech.com>
Date: Thu, 12 Jul 2018 17:01:17 +0300
Subject: [PATCH] ADXRS610Gyroscope: Add string based constructor

Signed-off-by: Adelin Dobre <adelin.dobre@rinftech.com>
Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
---
 src/adxrs610/adxrs610.cxx | 58 +++++++++++++++++++++++++++++++++++----
 src/adxrs610/adxrs610.hpp | 15 ++++++++--
 2 files changed, 64 insertions(+), 9 deletions(-)

diff --git a/src/adxrs610/adxrs610.cxx b/src/adxrs610/adxrs610.cxx
index 98d78ae1..f82788eb 100644
--- a/src/adxrs610/adxrs610.cxx
+++ b/src/adxrs610/adxrs610.cxx
@@ -24,39 +24,85 @@
 
 #include <iostream>
 
+#include "upm_string_parser.hpp"
 #include "adxrs610.hpp"
 
 using namespace std;
 using namespace upm;
 
 ADXRS610::ADXRS610(int dPin, int tPin, float aref) :
-  m_aioData(dPin), m_aioTemp(tPin)
+  m_aioData(new mraa::Aio(dPin)), m_aioTemp(new mraa::Aio(tPin))
 {
   // ADC resolution of data and temp should be the same...
-  m_aRes = (1 << m_aioData.getBit());
+  m_aRes = (1 << m_aioData->getBit());
   m_aref = aref;
 
   setZeroPoint(calibrateZeroPoint());
   setDeadband(0.0);
 
   m_centerVolts = aref / 2.0;
+}
 
+ADXRS610::ADXRS610(std::string initStr) : mraaIo(new mraa::MraaIo(initStr))
+{
+  if(mraaIo == NULL)
+  {
+    throw std::invalid_argument(std::string(__FUNCTION__) +
+                            ": Failed to allocate memory for internal member");
+  }
+  if(!mraaIo->aios.empty())
+  {
+    m_aioData = &mraaIo->aios[0];
+    m_aioTemp = &mraaIo->aios[1];
+  }
+  else
+  {
+    throw std::invalid_argument(std::string(__FUNCTION__) +
+                            ": mraa_i2c_init() failed");
+  }
+
+  std::vector<std::string> upmTokens;
+
+  if (!mraaIo->getLeftoverStr().empty()) {
+    upmTokens = UpmStringParser::parse(mraaIo->getLeftoverStr());
+  }
+
+  // ADC resolution of data and temp should be the same...
+  m_aref = 5.0;
+  m_aRes = (1 << m_aioData->getBit());
+  setZeroPoint(calibrateZeroPoint());
+  setDeadband(0.0);
+
+  for (std::string tok : upmTokens) {
+    if(tok.substr(0,5) == "aref:") {
+      m_aref = std::stof(tok.substr(5));
+    }
+  }
+
+  m_centerVolts = m_aref / 2.0;
 }
 
 ADXRS610::~ADXRS610()
 {
+  if(mraaIo != NULL)
+    delete mraaIo;
+  else
+  {
+    delete m_aioData;
+    delete m_aioTemp;
+  }
 }
 
 float ADXRS610::getDataVolts()
 {
-  int val = m_aioData.read();
+  int val = m_aioData->read();
 
   return(float(val) * (m_aref / float(m_aRes)));
 }
 
 float ADXRS610::getTemperatureVolts()
 {
-  int val = m_aioTemp.read();
+  int val = m_aioTemp->read();
 
   return(float(val) * (m_aref / float(m_aRes)));
 }
@@ -75,7 +121,7 @@ float ADXRS610::calibrateZeroPoint(unsigned int samples)
 float ADXRS610::getTemperature()
 {
   float tempV = getTemperatureVolts();
-  
+
   // nominal 2.5 volts at 25C
   if (tempV > m_centerVolts)
     return (m_temperatureNom + ((tempV - m_centerVolts) / m_temperatureCoeff));
@@ -91,7 +137,7 @@ float ADXRS610::getAngularVelocity()
   if (dataV < (m_zeroPoint + m_deadband) &&
       dataV > (m_zeroPoint - m_deadband))
     return 0.0;
-  
+
   if (dataV > m_zeroPoint)
     return ((dataV - m_zeroPoint) / m_degreeCoeff);
   else
diff --git a/src/adxrs610/adxrs610.hpp b/src/adxrs610/adxrs610.hpp
index 4807eb16..9f9e299f 100644
--- a/src/adxrs610/adxrs610.hpp
+++ b/src/adxrs610/adxrs610.hpp
@@ -26,6 +26,7 @@
 #include <iostream>
 #include <string>
 #include <mraa/aio.hpp>
+#include <mraa/initio.hpp>
 
 #include <interfaces/iGyroscope.hpp>
 #include <interfaces/iTemperature.hpp>
@@ -84,6 +85,13 @@ namespace upm {
      */
     ADXRS610(int dPin, int tPin, float aref=5.0);
 
+    /**
+     * Instantiates ADXRS610 Gyroscope based on a given string.
+     *
+     * @param initStr string containing specific information for ADXRS610 initialization.
+     */
+    ADXRS610(std::string initStr);
+
     /**
      * ADXRS610 destructor
      */
@@ -128,7 +136,7 @@ namespace upm {
      * (setZeroPoint()).
      *
      * @param samples the number of samples to take an average over.
-     * The default is 50.  
+     * The default is 50.
      * @return the average of the reading over samples times.
      */
     float calibrateZeroPoint(unsigned int samples=50);
@@ -167,8 +175,9 @@ namespace upm {
     std::vector<float> getGyroscope();
 
   protected:
-    mraa::Aio m_aioData;
-    mraa::Aio m_aioTemp;
+    mraa::Aio* m_aioData = NULL;
+    mraa::Aio* m_aioTemp = NULL;
+    mraa::MraaIo* mraaIo = NULL;
 
   private:
     float m_aref;