Subversion Repositories HomeAutomation

Rev

Rev 1990 | Rev 2081 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1. /*
  2.  *
  3.  *  Copyright (C) 2010  Mattias Runge
  4.  *
  5.  *  This program is free software; you can redistribute it and/or modify
  6.  *  it under the terms of the GNU General Public License as published by
  7.  *  the Free Software Foundation; either version 2 of the License, or
  8.  *  (at your option) any later version.
  9.  *
  10.  *  This program is distributed in the hope that it will be useful,
  11.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13.  *  GNU General Public License for more details.
  14.  *
  15.  *  You should have received a copy of the GNU General Public License along
  16.  *  with this program; if not, write to the Free Software Foundation, Inc.,
  17.  *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18.  *
  19.  */
  20.  
  21. #include "Network.h"
  22.  
  23. #include <vector>
  24.  
  25. #include <boost/algorithm/string.hpp>
  26. #include <boost/lexical_cast.hpp>
  27.  
  28. #include "net/Manager.h"
  29.  
  30. #include "broker/Manager.h"
  31.  
  32. #include "Protocol.h"
  33. #include "Message.h"
  34.  
  35. #include "common/Bitset.h"
  36. #include "common/common.h"
  37. #include "common/log.h"
  38.  
  39. namespace atom {
  40. namespace can {
  41.  
  42. enum
  43. {
  44.     PACKET_START = 253,
  45.     PACKET_END   = 250,
  46.     PACKET_PING  = 251
  47. };
  48.  
  49. static const std::string log_module_ = "can::network";
  50.  
  51. Network::Network(std::string address): broker::Subscriber(false), LOG("can::Network")
  52. {
  53.     this->address_ = address;
  54.     this->client_id_ = 0;
  55.        
  56.     // Examples of address
  57.     // udp:192.168.1.250:1100
  58.     // serial:/dev/ttyUSB0:38400
  59.    
  60.     common::StringList parts;
  61.     boost::algorithm::split(parts, address, boost::is_any_of(":"), boost::algorithm::token_compress_off);
  62.    
  63.     if (parts.size() < 3)
  64.     {
  65.         LOG.Error("Malformed address string: " + address);
  66.         return;
  67.     }
  68.    
  69.     boost::algorithm::to_lower(parts[0]);
  70.    
  71.     if (parts[0] == "udp")
  72.     {
  73.         this->protocol_ = net::TRANSPORT_PROTOCOL_UDP;
  74.     }
  75.     else if (parts[0] == "serial")
  76.     {
  77.         this->protocol_ = net::TRANSPORT_PROTOCOL_SERIAL;
  78.     }
  79.     else
  80.     {
  81.         LOG.Error("Unknown protocol, only support udp and serial, got " + parts[0]);
  82.         return;
  83.     }
  84.    
  85.     this->address_ = parts[1];
  86.     this->port_or_baud_ = boost::lexical_cast<unsigned int>(parts[2]);
  87.    
  88.     try
  89.     {
  90.         this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);
  91.         LOG.Info("Connected to " + address);
  92.        
  93.         LOG.Info("Sending ping...");
  94.  
  95.         common::Byteset buffer(1);
  96.         buffer[0] = PACKET_PING;
  97.        
  98.         net::Manager::Instance()->SendTo(this->client_id_, buffer);
  99.     }
  100.     catch (std::runtime_error& e)
  101.     {
  102.         LOG.Error(e.what());
  103.     }
  104. }
  105.  
  106. Network::~Network()
  107. {
  108.     net::Manager::Instance()->Disconnect(this->client_id_);
  109. }
  110.  
  111. void Network::SlotOnTimeout(timer::TimerId timer_id)
  112. {
  113.     if (timer_id == this->timer_id_)
  114.     {
  115.         this->io_service_.post(boost::bind(&Network::SlotOnTimeoutHandler, this, timer_id));
  116.     }
  117. }
  118.  
  119. void Network::SlotOnMessageHandler(broker::Message::Pointer message)
  120. {
  121.     if (message->GetType() == broker::Message::CAN_MESSAGE)
  122.     {
  123.         Message* payload = static_cast<Message*>(message->GetPayload().get());
  124.         common::Byteset data(17);
  125.        
  126.         data[0] = PACKET_START;
  127.        
  128.         unsigned int class_id = Protocol::Instance()->ResolveClassId(payload->GetClassName());
  129.         data[4] = class_id << 1;
  130.  
  131.         if (payload->GetClassName() == "nmt")
  132.         {
  133.             unsigned int command_id = Protocol::Instance()->ResolveNMTCommandId(payload->GetCommandName());
  134.             data[3] = command_id;
  135.         }
  136.         else
  137.         {
  138.             unsigned int direction_flag = Protocol::Instance()->ResolveDirectionFlag(payload->GetDirectionName());
  139.             data[4] |= (direction_flag & 0x01);
  140.            
  141.             unsigned int module_id = Protocol::Instance()->ResolveModuleId(payload->GetModuleName());
  142.             data[3] = module_id;
  143.            
  144.             data[2] = payload->GetId();
  145.            
  146.             unsigned int command_id = Protocol::Instance()->ResolveCommandId(payload->GetCommandName(), payload->GetModuleName());
  147.             data[1] = command_id;
  148.         }
  149.        
  150.         unsigned int highest_bit = 0;
  151.         common::Bitset databits(64);
  152.        
  153.         xml::Node::NodeList variable_nodes;
  154.         unsigned int start_bit;
  155.         unsigned int bit_length;
  156.         std::string type;
  157.         std::string value;
  158.        
  159.         if (payload->GetClassName() == "nmt")
  160.         {
  161.             variable_nodes = Protocol::Instance()->GetNMTCommandVariables(payload->GetCommandName());
  162.         }
  163.         else
  164.         {
  165.             variable_nodes = Protocol::Instance()->GetCommandVariables(payload->GetCommandName(), payload->GetModuleName());
  166.         }
  167.        
  168.         for (unsigned int n = 0; n < variable_nodes.size(); n++)
  169.         {
  170.             value = payload->GetVariable(variable_nodes[n].GetAttributeValue("name"));
  171.             //LOG.Debug(variable_nodes[n].GetAttributeValue("name") + "=" + value);
  172.             if (value == "")
  173.             {
  174.                 continue;
  175.             }
  176.            
  177.             start_bit = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("start_bit"));
  178.             bit_length = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("bit_length"));
  179.             type = variable_nodes[n].GetAttributeValue("type");
  180.            
  181.             if (type == "int")
  182.             {
  183.                 Protocol::Instance()->EncodeInt(databits, start_bit, bit_length, value);
  184.             }
  185.             else if (type == "float")
  186.             {
  187.                 Protocol::Instance()->EncodeFloat(databits, start_bit, bit_length, value);
  188.             }
  189.             else if (type == "ascii")
  190.             {
  191.                 Protocol::Instance()->EncodeAscii(databits, start_bit, bit_length, value);
  192.                 bit_length = value.length() * 8;
  193.             }
  194.             else if (type == "hexstring")
  195.             {
  196.                 Protocol::Instance()->EncodeHexstring(databits, start_bit, bit_length, value);
  197.                 bit_length = value.length() * 4;
  198.             }
  199.             else if (type == "enum")
  200.             {
  201.                 value = variable_nodes[n].SelectChild("name", value).GetAttributeValue("id");
  202.                 Protocol::Instance()->EncodeUint(databits, start_bit, bit_length, value);
  203.             }
  204.             else// if (type == "uint")
  205.             {
  206.                 Protocol::Instance()->EncodeUint(databits, start_bit, bit_length, value);
  207.             }
  208.  
  209.             if (highest_bit < start_bit + bit_length)
  210.             {
  211.                 highest_bit = start_bit + bit_length;
  212.             }
  213.         }
  214.        
  215.         //LOG.Debug(databits.ToDebugString());
  216.        
  217.         unsigned int length = std::min((int)ceil((float)highest_bit / 8.0f), 8);
  218.        
  219.         data[5] = 1;
  220.         data[6] = 0;
  221.        
  222.         data[7] = length;
  223.        
  224.         for (unsigned int n = 0; n < 8; n++)
  225.         {
  226.             data[8 + n] = databits.GetBytes()[n];
  227.         }
  228.        
  229.         data[16] = PACKET_END;
  230.        
  231.         //LOG.Debug("Bytes: " + data.ToDebugString());
  232.         net::Manager::Instance()->SendTo(this->client_id_, data);
  233.     } else if (message->GetType() == broker::Message::CAN_RAW_MESSAGE)
  234.     {
  235.    
  236.         std::string* payload_str;
  237.     payload_str = static_cast<std::string*>(message->GetPayload().get());
  238.     std::string line = *payload_str;
  239.         common::Byteset data(17);
  240.         //LOG.Info("Got "+ line + "end");
  241.    
  242.         data[0] = PACKET_START;
  243.         std::string value = line.substr(4,2);
  244.     //LOG.Info("<"+ value + ">");
  245.     data[4] = common::FromHex(value);
  246.     //LOG.Info("id1: " + value + " data: ");//+ data[1]);
  247.    
  248.     value = line.substr(6,2);
  249.     //LOG.Info("<"+ value + ">");
  250.    
  251.     data[3] = common::FromHex(value);
  252.     //LOG.Info("id2: " + value + " data: ");//+ data[2]);
  253.    
  254.     value = line.substr(8,2);
  255.     data[2] = common::FromHex(value);
  256.     //LOG.Info("id3: " + value + " data: ");//+ data[3]);
  257.    
  258.     value = line.substr(10,2);
  259.     data[1] = common::FromHex(value);
  260.     //LOG.Info("id4: " + value + " data: ");//+ data[4]);
  261.    
  262.     value = line.substr(13,1);
  263.     data[5] = common::FromHex(value);
  264.     //LOG.Info("1: " + value + " data: ");//+ data[5]);
  265.    
  266.     value = line.substr(15,1);
  267.     data[6] = common::FromHex(value);
  268.     //LOG.Info("1: " + value + " data: ");//+ data[6]);
  269.  
  270.         unsigned char length = 0;
  271.         unsigned char index = 0;
  272.         while (length < 8 && index + 16 < (unsigned char)line.length())
  273.         {
  274.             value = line.substr(index+17,2);
  275.         data[8+length] = common::FromHex(value);
  276.         //LOG.Info("data: " + value + " data: ");//+ data[6]);
  277.         index += 3;
  278.         length++;
  279.         }
  280.        
  281.         data[7] = length;
  282.         data[16] = PACKET_END;
  283.  
  284.        
  285.         //LOG.Info("Bytes: " + data.ToDebugString());
  286.         net::Manager::Instance()->SendTo(this->client_id_, data);
  287.     }
  288. }
  289.  
  290. void Network::SlotOnNewDataHandler(net::SocketId client_id, common::Byteset data)
  291. {
  292.   if (client_id != this->client_id_)
  293.   {
  294.     return;
  295.   }
  296.  
  297.   static bool have_start = false;
  298.  
  299.   for (unsigned int n = 0; n < data.size(); n++)
  300.   {
  301.     log::Debug(log_module_, "data[%u] = %u, have_start = %s, this->buffer_.size() = %u", n, (unsigned int)data[n], have_start ? "true" : "false", this->buffer_.size());
  302.    
  303.     if (have_start)
  304.     {
  305.       if (data[n] == PACKET_END)
  306.       {
  307.         log::Debug(log_module_, "PACKET_END");
  308.        
  309.         while (this->buffer_.size() < 15)
  310.         {
  311.           this->buffer_.push_back(0);
  312.         }
  313.        
  314.         for (unsigned int k = 0; k < this->buffer_.size(); k++)
  315.         {
  316.           log::Debug(log_module_, "this->buffer_[%u]=%u", k, (unsigned int)this->buffer_[k]);
  317.         }
  318.        
  319.         this->ProcessBuffer();
  320.        
  321.         have_start = false;
  322.       }
  323.       else
  324.       {
  325.         this->buffer_.push_back(data[n]);
  326.       }
  327.     }
  328.     else if (data[n] == PACKET_START)
  329.     {
  330.       log::Debug(log_module_, "PACKET_START");
  331.      
  332.       common::Byteset empty_vector;
  333.       this->buffer_.swap(empty_vector);
  334.  
  335.       have_start = true;
  336.     }
  337.     else if (data[n] == PACKET_PING)
  338.     {
  339.       log::Info(log_module_, "Got Pong!");
  340.     }
  341.   }
  342. }
  343.  
  344. void Network::SlotOnNewClientHandler(net::SocketId id, net::SocketId server_id)
  345. {
  346.  
  347. }
  348.  
  349. void Network::SlotOnNewStateHandler(net::SocketId client_id, net::ClientState client_state)
  350. {
  351.     if (client_id != this->client_id_)
  352.     {
  353.         return;
  354.     }
  355.    
  356.     if (client_state == net::CLIENT_STATE_DISCONNECTED)
  357.     {
  358.         LOG.Warning("Got disconnected, setting reconnect timer...");
  359.        
  360.         this->timer_id_ = timer::Manager::Instance()->SetTimer(10000, true);
  361.         this->client_id_ = 0;
  362.     }
  363.     else
  364.     {
  365.         LOG.Debug("Got state: " + boost::lexical_cast<std::string>((int)client_state));
  366.     }
  367. }
  368.  
  369. void Network::SlotOnTimeoutHandler(timer::TimerId timer_id)
  370. {
  371.     try
  372.     {
  373.         this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);
  374.         LOG.Info("Connected again.");
  375.        
  376.         timer::Manager::Instance()->Cancel(timer_id);
  377.         this->timer_id_ = 0;
  378.     }
  379.     catch (std::runtime_error& e)
  380.     {
  381.         LOG.Error(e.what());
  382.         LOG.Warning("Will try again soon...");
  383.     }
  384. }
  385.  
  386. void Network::ProcessBuffer()
  387. {
  388.   LOG_DEBUG_ENTER;
  389.  
  390.     try
  391.     {
  392.         std::string class_name = "";
  393.         std::string direction_name = "";
  394.         std::string module_name = "";
  395.         unsigned int id = 0;
  396.         std::string command_name = "";
  397.        
  398.     std::string PKTstring = "PKT " +
  399.                 atom::common::ToHex8bit((unsigned int)this->buffer_[3]) +
  400.                 atom::common::ToHex8bit((unsigned int)this->buffer_[2]) +
  401.                 atom::common::ToHex8bit((unsigned int)this->buffer_[1]) +
  402.                 atom::common::ToHex8bit((unsigned int)this->buffer_[0]) +
  403.                 " " +
  404.                 atom::common::ToHex4bit((unsigned int)this->buffer_[4]) +
  405.                 " " +
  406.                 atom::common::ToHex4bit((unsigned int)this->buffer_[5]);
  407.    
  408.     for (unsigned int index = 7; index < 7 + (unsigned int)this->buffer_[6]; index++)
  409.     {
  410.         PKTstring += " " + atom::common::ToHex8bit((unsigned int)this->buffer_[index]) ;
  411.     }
  412.    
  413.     PKTstring += "\n";
  414.    
  415.     LOG.Debug(PKTstring);
  416.    
  417.   for (unsigned int n = 0; n < this->buffer_.size(); n++)
  418.   {
  419.      log::Debug(log_module_, "this->buffer_[%u]=%u", n, (unsigned int)this->buffer_[n]);
  420.   }
  421.  
  422.     std::string* payload_str = new std::string(PKTstring);
  423.     broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_RAW_MESSAGE, broker::Message::PayloadPointer(payload_str), this)));
  424.  
  425.    
  426.         unsigned int class_id = (this->buffer_[3] >> 1) & 0x0F;
  427.         LOG.Debug("class_id=" + boost::lexical_cast<std::string>(class_id));
  428.         class_name = Protocol::Instance()->LookupClassName(class_id);
  429.        
  430.         if (class_name == "nmt")
  431.         {
  432.             unsigned int command_id = this->buffer_[2];
  433.             LOG.Debug("command_id=" + boost::lexical_cast<std::string>(command_id));
  434.             command_name = Protocol::Instance()->LookupNMTCommandName(command_id);
  435.         }
  436.         else
  437.         {
  438.             unsigned int direction_flag = this->buffer_[3] & 0x01;
  439.             LOG.Debug("direction_flag=" + boost::lexical_cast<std::string>(direction_flag));
  440.             direction_name = Protocol::Instance()->LookupDirectionFlag(direction_flag);
  441.            
  442.             unsigned int module_id = this->buffer_[2];
  443.             LOG.Debug("module_id=" + boost::lexical_cast<std::string>(module_id));
  444.             module_name = Protocol::Instance()->LookupModuleName(module_id);
  445.             LOG.Debug("module_name=" + module_name);
  446.             id = this->buffer_[1];
  447.            
  448.             unsigned int command_id = this->buffer_[0];
  449.             LOG.Debug("command_id=" + boost::lexical_cast<std::string>(command_id));
  450.             command_name = Protocol::Instance()->LookupCommandName(command_id, module_name);
  451.             LOG.Debug("command_name=" + command_name);
  452.         }
  453.  
  454.         Message* payload = new Message(class_name, direction_name, module_name, id, command_name);
  455.  
  456.         unsigned int length = this->buffer_[6];
  457.        
  458.         common::Byteset data_set;
  459.         data_set.reserve(length);
  460.        
  461.         for (unsigned int n = 0; n < length; n++)
  462.         {
  463.             data_set.push_back(this->buffer_[n + 7]);
  464.         }
  465.        
  466.         for (unsigned int n = 0; n < data_set.size(); n++)
  467.         {
  468.            log::Debug(log_module_, "data_set[%u]=%u", n, (unsigned int)data_set[n]);
  469.         }
  470.  
  471.         common::Bitset databits(data_set);
  472.         xml::Node::NodeList variable_nodes;
  473.         unsigned int start_bit;
  474.         int bit_length;
  475.         std::string type;
  476.         std::string value;
  477.         std::string name;
  478.        
  479.         if (class_name == "nmt")
  480.         {
  481.             variable_nodes = Protocol::Instance()->GetNMTCommandVariables(command_name);
  482.         }
  483.         else
  484.         {
  485.             variable_nodes = Protocol::Instance()->GetCommandVariables(command_name, module_name);
  486.         }
  487.        
  488.         for (unsigned int n = 0; n < variable_nodes.size(); n++)
  489.         {
  490.             name = variable_nodes[n].GetAttributeValue("name");
  491.             start_bit = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("start_bit"));
  492.             bit_length = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("bit_length"));
  493.            
  494.             if (databits.GetCount() < start_bit + bit_length)
  495.             {
  496.                 bit_length = databits.GetCount() - start_bit;
  497.                
  498.                 if (bit_length <= 0)
  499.                 {
  500.                     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?");
  501.                     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()));
  502.                     continue;
  503.                 }
  504.             }
  505.            
  506.             type = variable_nodes[n].GetAttributeValue("type");
  507.             LOG.Debug("type: type=" + type);
  508.            
  509.             if (type == "int")
  510.             {
  511.                 value = Protocol::Instance()->DecodeInt(databits, start_bit, bit_length);
  512.             }
  513.             else if (type == "float")
  514.             {
  515.                 value = Protocol::Instance()->DecodeFloat(databits, start_bit, bit_length);
  516.             }
  517.             else if (type == "ascii")
  518.             {
  519.                 value = Protocol::Instance()->DecodeAscii(databits, start_bit, bit_length);
  520.             }
  521.             else if (type == "hexstring")
  522.             {
  523.                 value = Protocol::Instance()->DecodeHexstring(databits, start_bit, bit_length);
  524.             }
  525.             else if (type == "enum")
  526.             {
  527.                 LOG.Debug("Enum: command name=" + command_name);
  528.                 value = Protocol::Instance()->DecodeUint(databits, start_bit, bit_length);
  529.                 LOG.Debug("Enum: value=" + boost::lexical_cast<std::string>(value));
  530.                 LOG.Debug("start_bit=" + boost::lexical_cast<std::string>(start_bit) + ", bit_length=" + boost::lexical_cast<std::string>(bit_length));
  531.                
  532.                 value = variable_nodes[n].SelectChild("id", value).GetAttributeValue("name");
  533.             }
  534.             else// if (type == "uint")
  535.             {
  536.                 value = Protocol::Instance()->DecodeUint(databits, start_bit, bit_length);
  537.             }
  538.            
  539.             LOG.Debug("value=\"" + value + "\"");
  540.            
  541.             payload->SetVariable(name, value);
  542.         }
  543.        
  544.         broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), this)));
  545.     }
  546.     catch (std::runtime_error& e)
  547.     {
  548.         LOG.Error("Malformed message received, " + std::string(e.what()));
  549.         LOG.Debug("Bytes: " + std::string(this->buffer_.begin(), this->buffer_.end()));
  550.     }
  551.    
  552.     common::Byteset empty_vector;
  553.     this->buffer_.swap(empty_vector);
  554.    
  555.    
  556.     LOG_DEBUG_EXIT;
  557. }
  558.    
  559. }; // namespace can
  560. }; // namespace atom
  561.