Subversion Repositories HomeAutomation

Rev

Rev 1595 | Rev 1597 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | SVN | RSS feed

Rev 1595 Rev 1596
Line 24... Line 24...
24 
 
 24 
 
25 
#include <boost/algorithm/string.hpp>
 25 
#include <boost/algorithm/string.hpp>
26 
#include <boost/lexical_cast.hpp>
 26 
#include <boost/lexical_cast.hpp>
27 
 
 27 
 
28 
#include "net/Manager.h"
 28 
#include "net/Manager.h"
- 
 
 29 
 
- 
 
 30 
#include "broker/Manager.h"
- 
 
 31 
 
- 
 
 32 
#include "Protocol.h"
- 
 
 33 
#include "Message.h"
- 
 
 34 
 
- 
 
 35 
#include "type/Bitset.h"
29 
 
 36 
 
30 
namespace atom {
 37 
namespace atom {
31 
namespace can {
 38 
namespace can {
32 
 
 39 
 
- 
 
 40 
enum
- 
 
 41 
{
- 
 
 42 
    PACKET_START = 253,
- 
 
 43 
    PACKET_END   = 250,
- 
 
 44 
    PACKET_PING  = 251
- 
 
 45 
};
- 
 
 46 
   
33 
Network::Network(std::string address): Subscriber(false), LOG("can::Network")
 47 
Network::Network(std::string address): Subscriber(false), LOG("can::Network"), buffer_(2048)
34 
{
 48 
{
35 
    this->address_ = address;
 49 
    this->address_ = address;
36 
    this->client_id_ = 0;
 50 
    this->client_id_ = 0;
37 
       
 51 
       
38 
    net::Manager::Instance()->ConnectSlots(net::Client::SignalOnNewState::slot_type(&Network::SlotOnNewState, this, _1, _2, _3).track(this->tracker_),
 52 
    net::Manager::Instance()->ConnectSlots(net::Client::SignalOnNewState::slot_type(&Network::SlotOnNewState, this, _1, _2, _3).track(this->tracker_),
Line 46... Line 60...
46 
    boost::algorithm::split(parts, address, boost::is_any_of(":"), boost::algorithm::token_compress_off);
 60 
    boost::algorithm::split(parts, address, boost::is_any_of(":"), boost::algorithm::token_compress_off);
47 
   
 61 
   
48 
    if (parts.size() < 3)
 62 
    if (parts.size() < 3)
49 
    {
 63 
    {
50 
        LOG.Error("Malformed address string: " + address);
 64 
        LOG.Error("Malformed address string: " + address);
51 
        return;
 65 
        return;
52 
    }
 66 
    }
53 
   
 67 
   
54 
    boost::algorithm::to_lower(parts[0]);
 68 
    boost::algorithm::to_lower(parts[0]);
55 
   
 69 
   
56 
    if (parts[0] == "udp")
 70 
    if (parts[0] == "udp")
57 
    {
 71 
    {
58 
        this->protocol_ = net::PROTOCOL_UDP;
 72 
        this->protocol_ = net::PROTOCOL_UDP;
59 
    }
 73 
    }
60 
    else if (parts[0] == "serial")
 74 
    else if (parts[0] == "serial")
61 
    {
 75 
    {
62 
        this->protocol_ = net::PROTOCOL_SERIAL;
 76 
        this->protocol_ = net::PROTOCOL_SERIAL;
63 
    }
 77 
    }
64 
    else
 78 
    else
65 
    {
 79 
    {
66 
        LOG.Error("Unknown protocol, only support udp and serial, got " + parts[0]);
 80 
        LOG.Error("Unknown protocol, only support udp and serial, got " + parts[0]);
67 
        return;
 81 
        return;
68 
    }
 82 
    }
69 
   
 83 
   
70 
    this->address_ = parts[1];
 84 
    this->address_ = parts[1];
71 
    this->port_or_baud_ = boost::lexical_cast<unsigned int>(parts[2]);
 85 
    this->port_or_baud_ = boost::lexical_cast<unsigned int>(parts[2]);
72 
   
 86 
   
73 
    try
 87 
    try
74 
    {
 88 
    {
75 
        this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);
 89 
        this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);
76 
        LOG.Info("Connected to CAN network at " + address);
 90 
        LOG.Info("Connected to " + address);
77 
       
 91 
       
78 
        // TODO: Send ping
 92 
        LOG.Info("Sending ping.");
- 
 
 93 
        type::Byteset buffer(1);
- 
 
 94 
       
- 
 
 95 
        buffer[0] = PACKET_PING;
- 
 
 96 
       
- 
 
 97 
        net::Manager::Instance()->SendTo(this->client_id_, buffer);
79 
    }
 98 
    }
80 
    catch (std::exception e)
 99 
    catch (std::exception e)
81 
    {
 100 
    {
82 
        LOG.Error(e.what());
 101 
        LOG.Error(e.what());
83 
    }
 102 
    }
84 
}
 103 
}
85 
 
 104 
 
86 
Network::~Network()
 105 
Network::~Network()
87 
{
 106 
{
88 
    net::Manager::Instance()->Disconnect(this->client_id_);
 107 
    net::Manager::Instance()->Disconnect(this->client_id_);
89 
}
 108 
}
90 
 
 109 
 
91 
void Network::SlotOnNewData(net::ClientId client_id, net::ServerId server_id, net::Buffer data)
 110 
void Network::SlotOnNewData(net::ClientId client_id, net::ServerId server_id, type::Byteset data)
92 
{
 111 
{
93 
    if (client_id == this->client_id_)
 112 
    if (client_id == this->client_id_)
94 
    {
 113 
    {
- 
 
 114 
        type::Byteset temp_buffer = data;
95 
        this->io_service_.post(boost::bind(&Network::SlotOnNewDataHandler, this, client_id, server_id, data));
 115 
        this->io_service_.post(boost::bind(&Network::SlotOnNewDataHandler, this, client_id, server_id, temp_buffer));
96 
    }
 116 
    }
97 
}
 117 
}
98 
 
 118 
 
99 
void Network::SlotOnNewState(net::ClientId client_id, net::ServerId server_id, net::ClientState client_state)
 119 
void Network::SlotOnNewState(net::ClientId client_id, net::ServerId server_id, net::ClientState client_state)
100 
{
 120 
{
101 
    if (client_id == this->client_id_)
 121 
    if (client_id == this->client_id_)
102 
    {
 122 
    {
103 
        this->io_service_.post(boost::bind(&Network::SlotOnNewStateHandler, this, client_id, server_id, client_state));
 123 
        this->io_service_.post(boost::bind(&Network::SlotOnNewStateHandler, this, client_id, server_id, client_state));
104 
    }
 124 
    }
105 
}
 125 
}
- 
 
 126 
 
- 
 
 127 
void Network::SlotOnTimeout(timer::TimerId timer_id)
- 
 
 128 
{
- 
 
 129 
    if (timer_id == this->timer_id_)
- 
 
 130 
    {
- 
 
 131 
        this->io_service_.post(boost::bind(&Network::SlotOnTimeoutHandler, this, timer_id));
- 
 
 132 
    }
- 
 
 133 
}
106 
 
 134 
 
107 
void Network::SlotOnMessageHandler(broker::Message::Pointer message)
 135 
void Network::SlotOnMessageHandler(broker::Message::Pointer message)
108 
{
 136 
{
- 
 
 137 
    if (message->GetType() == broker::Message::CAN_MESSAGE)
- 
 
 138 
    {
- 
 
 139 
        Message* payload = static_cast<Message*>(message->GetPayload().get());
- 
 
 140 
       
- 
 
 141 
       
109 
    // TODO: Send to network
 142 
       
- 
 
 143 
    }
110 
}
 144 
}
111 
 
 145 
 
112 
void Network::SlotOnNewDataHandler(net::ClientId client_id, net::ServerId server_id, net::Buffer data)
 146 
void Network::SlotOnNewDataHandler(net::ClientId client_id, net::ServerId server_id, type::Byteset data)
113 
{
 147 
{
- 
 
 148 
    static bool have_start = false;
- 
 
 149 
   
- 
 
 150 
    for (unsigned int n = 0; n < data.GetSize(); n++)
- 
 
 151 
    {
- 
 
 152 
        if (have_start)
- 
 
 153 
        {
- 
 
 154 
            if (data[n] == PACKET_END && this->buffer_.GetSize() == 15)
- 
 
 155 
            {
- 
 
 156 
                //LOG.Debug("Received packet end and size is 15.");
- 
 
 157 
                this->ProcessBuffer();
- 
 
 158 
                have_start = false;
- 
 
 159 
            }
- 
 
 160 
            else
- 
 
 161 
            {
- 
 
 162 
                this->buffer_.Append(data[n]);
- 
 
 163 
            }
- 
 
 164 
        }
- 
 
 165 
        else if (data[n] == PACKET_START)
- 
 
 166 
        {
- 
 
 167 
            //LOG.Debug("Received packet start.");
- 
 
 168 
            this->buffer_.Clear();
114 
    // TODO: Send to broker
 169 
            have_start = true;
- 
 
 170 
        }
- 
 
 171 
        else if (data[n] == PACKET_PING)
- 
 
 172 
        {
- 
 
 173 
            LOG.Info("Received pong.");
- 
 
 174 
        }
- 
 
 175 
    }
115 
}
 176 
}
116 
 
 177 
 
117 
void Network::SlotOnNewStateHandler(net::ClientId client_id, net::ServerId server_id, net::ClientState client_state)
 178 
void Network::SlotOnNewStateHandler(net::ClientId client_id, net::ServerId server_id, net::ClientState client_state)
118 
{
 179 
{
119 
    if (client_state == net::CLIENT_STATE_DISCONNECTED)
 180 
    if (client_state == net::CLIENT_STATE_DISCONNECTED)
120 
    {
 181 
    {
121 
        LOG.Warning("Got disconnected from CAN network, will try to reconnect...");
 182 
        LOG.Warning("Got disconnected, setting reconnect timer...");
122 
     
 183 
       
- 
 
 184 
        this->timer_id_ = timer::Manager::Instance()->Set(10000, true);
123 
        this->client_id_ = 0;
 185 
        this->client_id_ = 0;
- 
 
 186 
    }
124 
       
 187 
    else
- 
 
 188 
    {
- 
 
 189 
        LOG.Debug("Got state: " + boost::lexical_cast<std::string>((int)client_state));
- 
 
 190 
    }
- 
 
 191 
}
- 
 
 192 
 
- 
 
 193 
void Network::SlotOnTimeoutHandler(timer::TimerId timer_id)
- 
 
 194 
{
125 
        try
 195 
    try
126 
        {
 196 
    {
127 
            this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);
 197 
        this->client_id_ = net::Manager::Instance()->Connect(this->protocol_, this->address_, this->port_or_baud_);
128 
            LOG.Info("Connected to CAN network again.");
 198 
        LOG.Info("Connected again.");
- 
 
 199 
       
- 
 
 200 
        timer::Manager::Instance()->Cancel(timer_id);
- 
 
 201 
        this->timer_id_ = 0;
129 
        }
 202 
    }
130 
        catch (std::exception e)
 203 
    catch (std::exception e)
131 
        {
 204 
    {
132 
            LOG.Error(e.what());
 205 
        LOG.Error(e.what());
- 
 
 206 
        LOG.Warning("Will try again soon...");
- 
 
 207 
    }
- 
 
 208 
}
- 
 
 209 
 
- 
 
 210 
void Network::ProcessBuffer()
- 
 
 211 
{
- 
 
 212 
    try
- 
 
 213 
    {
- 
 
 214 
        std::string class_name = "";
- 
 
 215 
        std::string direction_name = "";
- 
 
 216 
        std::string module_name = "";
- 
 
 217 
        unsigned int id = 0;
- 
 
 218 
        std::string command_name = "";
- 
 
 219 
       
- 
 
 220 
        unsigned int class_id = (this->buffer_[3] >> 1) & 0x0F;
- 
 
 221 
       
- 
 
 222 
        //LOG.Debug("class_id=" + boost::lexical_cast<std::string>(class_id) + ", byte[3] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[3]));
- 
 
 223 
       
- 
 
 224 
        class_name = Protocol::Instance()->LookupClassName(class_id);
- 
 
 225 
       
- 
 
 226 
        //LOG.Debug("class_name=" + class_name);
- 
 
 227 
       
- 
 
 228 
        if (class_name == "nmt")
- 
 
 229 
        {
- 
 
 230 
            unsigned int command_id = this->buffer_[2];
- 
 
 231 
            //LOG.Debug("command_id=" + boost::lexical_cast<std::string>(command_id) + ", byte[2] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[2]));
- 
 
 232 
           
- 
 
 233 
            command_name = Protocol::Instance()->LookupNMTCommandName(command_id);
- 
 
 234 
            //LOG.Debug("command_name=" + command_name);
- 
 
 235 
        }
- 
 
 236 
        else
- 
 
 237 
        {
- 
 
 238 
            unsigned int direction_flag = this->buffer_[3] & 0x01;
- 
 
 239 
            //LOG.Debug("direction_flag=" + boost::lexical_cast<std::string>(direction_flag) + ", byte[3] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[3]));
- 
 
 240 
           
- 
 
 241 
            direction_name = Protocol::Instance()->LookupDirectionFlag(direction_flag);
- 
 
 242 
            //LOG.Debug("direction_name=" + direction_name);
- 
 
 243 
           
- 
 
 244 
            unsigned int module_id = this->buffer_[2];
- 
 
 245 
            //LOG.Debug("module_id=" + boost::lexical_cast<std::string>(module_id) + ", byte[2] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[2]));
- 
 
 246 
           
- 
 
 247 
            module_name = Protocol::Instance()->LookupModuleName(module_id);
- 
 
 248 
            //LOG.Debug("module_name=" + module_name);
- 
 
 249 
           
- 
 
 250 
            id = this->buffer_[1];
- 
 
 251 
            //LOG.Debug("id=" + boost::lexical_cast<std::string>(id) + ", byte[1] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[1]));
- 
 
 252 
           
- 
 
 253 
            unsigned int command_id = this->buffer_[0];
- 
 
 254 
            //LOG.Debug("command_id=" + boost::lexical_cast<std::string>(command_id) + ", byte[0] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[0]));
- 
 
 255 
           
- 
 
 256 
            command_name = Protocol::Instance()->LookupCommandName(command_id, module_name);
- 
 
 257 
            //LOG.Debug("command_name=" + command_name);
- 
 
 258 
        }
- 
 
 259 
 
- 
 
 260 
        Message* payload = new Message(class_name, direction_name, module_name, id, command_name);
- 
 
 261 
 
- 
 
 262 
        unsigned int length = this->buffer_[6];
- 
 
 263 
        //LOG.Debug("Data length = " + boost::lexical_cast<std::string>(length));
- 
 
 264 
       
- 
 
 265 
        type::Byteset data_set(length);
- 
 
 266 
       
- 
 
 267 
        for (unsigned int n = 0; n < length; n++)
- 
 
 268 
        {
- 
 
 269 
            //LOG.Debug("add byte[" + boost::lexical_cast<std::string>(n + 7) + "] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[n + 7]));
- 
 
 270 
            data_set.Append(this->buffer_[n + 7]);
- 
 
 271 
        }
- 
 
 272 
       
- 
 
 273 
        /*for (unsigned int n = 0; n < this->buffer_.GetSize(); n++)
- 
 
 274 
        {
- 
 
 275 
            LOG.Debug("byte[" + boost::lexical_cast<std::string>(n) + "] = " + boost::lexical_cast<std::string>((unsigned int)this->buffer_[n]));
- 
 
 276 
        }*/
- 
 
 277 
       
- 
 
 278 
        type::Bitset databits(data_set);
- 
 
 279 
       
- 
 
 280 
       
- 
 
 281 
        std::string temp = "";
- 
 
 282 
        for (unsigned int n = 0; n < databits.GetCount(); n ++)
- 
 
 283 
        {
- 
 
 284 
            temp += boost::lexical_cast<std::string>(databits.Get(n));
133 
        }
 285 
        }
- 
 
 286 
       
- 
 
 287 
        //LOG.Debug("databits = " + temp);
- 
 
 288 
       
- 
 
 289 
        xml::Node::NodeList variable_nodes;
- 
 
 290 
       
- 
 
 291 
        if (class_name == "nmt")
- 
 
 292 
        {
- 
 
 293 
            variable_nodes = Protocol::Instance()->GetNMTCommandVariables(command_name);
134 
    }
 294 
        }
135 
    else
 295 
        else
136 
    {
 296 
        {
- 
 
 297 
            variable_nodes = Protocol::Instance()->GetCommandVariables(command_name, module_name);
- 
 
 298 
        }
- 
 
 299 
       
- 
 
 300 
        for (unsigned int n = 0; n < variable_nodes.size(); n++)
- 
 
 301 
        {
- 
 
 302 
            unsigned int start_bit = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("start_bit"));
- 
 
 303 
            unsigned int bit_length = boost::lexical_cast<unsigned int>(variable_nodes[n].GetAttributeValue("bit_length"));
- 
 
 304 
            std::string type = variable_nodes[n].GetAttributeValue("type");
- 
 
 305 
           
137 
        LOG.Debug("Got state: " + boost::lexical_cast<std::string>((int)client_state));
 306 
            std::string value = boost::lexical_cast<std::string>(databits.Read(start_bit, bit_length));
- 
 
 307 
 
- 
 
 308 
            if (type == "enum")
- 
 
 309 
            {
- 
 
 310 
                value = variable_nodes[n].SelectChild("id", value).GetAttributeValue("name");
- 
 
 311 
            }
- 
 
 312 
           
- 
 
 313 
            // TODO handle all datatypes!
- 
 
 314 
           
- 
 
 315 
            //LOG.Debug("start_bit=" + boost::lexical_cast<std::string>(start_bit) + ",bit_length=" + boost::lexical_cast<std::string>(bit_length));
- 
 
 316 
           
- 
 
 317 
            payload->SetVariable(variable_nodes[n].GetAttributeValue("name"), value);
- 
 
 318 
            //LOG.Debug("Variable:" + variable_nodes[n].GetAttributeValue("name") + " = " + value);
- 
 
 319 
        }
- 
 
 320 
       
- 
 
 321 
        broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), this)));
- 
 
 322 
    }
- 
 
 323 
    catch (std::runtime_error& e)
- 
 
 324 
    {
- 
 
 325 
        LOG.Error("Malformed message received, " + std::string(e.what()));
138 
    }
 326 
    }
- 
 
 327 
   
- 
 
 328 
    this->buffer_.Clear();
139 
}
 329 
}
140 
   
 330 
   
141 
}; // namespace can
 331 
}; // namespace can
142 
}; // namespace atom
 332 
}; // namespace atom