NodeGuard.hpp

/***************************************************************************
  tag: Peter Soetens  Mon Jan 19 14:11:20 CET 2004  NodeGuard.hpp

                        NodeGuard.hpp -  description
                           -------------------
    begin                : Mon January 19 2004
    copyright            : (C) 2004 Peter Soetens
    email                : peter.soetens@mech.kuleuven.ac.be

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#ifndef ORO_NODEGUARD_HPP
#define ORO_NODEGUARD_HPP

#include "CAN.hpp"
#include <rtt/RunnableInterface.hpp>
#include "CANConfigurator.hpp"
#include "CANMessage.hpp"
#include "CANOpenBus.hpp"

namespace RTT
{namespace CAN
{

    class NodeGuard
        : public RunnableInterface ,
          public CANListenerInterface
    {
        CANOpenBus* bus;
        unsigned int nodeId;
        CANMessage* rtr;
        unsigned int toggle;
        bool toggle_ok;
        char status;
        bool cres;
        CANConfigurator config;
    public:

        NodeGuard( CANOpenBus* _bus, CANDeviceInterface* node )
            : bus(_bus), nodeId( node->nodeId() ), toggle(2), toggle_ok(false), cres(false),
              config( _bus) {}

        unsigned int getStatus() { return status; }

        bool isPreOperational() {  return status == 0x7F; }

        bool isOperational() {  return status == 0x05; }

        bool isStopped() {  return status == 0x04; }

        bool validToggle() { return toggle_ok; }

        bool configure() {
            CANMessage *guardtime = new CANMessage(); // 50 ms
            CANMessage *answer = new CANMessage(); // 50 ms
            CANMessage *lifetime  = new CANMessage(); // 3 times

            guardtime->setStdId(0x600 + nodeId );
            CANMessage::Data d[] = { 0x22, 0x0c, 0x10, 0x00, 0x32, 0x00, 0x00, 0x00 };
            guardtime->setDataDLC( d, 8);

            answer->setStdId(0x580 + nodeId );
            fillData(d,  0x60, 0x0c, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00 );
            answer->setDataDLC( d, 8);

            config.addRequest( new CANRequest( guardtime, answer, 0.5 ) );

            lifetime->setStdId(0x600 + nodeId );
            fillData(d, 0x22, 0x0d, 0x10, 0x00, 0x03, 0x00, 0x00, 0x00 );
            lifetime->setDataDLC( d, 8);

            answer = new CANMessage(); // 50 ms
            answer->setStdId(0x580 + nodeId );
            fillData(d, 0x60, 0x0d, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00 );
            answer->setDataDLC( d, 8);

            config.addRequest( new CANRequest( lifetime, answer, 0.5 ) );

            config.configInit();

            while ( !config.isFinished() && config.configStep() )
                sleep(1);

            cres = config.isFinished();
            config.configCleanup();
            return cres;
        }

        bool initialize()
        {
            if (cres) {
                rtr = CANMessage::createStdRemote( 0, 0x700+nodeId, 0, 0 );
                bus->addListener(this);
            }

            return cres;
        }

        void step()
        {
            bus->write(rtr);
        }

        void finalize() {
            bus->removeListener(this);
            delete rtr;
        }

        void process(const CANMessage* msg)
        {
            if ( msg->getStdId() == 0x700 + nodeId && ! msg->isRemote() )
                {
                    toggle_ok = false;
                    status = msg->getData(0) & 0x7F;
                    switch (toggle) {
                    case 0:
                        if ( msg->getData(0) >> 7 == 1)
                            toggle_ok = true;
                        break;
                    case 1:
                        if ( msg->getData(0) >> 7 == 0)
                            toggle_ok = true;
                        break;
                    case 2:
                        toggle_ok = true;
                        break;
                    }
                    toggle = msg->getData(0) >> 7;
                }
        }
    };

}}

#endif

---