Subversion Repositories HomeAutomation

Rev

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