Subversion Repositories HomeAutomation

Rev

Rev 2266 | 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 "Protocol.h"
  22.  
  23. #include <stdexcept>
  24. #include <string.h>
  25. #include <stdint.h>
  26. #include <boost/lexical_cast.hpp>
  27.  
  28. #include "common/log.h"
  29.  
  30. namespace atom {
  31. namespace can {
  32.  
  33. Protocol::Pointer Protocol::instance_;
  34.  
  35. static const std::string log_module_ = "can::protocol";
  36.  
  37. Protocol::Protocol() : LOG("can::Protocol")
  38. {
  39. }
  40.  
  41. Protocol::~Protocol()
  42. {
  43. }
  44.  
  45. void Protocol::Create()
  46. {
  47.   Protocol::instance_ = Protocol::Pointer(new Protocol());
  48. }
  49.  
  50. Protocol::Pointer Protocol::Instance()
  51. {
  52.   return Protocol::instance_;
  53. }
  54.  
  55. void Protocol::Delete()
  56. {
  57.   Protocol::instance_.reset();
  58. }
  59.  
  60. bool Protocol::Load(std::string file_path)
  61. {
  62.   try
  63.   {
  64.     this->root_node_.LoadFile(file_path);
  65.   }
  66.   catch (std::runtime_error& e)
  67.   {
  68.     LOG.Error(e.what());
  69.     return false;
  70.   }
  71.  
  72.   LOG.Info("Protocol loaded successfully.");
  73.   return true;
  74. }
  75.  
  76. std::string Protocol::LookupClassName(unsigned int class_id)
  77. {
  78.     try
  79.     {
  80.         return this->root_node_.FindChild("classes").SelectChild("id", boost::lexical_cast<std::string>(class_id)).GetAttributeValue("name");
  81.     }
  82.     catch (std::runtime_error& e)
  83.     {
  84.         LOG.Error(e.what());
  85.         throw std::runtime_error("No such class_id found, " + boost::lexical_cast<std::string>(class_id));
  86.     }
  87. }
  88.  
  89. unsigned int Protocol::ResolveClassId(std::string class_name)
  90. {
  91.     try
  92.     {
  93.         return boost::lexical_cast<unsigned int>(this->root_node_.FindChild("classes").SelectChild("name", class_name).GetAttributeValue("id"));
  94.     }
  95.     catch (std::runtime_error& e)
  96.     {
  97.         LOG.Error(e.what());
  98.         throw std::runtime_error("No such class_name found, " + class_name);
  99.     }
  100. }
  101.  
  102. std::string Protocol::LookupCommandName(unsigned int command_id, std::string module_name)
  103. {
  104.     try
  105.     {
  106.         if (command_id >= 128)
  107.         {
  108.             return this->root_node_.FindChild("commands").SelectChild("id", boost::lexical_cast<std::string>(command_id), "module", module_name).GetAttributeValue("name");
  109.         }
  110.  
  111.         return this->root_node_.FindChild("commands").SelectChild("id", boost::lexical_cast<std::string>(command_id)).GetAttributeValue("name");
  112.     }
  113.     catch (std::runtime_error& e)
  114.     {
  115.         LOG.Error(e.what());
  116.         throw std::runtime_error("No such command found, id = " + boost::lexical_cast<std::string>(command_id) + ", module = " + module_name);
  117.     }
  118. }
  119.  
  120. unsigned int Protocol::ResolveCommandId(std::string command_name, std::string module_name)
  121. {
  122.     try
  123.     {
  124.         return boost::lexical_cast<unsigned int >(this->root_node_.FindChild("commands").SelectChild("name", command_name, "module", module_name).GetAttributeValue("id"));
  125.     }
  126.     catch (std::runtime_error& e)
  127.     {
  128.     }
  129.  
  130.     try
  131.     {
  132.         return boost::lexical_cast<unsigned int >(this->root_node_.FindChild("commands").SelectChild("name", command_name).GetAttributeValue("id"));
  133.     }
  134.     catch (std::runtime_error& e)
  135.     {
  136.         LOG.Error(e.what());
  137.         throw std::runtime_error("No such command found, name = " + command_name + ", module = " + module_name);
  138.     }
  139. }
  140.  
  141. std::string Protocol::LookupNMTCommandName(unsigned int command_id)
  142. {
  143.     try
  144.     {
  145.         return this->root_node_.FindChild("nmt_messages").SelectChild("id", boost::lexical_cast<std::string>(command_id)).GetAttributeValue("name");
  146.     }
  147.     catch (std::runtime_error& e)
  148.     {
  149.         LOG.Error(e.what());
  150.         throw std::runtime_error("No such nmt command found, id = " + boost::lexical_cast<std::string>(command_id));
  151.     }
  152. }
  153.  
  154. unsigned int Protocol::ResolveNMTCommandId(std::string command_name)
  155. {
  156.     try
  157.     {
  158.         return boost::lexical_cast<unsigned int >(this->root_node_.FindChild("nmt_messages").SelectChild("name", command_name).GetAttributeValue("id"));
  159.     }
  160.     catch (std::runtime_error& e)
  161.     {
  162.         LOG.Error(e.what());
  163.         throw std::runtime_error("No such nmt command found, name = " + command_name);
  164.     }
  165. }
  166.  
  167. std::string Protocol::GetDefineId(std::string name, std::string group)
  168. {
  169.     try
  170.     {
  171.         return this->root_node_.FindChild("defines").SelectChild("name", name, "group", group).GetAttributeValue("id");
  172.     }
  173.     catch (std::runtime_error& e)
  174.     {
  175.         LOG.Error(e.what());
  176.         throw std::runtime_error("No such define found, name = " + name + ", group = " + group);
  177.     }
  178. }
  179.  
  180. std::string Protocol::LookupDirectionFlag(unsigned int direction_flag)
  181. {
  182.     try
  183.     {
  184.         return this->root_node_.FindChild("defines").SelectChild("id", boost::lexical_cast<std::string>(direction_flag), "group", "DirectionFlag").GetAttributeValue("name");
  185.     }
  186.     catch (std::runtime_error& e)
  187.     {
  188.         LOG.Error(e.what());
  189.         throw std::runtime_error("No such direction flag found, direction_flag = " + boost::lexical_cast<std::string>(direction_flag));
  190.     }
  191. }
  192.  
  193. unsigned int Protocol::ResolveDirectionFlag(std::string direction_name)
  194. {
  195.     try
  196.     {
  197.         return boost::lexical_cast<unsigned int>(this->root_node_.FindChild("defines").SelectChild("name", direction_name, "group", "DirectionFlag").GetAttributeValue("id"));
  198.     }
  199.     catch (std::runtime_error& e)
  200.     {
  201.         LOG.Error(e.what());
  202.         throw std::runtime_error("No such direction flag found, direction_name = " + direction_name);
  203.     }
  204. }
  205.  
  206. std::string Protocol::LookupModuleName(unsigned int module_id)
  207. {
  208.     try
  209.     {
  210.         return this->root_node_.FindChild("modules").SelectChild("id", boost::lexical_cast<std::string>(module_id)).GetAttributeValue("name");
  211.     }
  212.     catch (std::runtime_error& e)
  213.     {
  214.         LOG.Error(e.what());
  215.         throw std::runtime_error("No such module id found, " + boost::lexical_cast<std::string>(module_id));
  216.     }
  217. }
  218.  
  219. unsigned int Protocol::ResolveModuleId(std::string module_name)
  220. {
  221.     if (module_name == "")
  222.     {
  223.         return 0;
  224.     }
  225.  
  226.     try
  227.     {
  228.         return boost::lexical_cast<unsigned int>(this->root_node_.FindChild("modules").SelectChild("name", module_name).GetAttributeValue("id"));
  229.     }
  230.     catch (std::runtime_error& e)
  231.     {
  232.         LOG.Error(e.what());
  233.         throw std::runtime_error("No such module name found, " + module_name);
  234.     }
  235. }
  236.  
  237. xml::Node::NodeList Protocol::GetCommandVariables(std::string command_name, std::string module_name)
  238. {
  239.     xml::Node node;
  240.  
  241.     try
  242.     {
  243.         node = this->root_node_.FindChild("commands").SelectChild("name", command_name, "module", module_name);
  244.  
  245.         try
  246.         {
  247.             return node.FindChild("variables").GetChildren();
  248.         }
  249.         catch (std::runtime_error& e)
  250.         {
  251.             return xml::Node::NodeList();
  252.         }
  253.     }
  254.     catch (std::runtime_error& e)
  255.     {
  256.     }
  257.  
  258.  
  259.     try
  260.     {
  261.         node = this->root_node_.FindChild("commands").SelectChild("name", command_name);
  262.  
  263.         try
  264.         {
  265.             return node.FindChild("variables").GetChildren();
  266.         }
  267.         catch (std::runtime_error& e)
  268.         {
  269.             return xml::Node::NodeList();
  270.         }
  271.     }
  272.     catch (std::runtime_error& e)
  273.     {
  274.         LOG.Error(e.what());
  275.         throw std::runtime_error("Could not find variables for command, name = " + command_name + ", module = " + module_name);
  276.     }
  277.  
  278.     return xml::Node::NodeList();
  279. }
  280.  
  281. xml::Node::NodeList Protocol::GetNMTCommandVariables(std::string command_name)
  282. {
  283.     xml::Node node;
  284.  
  285.     try
  286.     {
  287.         node = this->root_node_.FindChild("nmt_messages").SelectChild("name", command_name);
  288.  
  289.         try
  290.         {
  291.             return node.FindChild("variables").GetChildren();
  292.         }
  293.         catch (std::runtime_error& e)
  294.         {
  295.             return xml::Node::NodeList();
  296.         }
  297.     }
  298.     catch (std::runtime_error& e)
  299.     {
  300.         LOG.Error(e.what());
  301.         throw std::runtime_error("Could not find variables for nmt command, name = " + command_name);
  302.     }
  303.  
  304.     return xml::Node::NodeList();
  305. }
  306.  
  307. std::string Protocol::DecodeInt(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length)
  308. {
  309.     bool sign = bitset.Get(start_bit) == 1;
  310.  
  311.     unsigned long raw_bit_value = bitset.Read(start_bit, bit_length);
  312.  
  313.     long raw_value = raw_bit_value;
  314.  
  315.     if (sign)
  316.     {
  317.         long mask = 0;
  318.  
  319.         for (unsigned int n = 0; n < bit_length; n++)
  320.         {
  321.             mask += (1 << n);
  322.         }
  323.  
  324.         raw_value = -((~raw_bit_value) & mask);
  325.     }
  326.  
  327.     return boost::lexical_cast<std::string>(raw_value);
  328. }
  329.  
  330. void Protocol::EncodeInt(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length, std::string value)
  331. {
  332.     unsigned long raw_bit_value = 0;
  333.     long raw_value = boost::lexical_cast<long>(value);
  334.     //LOG.Info("Float as int: " + raw_value);
  335.  
  336.     raw_bit_value = raw_value;
  337.  
  338.     if (raw_value < 0)
  339.     {
  340.         long mask = 0;
  341.  
  342.         for (unsigned int n = 0; n < bit_length; n++)
  343.         {
  344.             mask += (1 << n);
  345.         }
  346.  
  347.         raw_bit_value = (~(-raw_value)) & mask;
  348.     }
  349.  
  350.     bitset.Write(start_bit, bit_length, raw_bit_value);
  351. }
  352.  
  353. std::string Protocol::DecodeUint(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length)
  354. {
  355.     uint64_t raw_bit_value = bitset.Read(start_bit, bit_length);
  356.  
  357.     //LOG.Debug("DecodeUint: raw_bit_value=" + boost::lexical_cast<std::string>(raw_bit_value));
  358.  
  359.     return boost::lexical_cast<std::string>((uint64_t)raw_bit_value);
  360. }
  361.  
  362. void Protocol::EncodeUint(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length, std::string value)
  363. {
  364.   //LOG.Debug("start_bit:" + boost::lexical_cast<std::string>(start_bit) + ", bit_lengt:" + boost::lexical_cast<std::string>(bit_length) + ", value:\"" + value + "\"");
  365.     uint64_t raw_bit_value = boost::lexical_cast<uint64_t>(value);
  366.  
  367.     bitset.Write(start_bit, bit_length, raw_bit_value);
  368. }
  369.  
  370. std::string Protocol::DecodeFloat(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length)
  371. {
  372.     float float_value = boost::lexical_cast<float>(this->DecodeInt(bitset, start_bit, bit_length));
  373.     //LOG.Debug("DecodeFloat::float_value1=" + boost::lexical_cast<std::string>(float_value));
  374.  
  375.  
  376.     float_value = float_value / 64.0f;
  377.  
  378.     //LOG.Debug("DecodeFloat::float_value2=" + boost::lexical_cast<std::string>(float_value));
  379.  
  380.     return boost::lexical_cast<std::string>(float_value);
  381. }
  382.  
  383. void Protocol::EncodeFloat(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length, std::string value)
  384. {
  385.     long int_value = boost::lexical_cast<float>(value) * 64.0f;
  386.     //LOG.Info("Float: " + int_value);
  387.  
  388.     this->EncodeInt(bitset, start_bit, bit_length, boost::lexical_cast<std::string>(int_value));
  389. }
  390.  
  391. std::string Protocol::DecodeIEEE32(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length)
  392. {
  393.     if (bit_length != 32) { return 0; }
  394.     uint32_t result = bitset.Read(start_bit, bit_length);
  395.     //LOG.Info("uint32_t: " + boost::lexical_cast<std::string>((uint32_t)result));
  396.    
  397.     uint32_t result2 = (0xFF000000 & result) >> 24;
  398.     result2 += (0x00FF0000 & result) >> 8;
  399.     result2 += (0x0000FF00 & result) << 8;
  400.     result2 += (0x000000FF & result) << 24;
  401.     //LOG.Info("uint32_t: " + boost::lexical_cast<std::string>((uint32_t)result2));
  402.     float *ptr;
  403.     ptr = (float*)&result2;
  404.     float float_value = *ptr;
  405.     //LOG.Info("float: " + boost::lexical_cast<std::string>(float_value));    
  406.     return boost::lexical_cast<std::string>(float_value);
  407. }
  408.  
  409. void Protocol::EncodeIEEE32(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length, std::string value)
  410. {
  411.     if (bit_length != 32) { return; }
  412.     float float_value = boost::lexical_cast<float>(value);
  413.     //LOG.Info("Float: " + int_value);
  414.     uint32_t *ptr;
  415.     ptr = (uint32_t*)&float_value;
  416.     uint32_t result = *ptr;
  417.     uint32_t raw_bit_value = (0xFF000000 & result) >> 24;
  418.     raw_bit_value += (0x00FF0000 & result) >> 8;
  419.     raw_bit_value += (0x0000FF00 & result) << 8;
  420.     raw_bit_value += (0x000000FF & result) << 24;
  421.     bitset.Write(start_bit, bit_length, raw_bit_value);
  422. }
  423.  
  424. std::string Protocol::DecodeAscii(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length)
  425. {
  426.     std::string value;
  427.  
  428.     for (unsigned int n = 0; n < bit_length; n += 8)
  429.     {
  430.         unsigned long raw_value = bitset.Read(start_bit + n, 8);
  431.         value += (char)raw_value;
  432.     }
  433.  
  434.     return value;
  435. }
  436.  
  437. void Protocol::EncodeAscii(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length, std::string value)
  438. {
  439.     //LOG.Debug("EncodeAscii: " + value + ", length=" + boost::lexical_cast<std::string>(value.length()) + ", bit_length=" + boost::lexical_cast<std::string>(bit_length));
  440.  
  441.     for (unsigned int n = 0; n < bit_length; n += 8)
  442.     {
  443.         if (n / 8 >= value.length())
  444.         {
  445.             break;
  446.         }
  447.  
  448.         //LOG.Debug(boost::lexical_cast<std::string>((unsigned int)value[n / 8]));
  449.  
  450.         bitset.Write(start_bit + n, 8, value[n / 8]);
  451.     }
  452. }
  453.  
  454. std::string Protocol::DecodeHexstring(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length)
  455. {
  456.     std::string value;
  457.     char hex[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
  458.  
  459.     for (unsigned int n = 0; n < bit_length; n += 4)
  460.     {
  461.         unsigned long raw_bit_value = bitset.Read(start_bit + n, 4);
  462.  
  463.         if (raw_bit_value >= sizeof(hex))
  464.         {
  465.             value += '-';
  466.         }
  467.         else
  468.         {
  469.             value += hex[raw_bit_value];
  470.         }
  471.     }
  472.  
  473.     return value;
  474. }
  475.  
  476. void Protocol::EncodeHexstring(common::Bitset& bitset, unsigned int start_bit, unsigned int bit_length, std::string value)
  477. {
  478.     long ascii_value;
  479.  
  480.     for (unsigned int n = 0; n < bit_length; n += 4)
  481.     {
  482.         ascii_value = boost::lexical_cast<long>((long)std::toupper(value[n / 4]));
  483.  
  484.         if (48 <= ascii_value && ascii_value <= 57)
  485.         {
  486.             bitset.Write(start_bit + n, 4, ascii_value - 48);
  487.         }
  488.         else if (65 <= ascii_value && ascii_value <= 70)
  489.         {
  490.             bitset.Write(start_bit + n, 4, ascii_value - 65);
  491.         }
  492.         else
  493.         {
  494.             throw std::runtime_error("This is not an hex string, " + value);
  495.         }
  496.     }
  497. }
  498.  
  499. }; // namespace can
  500. }; // namespace atom
  501.