Subversion Repositories HomeAutomation

/*

 *

 *  Copyright (C) 2010  Mattias Runge

 *

 *  This program is free software; you can redistribute it and/or modify

 *  it under the terms of the GNU General Public License as published by

 *  the Free Software Foundation; either version 2 of the License, or

 *  (at your option) any later version.

 *

 *  This program 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 General Public License for more details.

 *

 *  You should have received a copy of the GNU General Public License along

 *  with this program; if not, write to the Free Software Foundation, Inc.,

 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

 *

 */

#include "Network.h"

#include <vector>

#include <boost/algorithm/string.hpp>

#include <boost/lexical_cast.hpp>

#include "net/Manager.h"

#include "broker/Manager.h"

#include "Protocol.h"

#include "Message.h"

#include "common/Bitset.h"

#include "common/common.h"

#include "common/log.h"

namespace atom {

namespace can {

enum

{

    PACKET_START = 253,

    PACKET_END   = 250,

    PACKET_PING  = 251

};

static const std::string log_module_ = "can::network";

Network::Network(std::string address): broker::Subscriber(false), LOG("can::Network")

{

    this->address_ = address;

    this->client_id_ = 0;

    // Examples of address

    // udp:192.168.1.250:1100

    // serial:/dev/ttyUSB0:38400

    common::StringList parts;

    boost::algorithm::split(parts, address, boost::is_any_of(":"), boost::algorithm::token_compress_off);

    if (parts.size() < 3)

    {

        LOG.Error("Malformed address string: " + address);

        return;

    }

    boost::algorithm::to_lower(parts[0]);

    if (parts[0] == "udp")

    {

        this->protocol_ = net::TRANSPORT_PROTOCOL_UDP;

    }

    else if (parts[0] == "serial")

    {

        this->protocol_ = net::TRANSPORT_PROTOCOL_SERIAL;

    }

    else

    {

        LOG.Error("Unknown protocol, only support udp and serial, got " + parts[0]);

        return;

    }

    this->address_ = parts[1];

    this->port_or_baud_ = boost::lexical_cast<unsigned int>(parts[2]);

    try

    {

        this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);

        LOG.Info("Connected to " + address);

        LOG.Info("Sending ping...");

        common::Byteset buffer(1);

        buffer[0] = PACKET_PING;

        net::Manager::Instance()->SendTo(this->client_id_, buffer);

    }

    catch (std::runtime_error& e)

    {

        LOG.Error(e.what());

    }

}

Network::~Network()

{

    net::Manager::Instance()->Disconnect(this->client_id_);

}

void Network::SlotOnTimeout(timer::TimerId timer_id)

{

    if (timer_id == this->timer_id_)

    {

        this->io_service_.post(boost::bind(&Network::SlotOnTimeoutHandler, this, timer_id));

    }

}

void Network::SlotOnMessageHandler(broker::Message::Pointer message)

{

    if (message->GetType() == broker::Message::CAN_RAW_BYTES)

    {

      net::Manager::Instance()->SendTo(this->client_id_, message->GetRawData());

    }

    else if (message->GetType() == broker::Message::CAN_MESSAGE)

    {

        Message* payload = static_cast<Message*>(message->GetPayload().get());

        common::Byteset data(17);

        data[0] = PACKET_START;

        unsigned int class_id = Protocol::Instance()->ResolveClassId(payload->GetClassName());

        data[4] = class_id << 1;

        if (payload->GetClassName() == "nmt")

        {

            unsigned int command_id = Protocol::Instance()->ResolveNMTCommandId(payload->GetCommandName());

            data[3] = command_id;

        }

        else

        {

            unsigned int direction_flag = Protocol::Instance()->ResolveDirectionFlag(payload->GetDirectionName());

            data[4] |= (direction_flag & 0x01);

            unsigned int module_id = Protocol::Instance()->ResolveModuleId(payload->GetModuleName());

            data[3] = module_id;

            data[2] = payload->GetId();

            unsigned int command_id = Protocol::Instance()->ResolveCommandId(payload->GetCommandName(), payload->GetModuleName());

            data[1] = command_id;

        }

        unsigned int highest_bit = 0;

        common::Bitset databits(64);

        xml::Node::NodeList variable_nodes;

        unsigned int start_bit;

        unsigned int bit_length;

        std::string type;

        std::string value;

        if (payload->GetClassName() == "nmt")

        {

            variable_nodes = Protocol::Instance()->GetNMTCommandVariables(payload->GetCommandName());

        }

        else

        {

            variable_nodes = Protocol::Instance()->GetCommandVariables(payload->GetCommandName(), payload->GetModuleName());

        }

        for (unsigned int n = 0; n < variable_nodes.size(); n++)

        {

            value = payload->GetVariable(variable_nodes[n].GetAttributeValue("name"));

            //LOG.Debug(variable_nodes[n].GetAttributeValue("name") + "=" + value);

            if (value == "")

            {

                continue;

            }

            start_bit = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("start_bit"));

            bit_length = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("bit_length"));

            type = variable_nodes[n].GetAttributeValue("type");

            if (type == "int")

            {

                Protocol::Instance()->EncodeInt(databits, start_bit, bit_length, value);

            }

            else if (type == "float")

            {

                Protocol::Instance()->EncodeFloat(databits, start_bit, bit_length, value);

            }

            else if (type == "ascii")

            {

                Protocol::Instance()->EncodeAscii(databits, start_bit, bit_length, value);

                bit_length = value.length() * 8;

            }

            else if (type == "hexstring")

            {

                Protocol::Instance()->EncodeHexstring(databits, start_bit, bit_length, value);

                bit_length = value.length() * 4;

            }

            else if (type == "enum")

            {

                value = variable_nodes[n].SelectChild("name", value).GetAttributeValue("id");

                Protocol::Instance()->EncodeUint(databits, start_bit, bit_length, value);

            }

            else// if (type == "uint")

            {

                Protocol::Instance()->EncodeUint(databits, start_bit, bit_length, value);

            }

            if (highest_bit < start_bit + bit_length)

            {

                highest_bit = start_bit + bit_length;

            }

        }

        //LOG.Debug(databits.ToDebugString());

        unsigned int length = std::min((int)ceil((float)highest_bit / 8.0f), 8);

        data[5] = 1;

        data[6] = 0;

        data[7] = length;

        for (unsigned int n = 0; n < 8; n++)

        {

            data[8 + n] = databits.GetBytes()[n];

        }

        data[16] = PACKET_END;

        //LOG.Debug("Bytes: " + data.ToDebugString());

        net::Manager::Instance()->SendTo(this->client_id_, data);

    }

    else if (message->GetType() == broker::Message::CAN_RAW_MESSAGE)

    {

        std::string* payload_str;

    payload_str = static_cast<std::string*>(message->GetPayload().get());

    std::string line = *payload_str;

        if (line.length() < 16)

        {

          log::Error(log_module_, "Packet was to short: \"%s\", length was %u.", line.data(), line.length());

          return;

        }

        common::Byteset data(17);

        //LOG.Info("Got "+ line + "end");

        data[0] = PACKET_START;

        std::string value = line.substr(4,2);

    //LOG.Info("<"+ value + ">");

    data[4] = common::FromHex(value);

    //LOG.Info("id1: " + value + " data: ");//+ data[1]);

    value = line.substr(6,2);

    //LOG.Info("<"+ value + ">");

    data[3] = common::FromHex(value);

    //LOG.Info("id2: " + value + " data: ");//+ data[2]);

    value = line.substr(8,2);

    data[2] = common::FromHex(value);

    //LOG.Info("id3: " + value + " data: ");//+ data[3]);

    value = line.substr(10,2);

    data[1] = common::FromHex(value);

    //LOG.Info("id4: " + value + " data: ");//+ data[4]);

    value = line.substr(13,1);

    data[5] = common::FromHex(value);

    //LOG.Info("1: " + value + " data: ");//+ data[5]);

    value = line.substr(15,1);

    data[6] = common::FromHex(value);

    //LOG.Info("1: " + value + " data: ");//+ data[6]);

        unsigned char length = 0;

        unsigned char index = 0;

        while (length < 8 && index + 16 < (unsigned char)line.length())

        {

            value = line.substr(index+17,2);

        data[8+length] = common::FromHex(value);

        //LOG.Info("data: " + value + " data: ");//+ data[6]);

        index += 3;

        length++;

        }

        data[7] = length;

        data[16] = PACKET_END;

        //LOG.Info("Bytes: " + data.ToDebugString());

        net::Manager::Instance()->SendTo(this->client_id_, data);

    }

}

void Network::SlotOnNewDataHandler(net::SocketId client_id, common::Byteset data)

{

  if (client_id != this->client_id_)

  {

    return;

  }

  broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_RAW_BYTES, data, this)));

  static bool have_start = false;

  for (unsigned int n = 0; n < data.size(); n++)

  {

    log::Extreme(log_module_, "data[%u] = %u, have_start = %s, this->buffer_.size() = %u", n, (unsigned int)data[n], have_start ? "true" : "false", this->buffer_.size());

    if (have_start)

    {

      if (data[n] == PACKET_END && this->buffer_.size() >= 15)

      {

        log::Extreme(log_module_, "PACKET_END");

        while (this->buffer_.size() < 15)

        {

          this->buffer_.push_back(0);

        }

        for (unsigned int k = 0; k < this->buffer_.size(); k++)

        {

          log::Extreme(log_module_, "this->buffer_[%u]=%u", k, (unsigned int)this->buffer_[k]);

        }

        this->ProcessBuffer();

        have_start = false;

      }

      else

      {

        this->buffer_.push_back(data[n]);

      }

    }

    else if (data[n] == PACKET_START)

    {

      log::Extreme(log_module_, "PACKET_START");

      common::Byteset empty_vector;

      this->buffer_.swap(empty_vector);

      have_start = true;

    }

    else if (data[n] == PACKET_PING)

    {

      log::Info(log_module_, "Got Pong!");

    }

  }

}

void Network::SlotOnNewClientHandler(net::SocketId id, net::SocketId server_id)

{

}

void Network::SlotOnNewStateHandler(net::SocketId client_id, net::ClientState client_state)

{

    if (client_id != this->client_id_)

    {

        return;

    }

    if (client_state == net::CLIENT_STATE_DISCONNECTED)

    {

        LOG.Warning("Got disconnected, setting reconnect timer...");

        this->timer_id_ = timer::Manager::Instance()->SetTimer(10000, true);

        this->client_id_ = 0;

    }

    else

    {

        LOG.Debug("Got state: " + boost::lexical_cast<std::string>((int)client_state));

    }

}

void Network::SlotOnTimeoutHandler(timer::TimerId timer_id)

{

    try

    {

        this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);

        LOG.Info("Connected again.");

        timer::Manager::Instance()->Cancel(timer_id);

        this->timer_id_ = 0;

    }

    catch (std::runtime_error& e)

    {

        LOG.Error(e.what());

        LOG.Warning("Will try again soon...");

    }

}

void Network::ProcessBuffer()

{

  LOG_DEBUG_ENTER;

    try

    {

        std::string class_name = "";

        std::string direction_name = "";

        std::string module_name = "";

        unsigned int id = 0;

        std::string command_name = "";

    std::string PKTstring = "PKT " +

                atom::common::ToHex8bit((unsigned int)this->buffer_[3]) +

                atom::common::ToHex8bit((unsigned int)this->buffer_[2]) +

                atom::common::ToHex8bit((unsigned int)this->buffer_[1]) +

                atom::common::ToHex8bit((unsigned int)this->buffer_[0]) +

                " " +

                atom::common::ToHex4bit((unsigned int)this->buffer_[4]) +

                " " +

                atom::common::ToHex4bit((unsigned int)this->buffer_[5]);

    for (unsigned int index = 7; index < 7 + (unsigned int)this->buffer_[6]; index++)

    {

        PKTstring += " " + atom::common::ToHex8bit((unsigned int)this->buffer_[index]) ;

    }

    PKTstring += "\n";

    LOG.Extreme(PKTstring);

  for (unsigned int n = 0; n < this->buffer_.size(); n++)

  {

     log::Extreme(log_module_, "this->buffer_[%u]=%u", n, (unsigned int)this->buffer_[n]);

  }

    std::string* payload_str = new std::string(PKTstring);

    broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_RAW_MESSAGE, broker::Message::PayloadPointer(payload_str), this)));

        unsigned int class_id = (this->buffer_[3] >> 1) & 0x0F;

        LOG.Extreme("class_id=" + boost::lexical_cast<std::string>(class_id));

        class_name = Protocol::Instance()->LookupClassName(class_id);

        if (class_name == "nmt")

        {

            unsigned int command_id = this->buffer_[2];

            LOG.Extreme("command_id=" + boost::lexical_cast<std::string>(command_id));

            command_name = Protocol::Instance()->LookupNMTCommandName(command_id);

        }

        else

        {

            unsigned int direction_flag = this->buffer_[3] & 0x01;

            LOG.Extreme("direction_flag=" + boost::lexical_cast<std::string>(direction_flag));

            direction_name = Protocol::Instance()->LookupDirectionFlag(direction_flag);

            unsigned int module_id = this->buffer_[2];

            LOG.Extreme("module_id=" + boost::lexical_cast<std::string>(module_id));

            module_name = Protocol::Instance()->LookupModuleName(module_id);

            LOG.Extreme("module_name=" + module_name);

            id = this->buffer_[1];

            unsigned int command_id = this->buffer_[0];

            LOG.Extreme("command_id=" + boost::lexical_cast<std::string>(command_id));

            command_name = Protocol::Instance()->LookupCommandName(command_id, module_name);

            LOG.Extreme("command_name=" + command_name);

        }

        Message* payload = new Message(class_name, direction_name, module_name, id, command_name);

        unsigned int length = this->buffer_[6];

        common::Byteset data_set;

        data_set.reserve(length);

        for (unsigned int n = 0; n < length; n++)

        {

            data_set.push_back(this->buffer_[n + 7]);

        }

        for (unsigned int n = 0; n < data_set.size(); n++)

        {

           log::Extreme(log_module_, "data_set[%u]=%u", n, (unsigned int)data_set[n]);

        }

        common::Bitset databits(data_set);

        xml::Node::NodeList variable_nodes;

        unsigned int start_bit;

        int bit_length;

        std::string type;

        std::string value;

        std::string name;

        if (class_name == "nmt")

        {

            variable_nodes = Protocol::Instance()->GetNMTCommandVariables(command_name);

        }

        else

        {

            variable_nodes = Protocol::Instance()->GetCommandVariables(command_name, module_name);

        }

        for (unsigned int n = 0; n < variable_nodes.size(); n++)

        {

            name = variable_nodes[n].GetAttributeValue("name");

            start_bit = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("start_bit"));

            bit_length = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("bit_length"));

            if (databits.GetCount() < start_bit + bit_length)

            {

                bit_length = databits.GetCount() - start_bit;

                if (bit_length <= 0)

                {

                    LOG.Warning("Can not read variable " + name + " for command " + command_name + ", message is to short, is there a match between the module and the protocol XML file?");

                    LOG.Debug("start_bit=" + boost::lexical_cast<std::string>(start_bit) + ", bit_length=" + boost::lexical_cast<std::string>(bit_length) + ", databits.GetCount()=" + boost::lexical_cast<std::string>(databits.GetCount()));

                    continue;

                }

            }

            type = variable_nodes[n].GetAttributeValue("type");

            LOG.Extreme("type: type=" + type);

            if (type == "int")

            {

                value = Protocol::Instance()->DecodeInt(databits, start_bit, bit_length);

            }

            else if (type == "float")

            {

                value = Protocol::Instance()->DecodeFloat(databits, start_bit, bit_length);

            }

            else if (type == "ascii")

            {

                value = Protocol::Instance()->DecodeAscii(databits, start_bit, bit_length);

            }

            else if (type == "hexstring")

            {

                value = Protocol::Instance()->DecodeHexstring(databits, start_bit, bit_length);

            }

            else if (type == "enum")

            {

                LOG.Extreme("Enum: command name=" + command_name);

                value = Protocol::Instance()->DecodeUint(databits, start_bit, bit_length);

                LOG.Extreme("Enum: value=" + boost::lexical_cast<std::string>(value));

                LOG.Extreme("start_bit=" + boost::lexical_cast<std::string>(start_bit) + ", bit_length=" + boost::lexical_cast<std::string>(bit_length));

                value = variable_nodes[n].SelectChild("id", value).GetAttributeValue("name");

            }

            else// if (type == "uint")

            {

                value = Protocol::Instance()->DecodeUint(databits, start_bit, bit_length);

            }

            LOG.Extreme("value=\"" + value + "\"");

            payload->SetVariable(name, value);

        }

        broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), this)));

    }

    catch (std::runtime_error& e)

    {

        LOG.Error("Malformed message received, " + std::string(e.what()));

        LOG.Debug("Bytes: " + std::string(this->buffer_.begin(), this->buffer_.end()));

    }

    common::Byteset empty_vector;

    this->buffer_.swap(empty_vector);

    LOG_DEBUG_EXIT;

}

}; // namespace can

}; // namespace atom