Refacor whole structure
[opengigevision:opengigevision.git] / Gvcp.cpp
1 #include <boost/date_time/posix_time/posix_time.hpp>
2
3 #include "GigEVision.h"
4 #include "GenICam.h"
5 #include "Gvcp.h"
6
7 GvcpManager::GvcpManager(GigEVision& gev):
8     m_gev(gev),
9     m_sock(m_gev.Service(), boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)),
10     m_heartbeatTimer(new boost::asio::deadline_timer(gev.Service(), boost::posix_time::millisec(700))),
11     m_nMsgNr(1),
12     m_bHeartbeatRun(false)
13 {
14
15 }
16
17 void GvcpManager::Connect(const std::string& sAddr, const std::string& sPort)
18 {
19   m_nMsgNr = 1;
20   boost::asio::ip::udp::resolver resolver(m_gev.Service());
21
22   boost::asio::ip::udp::resolver_query query(boost::asio::ip::udp::v4(), sAddr, sPort);
23   m_it = resolver.resolve(query);
24 }
25
26 GvcpManager::~GvcpManager()
27 {
28   m_bHeartbeatRun = false;
29   // TODO: add wait for finish
30 }
31
32 uint32_t GvcpManager::Read(const std::string& sKey)
33 {
34   return Read(m_gev.GenICam().GetAddress(sKey));
35 }
36
37 uint32_t GvcpManager::Read(uint32_t addr)
38 {
39   boost::array<uint8_t, 12> buff = {{ 0x42, 0x01, 0x00, 0x80, 0x00, 0x04 }};
40   ((uint16_t*)&buff)[3] = htons(m_nMsgNr++);
41   ((uint32_t*)&buff)[2] = htonl(addr);
42   m_sock.send_to(boost::asio::buffer(buff), *m_it);
43
44   boost::array<uint8_t, 256> recv_buf;
45   boost::asio::ip::udp::endpoint sender_endpoint;
46   if(m_sock.receive_from(boost::asio::buffer(recv_buf), sender_endpoint) != 12)
47     throw std::runtime_error("Error reading data from camera");
48
49   return htonl(((uint32_t*)&recv_buf)[2]);
50 }
51
52 std::vector<uint8_t> GvcpManager::ReadBlock(const std::string& sKey, uint32_t nSize)
53 {
54   // todo: expensive duplicate copy of memory, perhaps change interface to pass
55   // the vector as parameter by reference
56   return ReadBlock(m_gev.GenICam().GetAddress(sKey), nSize);
57 }
58
59 std::vector<uint8_t> GvcpManager::ReadBlock(uint32_t addr, uint32_t nSize)
60 {
61   static const int cnHdLen = 12;
62   boost::array<uint8_t, 16> buff = {{ 0x42, 0x01, 0x00, 0x84, 0x00, 0x08 }};
63   std::vector<uint8_t> retVec;
64   boost::array<uint8_t, 528> recv_buf;
65   size_t len = 0;
66
67   do
68   {
69     ((uint16_t*)&buff)[3] = htons(m_nMsgNr++);
70     ((uint32_t*)&buff)[2] = htonl(addr + len);
71     ((uint32_t*)&buff)[3] = htonl((nSize-len < recv_buf.size()-cnHdLen ? nSize-len: recv_buf.size()-cnHdLen));
72     m_sock.send_to(boost::asio::buffer(buff), *m_it);
73
74     boost::asio::ip::udp::endpoint sender_endpoint;
75     size_t nRead = m_sock.receive_from(boost::asio::buffer(recv_buf), sender_endpoint) - cnHdLen;
76     len += nRead;
77     std::copy(recv_buf.begin() + cnHdLen, recv_buf.begin() + cnHdLen + nRead, std::back_inserter(retVec));
78   }
79   while(len < nSize);
80
81   return retVec;
82 }
83
84 bool GvcpManager::Write(const std::string& sKey, uint32_t nVal)
85 {
86   return Write(m_gev.GenICam().GetAddress(sKey), nVal);
87 }
88
89 bool GvcpManager::Write(uint32_t addr, uint32_t nVal)
90 {
91   boost::array<uint8_t, 16> buff = {{ 0x42, 0x01, 0x00, 0x82, 0x00, 0x08 }};
92   ((uint16_t*)&buff)[3] = htons(m_nMsgNr++);
93   ((uint32_t*)&buff)[2] = htonl(addr);
94   ((uint32_t*)&buff)[3] = htonl(nVal);
95   m_sock.send_to(boost::asio::buffer(buff), *m_it);
96
97   boost::array<uint8_t, 12> recv_buf;
98   boost::asio::ip::udp::endpoint sender_endpoint;
99   if(m_sock.receive_from(boost::asio::buffer(recv_buf), sender_endpoint) != 12)
100     return false;
101   else
102     return recv_buf[11] == 0x01;
103 }
104
105 bool GvcpManager::RequestResend(uint32_t nImage, uint32_t nStartPack, uint32_t nEndPack)
106 {
107   boost::array<uint8_t, 20> buff = {{ 0x42, 0x00, 0x00, 0x40, 0x00, 0x0c }};
108   ((uint16_t*)&buff)[3] = htons(m_nMsgNr++);
109   ((uint32_t*)&buff)[2] = htonl(nImage);
110   ((uint32_t*)&buff)[3] = htonl(nStartPack);
111   ((uint32_t*)&buff)[4] = htonl(nEndPack);
112   m_sock.send_to(boost::asio::buffer(buff), *m_it);
113
114   boost::array<uint8_t, 12> recv_buf;
115   boost::asio::ip::udp::endpoint sender_endpoint;
116   if(m_sock.receive_from(boost::asio::buffer(recv_buf), sender_endpoint) != 12)
117     return false;
118   else
119     return recv_buf[11] == 0x01;
120 }
121
122 boost::asio::ip::address_v4 GvcpManager::FindCam()
123 {
124   boost::array<uint8_t, 8> buff = {{ 0x42, 0x01, 0x00, 0x02, 0x00, 0x00 }};
125   ((uint16_t*)&buff)[3] = htons(m_nMsgNr++);
126   m_sock.send_to(boost::asio::buffer(buff), *m_it);
127
128   boost::array<uint8_t, 256> recv_buf;
129   boost::asio::ip::udp::endpoint sender_endpoint;
130   m_sock.receive_from(boost::asio::buffer(recv_buf), sender_endpoint);
131   boost::asio::ip::address_v4 addr(recv_buf[47] + ((uint32_t(recv_buf[46]))<<8) +
132                                    ((uint32_t(recv_buf[45]))<<16) + ((uint32_t(recv_buf[44]))<<24));
133   return addr;
134 }
135
136 void GvcpManager::StartHeartbeat()
137 {
138   m_bHeartbeatRun = true;
139   m_heartbeatTimer->async_wait(boost::bind(&GvcpManager::Heartbeat, this));
140   m_ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &m_gev.Service()));
141 }
142
143 void GvcpManager::StopHeartbeat()
144 {
145   m_bHeartbeatRun = false;
146   m_ioThread.join();
147 }
148
149 void GvcpManager::Heartbeat()
150 {
151   if(m_bHeartbeatRun == true)
152   {
153     m_heartbeatTimer->expires_at(m_heartbeatTimer->expires_at() + boost::posix_time::millisec(700));
154     m_heartbeatTimer->async_wait(boost::bind(&GvcpManager::Heartbeat, this));
155   }
156 }