CRaePID.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */



#include "hwdrivers-precomp.h"   // Precompiled headers
#include <mrpt/hwdrivers/CRaePID.h>
#include <mrpt/system/threads.h>
#include <mrpt/system/datetime.h>

#include <iostream>
#include <iterator>
#include <sstream>

using namespace std;
using namespace mrpt::utils;
using namespace mrpt::hwdrivers;


IMPLEMENTS_GENERIC_SENSOR(CRaePID,mrpt::hwdrivers)

/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CRaePID::CRaePID()
{
        m_sensorLabel = "RAE_PID";
}


/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void  CRaePID::loadConfig_sensorSpecific(
        const mrpt::utils::CConfigFileBase &configSource,
        const std::string                       &iniSection )
{
#ifdef MRPT_OS_WINDOWS
        com_port = configSource.read_string(iniSection, "COM_port_PID", "COM1", true ) ;
#else
        com_port = configSource.read_string(iniSection, "COM_port_PID", "/dev/tty0", true );
#endif

        com_bauds               = configSource.read_int( iniSection, "baudRate",9600, false );

        pose_x = configSource.read_float(iniSection,"pose_x",0,true);
        pose_y = configSource.read_float(iniSection,"pose_y",0,true);
        pose_z = configSource.read_float(iniSection,"pose_z",0,true);
        pose_roll = configSource.read_float(iniSection,"pose_roll",0,true);
        pose_pitch = configSource.read_float(iniSection,"pose_pitch",0,true);
        pose_yaw = configSource.read_float(iniSection,"pose_yaw",0,true);

}

/* -----------------------------------------------------
                                tryToOpenTheCOM
----------------------------------------------------- */
bool  CRaePID::tryToOpenTheCOM()
{
    if (COM.isOpen())
        return true;    // Already open

    if (m_verbose) cout << "[CRaePID] Opening " << com_port << " @ " <<com_bauds << endl;

        try
        {
        COM.open(com_port);
        // Config:
        COM.setConfig( com_bauds, 0, 8, 1 );
        //COM.setTimeouts( 1, 0, 1, 1, 1 );
                COM.setTimeouts(50,1,100, 1,20);
                //mrpt::system::sleep(10);
                COM.purgeBuffers();
                //mrpt::system::sleep(10);

                return true; // All OK!
        }
        catch (std::exception &e)
        {
                std::cerr << "[CRaePID::tryToOpenTheCOM] Error opening or configuring the serial port:" << std::endl << e.what();
        COM.close();
                return false;
        }
        catch (...)
        {
                std::cerr << "[CRaePID::tryToOpenTheCOM] Error opening or configuring the serial port." << std::endl;
                COM.close();
                return false;
        }
}

/* -----------------------------------------------------
                                doProcess
----------------------------------------------------- */
void CRaePID::doProcess()
{
        // Is the COM open?
        if (!tryToOpenTheCOM())
        {
                m_state = ssError;
                THROW_EXCEPTION("Cannot open the serial port");
        }

        bool have_reading = false;
        std::string power_reading;
        bool time_out = false;

        while (!have_reading)
        {
                // Send command to PID to request a measurement
                COM.purgeBuffers();
                COM.Write("R",1);

                // Read PID response
                power_reading = COM.ReadString(500,&time_out);
                if (time_out)
                {
                        //cout << "[CRaePID] " << com_port << " @ " <<com_bauds << " - measurement Timed-Out" << endl;
                        mrpt::system::sleep(10);
                }
                else
                        have_reading = true;
        }

        //cout << "[CRaePID] " << com_port << " @ " <<com_bauds << " - measurement -> " << power_reading << endl;

        // Convert the text to a number (ppm)
        const float readnum = atof(power_reading.c_str());
        const float val_ppm = readnum/1000;

        // Fill the observation
        mrpt::obs::CObservationGasSensors::TObservationENose obs;
        obs.readingsVoltage.push_back(val_ppm);
        obs.sensorTypes.push_back(0x0001);

        mrpt::obs::CObservationGasSensors obsG;
        obsG.sensorLabel = this->getSensorLabel();
        obsG.m_readings.push_back(obs);
        obsG.timestamp = mrpt::system::now();

        appendObservation(mrpt::obs::CObservationGasSensorsPtr(new mrpt::obs::CObservationGasSensors(obsG)));

}


std::string CRaePID::getFirmware()
{
        // Send the command
        cout << "Firmware version: " << endl;
        COM.purgeBuffers();
        size_t B_written = COM.Write("F",1);
        if (!B_written) return std::string("COMMS.ERROR");

        // Read the returned text
        bool time_out = false;
        std::string s_read = COM.ReadString(2000,&time_out);
        if (time_out)
                s_read =  "Time_out";
        return s_read;
}

std::string CRaePID::getModel()
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("M",1);

        // Read the returned text
        return COM.ReadString();
}


std::string CRaePID::getSerialNumber()
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("S",1);

        // Read the returned text
        return COM.ReadString();
}

std::string CRaePID::getName()
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("N",1);

        // Read the returned text
        return COM.ReadString();

}

bool CRaePID::switchPower()
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("P",1);

        // Read the returned text
        std::string reading;
        reading = COM.ReadString();

        if (strcmp(reading.c_str(),"Sleep...")==0)
                return true;
        else
                return false;
}

mrpt::obs::CObservationGasSensors CRaePID::getFullInfo()
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("C",1);

        // Read the returned text
        std::string reading;
        reading = COM.ReadString();

        // Iterate over each information component (tokenize first)
        // construct a stream from the string (as seen in http://stackoverflow.com/a/53921)
        std::stringstream readings_str(reading);

        // use stream iterators to copy the stream to the vector as whitespace separated strings
        std::istream_iterator<std::string> it(readings_str);
        std::istream_iterator<std::string> endit;
        std::vector<std::string> measurements_text(it, endit);

        // Convert the text to a number (ppm)
        mrpt::obs::CObservationGasSensors::TObservationENose obs;
        mrpt::obs::CObservationGasSensors obsG;

        for (size_t k=0; k < measurements_text.size(); k++)
        {
                const float readnum = atof(measurements_text[k].c_str());
                const float val_ppm = readnum/1000.f;

                // Fill the observation
                obs.readingsVoltage.push_back(val_ppm);
                obsG.m_readings.push_back(obs);
        }

        obsG.sensorLabel = this->getSensorLabel();
        obsG.timestamp = mrpt::system::now();

        return obsG;

}

bool CRaePID::errorStatus(std::string &errorString)
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("E",1);

        // Read the returned text
        std::string reading;
        reading = COM.ReadString();

        // Tokenize to separate the two components:
        // construct a stream from the string (as seen in http://stackoverflow.com/a/53921)
        std::stringstream readings_str(reading);

        // use stream iterators to copy the stream to the vector as whitespace separated strings
        std::istream_iterator<std::string> it(readings_str);
        std::istream_iterator<std::string> endit;
        std::vector<std::string> errors_text(it, endit);

        // Take the first part and check the possible error condition
        if((strcmp(errors_text[0].c_str(),"0")==0) && (strcmp(errors_text[1].c_str(),"0")==0)) // no error
        {
                return false;
        }
        else
        {
                // By the moment, return the raw error; note that if necessary a detailed description of the error can be obtained analyzing the two error strings separately
                errorString = reading;
                return true;
        }
}

void CRaePID::getLimits(float &min, float &max)
{
        // Send the command
        COM.purgeBuffers();
        COM.Write("L",1);

        // Read the returned text
        std::string reading;
        reading = COM.ReadString();

        // Tokenize to separate the two components:
        // construct a stream from the string (as seen in http://stackoverflow.com/a/53921)
        std::stringstream readings_str(reading);

        // use stream iterators to copy the stream to the vector as whitespace separated strings
        std::istream_iterator<std::string> it(readings_str);
        std::istream_iterator<std::string> endit;
        std::vector<std::string> readings_text(it, endit);

        // read min and max
        max = atof(readings_text[0].c_str());
        min = atof(readings_text[1].c_str());


}