Subversion Repositories HomeAutomation

Rev

Rev 1328 | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1. /*
  2.  * CanNet.cpp
  3.  *
  4.  *  Created on: Apr 27, 2009
  5.  *      Author: Mattias Runge
  6.  */
  7.  
  8. #include "CanNet.h"
  9.  
  10. #include <iostream>
  11.  
  12. namespace atom {
  13. namespace subscribers {
  14.  
  15. // TODO Fix input on the form of a URL:
  16. // ex. udp://192.168.1.250:1100
  17. // ex. dev://dev/ttyUSB0:57000
  18. CanNet::CanNet(string address, unsigned int port) : Subscriber(false)
  19. {
  20.     LOG.setName("CanNetSubscriber");
  21.  
  22.     this->udp_server = net::UdpServer::getInstance(port);
  23.     this->udp_server->connect(boost::bind(&CanNet::Slot_OnNewData, this, _1, _2));
  24.  
  25.     this->endpoint_ = this->udp_server->getEndpoint(address);
  26.  
  27.     this->SendPing();
  28.  
  29.     this->Start();
  30. }
  31.  
  32. CanNet::~CanNet()
  33. {
  34. }
  35.  
  36. ByteList CanNet::BuildPacket(unsigned int id, ByteList data)
  37. {
  38.     ByteList packet;
  39.  
  40.     packet.push_back((unsigned char) PACKET_START);
  41.  
  42.     packet.push_back((unsigned char) (id & 0xFF));
  43.     packet.push_back((unsigned char) ((id >> 8) & 0xFF));
  44.     packet.push_back((unsigned char) ((id >> 16) & 0xFF));
  45.     packet.push_back((unsigned char) ((id >> 24) & 0xFF));
  46.  
  47.     packet.push_back((unsigned char) 1); // Ext
  48.     packet.push_back((char) 0); // Rtr
  49.  
  50.     packet.push_back((unsigned char) data.size());
  51.  
  52.     for (unsigned char n = 0; n < 8; n++)
  53.     {
  54.         if (n < data.size())
  55.         {
  56.             packet.push_back(data[n]);
  57.         }
  58.         else
  59.         {
  60.             packet.push_back(' ');
  61.         }
  62.     }
  63.  
  64.     packet.push_back((unsigned char) PACKET_END);
  65.  
  66.     return packet;
  67. }
  68.  
  69. void CanNet::SendPing()
  70. {
  71.     ByteList data(1, (unsigned char) PACKET_PING);
  72.  
  73.     LOG.info("Sending ping packet...");
  74.     this->myUdpServer->sendTo(this->myEndpoint, data);
  75. }
  76.  
  77. void CanNet::ProcessBuffer()
  78. {
  79.     if (this->buffer_.size() != 15)
  80.     {
  81.         LOG.warn("Received packet of length " + convert::int2string(this->myBuffer.size() + 2) + ", should be 17.");
  82.         this->myBuffer.clear();
  83.         return;
  84.     }
  85.  
  86.     byte_list data;
  87.  
  88.     data.push_back(this->myBuffer[0]);
  89.     data.push_back(this->myBuffer[1]);
  90.     data.push_back(this->myBuffer[2]);
  91.     data.push_back(this->myBuffer[3]);
  92.  
  93.     unsigned char length = this->myBuffer[4];
  94.  
  95.     for (unsigned char n = 0; n < length; n++)
  96.     {
  97.         data.push_back(this->myBuffer[5 + n]);
  98.     }
  99.  
  100.     BitBuffer buffer(data);
  101.  
  102.     Header header(buffer);
  103.  
  104.     Message::pointer message = boost::make_shared<Message>(header);
  105.  
  106.     message->readBits(buffer);
  107.  
  108.     this->put(message);
  109.  
  110.     this->myBuffer.clear();
  111. }
  112.  
  113. void CanNet::OnMessage(Message::Pointer message)
  114. {
  115.     LOG.debug("update called");
  116.  
  117.     BitBuffer buffer;
  118.  
  119.     message->getHeader().writeBits(buffer);
  120.  
  121.     unsigned long id;
  122.     buffer.read(32, id);
  123.  
  124.     buffer.clear();
  125.  
  126.     message->writeBits(buffer);
  127.  
  128.     byte_list data = buffer.getAsBytes();
  129.  
  130.     this->myUdpServer->sendTo(this->myEndpoint, buildPacket(id, data));
  131. }
  132.  
  133. void CanNet::Slot_OnNewData(const udp::endpoint &sender, const ByteList &bytes)
  134. {
  135.     //LOG.debug(udp::endpoint(sender).address().to_string() + "==" + this->myEndpoint.address().to_string());
  136.     //LOG.debug(itos(udp::endpoint(sender).port()) + "==" + itos(this->myEndpoint.port()));
  137.     if (sender.address() == this->enporint_.address()) // Port is wrong sometimes, therefor we only compare address
  138.     {
  139.         LOG.debug("Got data.");
  140.  
  141.         for (unsigned int n = 0; n < bytes.size(); n++)
  142.         {
  143.             if (bytes[n] == PACKET_START)
  144.             {
  145.                 this->buffer_.clear();
  146.             }
  147.             else if (bytes[n] == PACKET_END)
  148.             {
  149.                 this->ProcessBuffer();
  150.             }
  151.             else if (bytes[n] == PACKET_PING)
  152.             {
  153.                 LOG.info("Received pong.");
  154.             }
  155.             else
  156.             {
  157.                 //LOG.info("Received byte. " + itos(bytes[n]));
  158.                 this->buffer_.push_back(bytes[n]);
  159.             }
  160.         }
  161.     }
  162.     else
  163.     {
  164.         LOG.debug("This message was not for me.");
  165.     }
  166. }
  167.  
  168. }
  169. }
  170.