forked from smartgridadsc/RGOOSE_RSV
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ied_utils.hpp
231 lines (198 loc) · 6.52 KB
/
ied_utils.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
/* A collection of data structure and functions for IED operations/debugging */
// GOOSE/SV Data to be tracked per sending/receiving cycle
struct GooseSvData
{
std::string cbName{};
std::string cbType{};
std::string appID{};
std::string multicastIP{};
unsigned int prev_spduNum{0};
unsigned int s_value{0};
// Specific to GOOSE
std::string datSetName{};
unsigned int goose_counter{0};
unsigned int prev_stNum_Value{0};
unsigned int prev_sqNum_Value{0};
unsigned int prev_numDatSetEntries{0};
std::vector<unsigned char> prev_allData_Value{};
// Specific to SV (Based on IEC 61850-9-2 Light Edition (LE) implementation)
unsigned int prev_smpCnt_Value{0};
std::vector<unsigned char> prev_seqOfData_Value{};
unsigned int sv_counter{0};
};
typedef union {
float f;
struct
{
// Order is important.
// Here the members of the union data structure
// use the same memory (32 bits).
// The ordering is taken
// from the LSB to the MSB.
unsigned int mantissa : 23;
unsigned int exponent : 8;
unsigned int sign : 1;
} raw;
} IEEEfloat;
// IPv4 address on ifname is saved into ifreq structure (passed by reference): ifr
void getIPv4Add(struct ifreq &ifr, const char* ifname)
{
int fd = socket(AF_INET, SOCK_DGRAM, 0);
ifr.ifr_addr.sa_family = AF_INET;
strncpy(ifr.ifr_name, ifname, IFNAMSIZ-1);
ioctl(fd, SIOCGIFADDR, &ifr);
close(fd);
}
/* Function to iterate over the contents of a vector
* and print all elements using indexing
*/
template <typename T>
void display_vector(const std::vector<T> &vec)
{
if (vec.size() > 0)
{
std::cout << "[ ";
for (size_t i = 0; i < (vec.size() - 1) ; i++)
{
std::cout << vec[i] << ", ";
}
std::cout << vec[vec.size() - 1] << " ]";
}
else
{
std::cout << "Vector is empty!\n";
}
}
/* Function to print values of variables in a given Control Block */
void printControlBlock(const ControlBlock &ctrl_blk)
{
std::cout << "\tHost IED \t\t\t= " << ctrl_blk.hostIED << '\n';
std::cout << "\tControl Block type \t\t= " << ctrl_blk.cbType << '\n';
std::cout << "\tMulticast IP Address \t\t= " << ctrl_blk.multicastIP << '\n';
std::cout << "\tAPP ID \t\t\t\t= " << ctrl_blk.appID << '\n';
std::cout << "\tVLAN ID \t\t\t= " << ctrl_blk.vlanID << '\n';
std::cout << "\tFully qualified cbName \t\t= " << ctrl_blk.cbName << '\n';
std::cout << "\tFully qualified datSetName \t= " << ctrl_blk.datSetName << '\n';
std::cout << "\tInformation Model \t\t= ";
display_vector(ctrl_blk.datSetVector);
std::cout << '\n';
std::cout << "\tSubscribing IED(s) \t\t= ";
display_vector(ctrl_blk.subscribingIEDs);
}
/* Function to print a std::vector collection of Control Blocks */
void printCtrlBlkVect(const std::vector<ControlBlock> &vector_of_ctrl_blks)
{
std::cout << "Total of " << vector_of_ctrl_blks.size() << " Control Block(s) in the following vector:\n";
std::cout << " {\n";
for (size_t i = 0; i < vector_of_ctrl_blks.size() ; i++)
{
printControlBlock(vector_of_ctrl_blks[i]);
if ( i != (vector_of_ctrl_blks.size() - 1) )
{
std::cout << "\n ,\n";
}
}
std::cout << "\n }\n\n";
}
// Returns the number of bytes to hold a given UINT32 number
unsigned char getUINT32Length(unsigned int num)
{
if (num < 0x10000)
{
if (num < 0x100)
return 0x01;
else
return 0x02;
}
else
{
if (num < 0x1000000)
return 0x03;
else
return 0x04;
}
}
// Converts a given UINT32 number into a vector of up to 4 bytes (as output parameter)
void convertUINT32IntoBytes(unsigned int num, std::vector<unsigned char> &vecOut)
{
const size_t byte_count{getUINT32Length(num)};
constexpr std::uint_fast32_t mask0 { 0xFF000000 };
constexpr std::uint_fast32_t mask1 { 0x00FF0000 };
constexpr std::uint_fast32_t mask2 { 0x0000FF00 };
constexpr std::uint_fast32_t mask3 { 0x000000FF };
if (byte_count == 4)
vecOut.push_back(static_cast<unsigned char>((mask0 & num) >> 24));
if (byte_count >= 3)
vecOut.push_back(static_cast<unsigned char>((mask1 & num) >> 16));
if (byte_count >= 2)
vecOut.push_back(static_cast<unsigned char>((mask2 & num) >> 8));
if (byte_count >= 1)
vecOut.push_back(static_cast<unsigned char>((mask3 & num)));
assert (vecOut.size() >= 1 && vecOut.size() <= 4);
}
void getHexFromBinary(std::string binaryString, std::vector<unsigned char> &seqOfData_Value)
{
int result = 0;
for(size_t count = 0; count < binaryString.length() ; ++count)
{
result *=2;
result += binaryString[count]=='1'? 1 :0;
}
std::stringstream ss;
ss << "0x" << std::hex << std::setw(2) << std::setfill('0') << result;
unsigned int c;
while (ss >> c)
{
seqOfData_Value.push_back(c);
}
}
void convertBinary(int n, int i, std::vector<std::string> &buffer)
{
int k;
for (k = i - 1; k >= 0; k--) {
if ((n >> k) & 1)
{
buffer.push_back("1");
//cout << "1";
}
else
{
buffer.push_back("0");
//cout << "0";
}
}
}
unsigned int convertToInt(std::vector<unsigned int> dataBytes, int low,int high)
{
unsigned int f=0,i;
for(i = high; i>= low; i--)
{
f = f + dataBytes[i] * pow(2, high - i);
}
return f;
}
void convertIEEE(IEEEfloat var, std::vector<unsigned char> &seqOfData_Value)
{
std::vector<std::string> buffer{};
// add sign bit
if(var.raw.sign){
buffer.push_back("1");
}
else
{
buffer.push_back("0");
}
// convert float to binary
convertBinary(var.raw.exponent, 8, buffer);
convertBinary(var.raw.mantissa, 23, buffer);
for (int i =0; i< buffer.size(); i++)
{
if ((i+1) % 8 == 0)
{
std::string binaryString = buffer[i-7] + buffer[i-6] + buffer[i-5] + buffer[i-4] +
buffer[i-3] + buffer[i-2] + buffer[i-1] + buffer[i];
//push hex data of float value into seqOfData_Value 8 bit by 8 bit.
getHexFromBinary(binaryString, seqOfData_Value);
}
}
}