source: trunk/MagicSoft/Cosy/candrv/network.cc@ 1404

/* ======================================================================== *\
!
! *
! * This file is part of Stesy, the MAGIC Steering System
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appear in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! *
!
!
!   Author(s): Thomas Bretz <mailto:tbretz@uni-sw.gwdg.de>, 2001
!
!   Copyright: MAGIC Software Development, 2000-2001
!
!
\* ======================================================================== */

///////////////////////////////////////////////////////////////////////
//
// Network
//
// is a collection of nodes which coordinates the network
//
///////////////////////////////////////////////////////////////////////
#include "network.h"

#include <iostream.h> // cout
#include <iomanip.h>  // setw, setfill

ClassImp(Network);

// --------------------------------------------------------------------------
//
// Start the canopen module
// Initialize all nodes (calling NodeDrv::Init()
//
void Network::Start()
{
    lout << "- Starting network." << endl;

    VmodIcan::Start();
    InitNodes();

    lout << "- Network started." << endl;
}

// --------------------------------------------------------------------------
//
// Stop all nodes, stop the can module
//
void Network::Stop()
{
    lout << "- Stopping network." << endl;

    StopNodes();
    VmodIcan::Stop();

    lout << "- Network stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Initialize the network, set all nodes to NULL (n/a)
//
Network::Network(const char *dev, const int baud, MLog &out) : CanOpen(dev, baud, out)
{
    for (int i=0; i<32; i++)
        fNodes[i] = NULL;

    lout << "- Network initialized." << endl;
}

// --------------------------------------------------------------------------
//
// Distributes the received SDO messages to the addressed nodes.
// Depending on the received command either HandleSDO, HandleSDOOK or
// HandleSDOError called.
//  HandleSDO:      Handles a received value
//  HandleSDOOK:    Handles the acknoledgment of a trasmitted SDO
//  HandleSDOError: Handles error occursion (see CanOpen standard)
//
void Network::HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, timeval_t *tv)
{
    switch (cmd)
    {
    case kSDO_TX4:       // answer to 0x40 with 4 bytes of data
        if (fNodes[node])
        {
            fNodes[node]->HandleSDO(idx, subidx, data, tv);
            return;
        }
        break;

    case kSDO_TX3:       // answer to 0x40 with 2 bytes of data
        if (fNodes[node])
        {
            fNodes[node]->HandleSDO(idx, subidx, data>>16, tv);
            return;
        }
        break;

    case kSDO_TX1:       // answer to 0x40 with 1 byte  of data
        if (fNodes[node])
        {
            fNodes[node]->HandleSDO(idx, subidx, data>>24, tv);
            return;
        }
        break;

    case kSDO_TX_OK:     // answer to a SDO_TX message
        if (fNodes[node])
        {
            fNodes[node]->HandleSDOOK(idx, subidx);
            return;
        }
        break;

    case kSDO_TX_ERROR:  // error message (instead of 0x60)
        if (fNodes[node])
        {
            fNodes[node]->HandleSDOError(data);
            return;
        }
        break;
    }
    cout << dec << setfill('0');
    cout << "Node=" << (int)node  << " Cmd=0x" << hex << (int)cmd << ": ";
    cout << "Sdo=" << idx  << "/" << (int)subidx << ": 0x" << setw(8) << data;
    cout << endl;
}

// --------------------------------------------------------------------------
//
// Distributes PDO1 messages to the correspoding node calling HandlePDO1
//
void Network::HandlePDO1(BYTE_t node, BYTE_t *data, timeval_t *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO1: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO1(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes PDO2 messages to the correspoding node calling HandlePDO2
//
void Network::HandlePDO2(BYTE_t node, BYTE_t *data, timeval_t *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO2: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO2(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes PDO3 messages to the correspoding node calling HandlePDO3
//
void Network::HandlePDO3(BYTE_t node, BYTE_t *data, timeval_t *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO3: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO3(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes PDO4 messages to the correspoding node calling HandlePDO4
//
void Network::HandlePDO4(BYTE_t node, BYTE_t *data, timeval_t *tv)
{
    if (!fNodes[node])
    {
        cout << "Node " << dec << (int)node << ", PDO4: " << hex;
        for (int i=0; i<8; i++)
            cout << " 0x" << (int)data[i];
        cout << endl;
        return;
    }

    fNodes[node]->HandlePDO4(data, tv);
}

// --------------------------------------------------------------------------
//
// Sets a node to a given nodedrv. The id is requested from the drv object.
//
void Network::SetNode(NodeDrv *drv)
{
    const BYTE_t nodeid = drv->GetId();

    if (nodeid>31)
    {
        cout << "SetNode - Error: Only node Numbers < 32 are allowed"<< endl;
        return;
    }

    fNodes[nodeid] = drv;
}

// --------------------------------------------------------------------------
//
// Initializes all nodes calling InitDevice
//
void Network::InitNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i])
        {
            lout << "- Setting up Node #" << dec << i << endl;
            fNodes[i]->InitDevice(this);
            fNodeInitialized[i] = TRUE;
        }
    lout << "- All Nodes setup." << endl;
}

// --------------------------------------------------------------------------
//
// Stop all nodes calling StopDevice
//
void Network::StopNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i] && fNodeInitialized[i])
        {
            lout << "- Stopping Node #" << dec << i << endl;
            fNodes[i]->StopDevice();
        }
    lout << "- All Nodes stopped." << endl;
}

// --------------------------------------------------------------------------
//
// returns true if one of the nodes has the error-flag set (HasError).
//
bool Network::HasError() const
{
    bool rc = false;

    for (int i=0; i<32; i++)
    {
        if (!fNodes[i])
            continue;

        if (!fNodes[i]->HasError())
            continue;

        rc = true;

        if (fNodes[i]->GetError() <= 0)
            continue;

        lout << "- Node #" << dec << i << " '" << fNodes[i]->GetNodeName();
        lout << "' has error #" << fNodes[i]->GetError() << endl;
    }

    return rc;
}