Subversion Repositories HomeAutomation

Rev

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

Rev Author Line No. Line
1309 runge 1 
/*
2 
 * CanNet.cpp
3 
 *
4 
 *  Created on: Apr 27, 2009
5 
 *      Author: Mattias Runge
6 
 */
7 
 
8 
#include "CanNet.h"
1403 runge 9 
 
1309 runge 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
1403 runge 18 
CanNet::CanNet(string address, unsigned int port) : Subscriber(false)
1309 runge 19 
{
20 
    LOG.setName("CanNetSubscriber");
21 
 
1403 runge 22 
    this->udp_server = net::UdpServer::getInstance(port);
23 
    this->udp_server->connect(boost::bind(&CanNet::Slot_OnNewData, this, _1, _2));
1309 runge 24 
 
1403 runge 25 
    this->endpoint_ = this->udp_server->getEndpoint(address);
1309 runge 26 
 
1403 runge 27 
    this->SendPing();
1309 runge 28 
 
1403 runge 29 
    this->Start();
1309 runge 30 
}
31 
 
32 
CanNet::~CanNet()
33 
{
34 
}
35 
 
1403 runge 36 
ByteList CanNet::BuildPacket(unsigned int id, ByteList data)
1309 runge 37 
{
1403 runge 38 
    ByteList packet;
1309 runge 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 
 
1403 runge 69 
void CanNet::SendPing()
1309 runge 70 
{
1403 runge 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)
1309 runge 80 
    {
1314 runge 81 
        LOG.warn("Received packet of length " + convert::int2string(this->myBuffer.size() + 2) + ", should be 17.");
1309 runge 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 
 
1323 runge 102 
    Header header(buffer);
1309 runge 103 
 
1328 runge 104 
    Message::pointer message = boost::make_shared<Message>(header);
1309 runge 105 
 
1328 runge 106 
    message->readBits(buffer);
107 
 
108 
    this->put(message);
109 
 
1309 runge 110 
    this->myBuffer.clear();
111 
}
112 
 
1403 runge 113 
void CanNet::OnMessage(Message::Pointer message)
1309 runge 114 
{
115 
    LOG.debug("update called");
116 
 
117 
    BitBuffer buffer;
118 
 
1323 runge 119 
    message->getHeader().writeBits(buffer);
120 
 
1309 runge 121 
    unsigned long id;
1326 runge 122 
    buffer.read(32, id);
1309 runge 123 
 
1323 runge 124 
    buffer.clear();
125 
 
126 
    message->writeBits(buffer);
127 
 
128 
    byte_list data = buffer.getAsBytes();
129 
 
1311 runge 130 
    this->myUdpServer->sendTo(this->myEndpoint, buildPacket(id, data));
1309 runge 131 
}
132 
 
1403 runge 133 
void CanNet::Slot_OnNewData(const udp::endpoint &sender, const ByteList &bytes)
1309 runge 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()));
1403 runge 137 
    if (sender.address() == this->enporint_.address()) // Port is wrong sometimes, therefor we only compare address
1309 runge 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 
            {
1403 runge 145 
                this->buffer_.clear();
1309 runge 146 
            }
147 
            else if (bytes[n] == PACKET_END)
148 
            {
1403 runge 149 
                this->ProcessBuffer();
1309 runge 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]));
1403 runge 158 
                this->buffer_.push_back(bytes[n]);
1309 runge 159 
            }
160 
        }
161 
    }
162 
    else
163 
    {
164 
        LOG.debug("This message was not for me.");
165 
    }
166 
}
167 
 
168 
}
169 
}