forked from manio/skymax-demo
-
Notifications
You must be signed in to change notification settings - Fork 0
/
skymax.cpp
232 lines (208 loc) · 4.86 KB
/
skymax.cpp
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
232
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "skymax.h"
#include "tools.h"
#include "main.h"
cSkymax::cSkymax(std::string devicename)
{
device = devicename;
status1[0] = 0;
status2[0] = 0;
mode = 0;
}
string *cSkymax::GetQpigsStatus()
{
m.lock();
string *result = new string(status1);
m.unlock();
return result;
}
string *cSkymax::GetQpiriStatus()
{
m.lock();
string *result = new string(status2);
m.unlock();
return result;
}
void cSkymax::SetMode(char newmode)
{
m.lock();
if (mode && newmode != mode)
ups_status_changed = true;
mode = newmode;
m.unlock();
}
int cSkymax::GetMode()
{
int result;
m.lock();
switch (mode)
{
case 'P': result = 1; break; // Power_On
case 'S': result = 2; break; // Standby
case 'L': result = 3; break; // Line
case 'B': result = 4; break; // Battery
case 'F': result = 5; break; // Fault
case 'H': result = 6; break; // Power_Saving
default: result = 0; break; // Unknown
}
m.unlock();
return result;
}
bool cSkymax::query(const char *cmd)
{
time_t started;
int fd;
int i = 0, n;
fd = open(this->device.data(), O_RDWR | O_NONBLOCK); // device is provided by program arg (usually /dev/hidraw0)
if (fd == -1)
{
lprintf("Skymax: Unable to open device file (errno=%d %s)", errno, strerror(errno));
sleep(10);
return false;
}
// Generating CRC for a command
uint16_t crc = cal_crc_half((uint8_t*)cmd, strlen(cmd));
n = strlen(cmd);
memcpy(&buf, cmd, n);
lprintf("SKYMAX: Current CRC: %X %X", crc >> 8, crc & 0xff);
buf[n++] = crc >> 8;
buf[n++] = crc & 0xff;
buf[n++] = 0x0d;
// Send a command
write(fd, &buf, n);
time(&started);
// Instead of using a fixed size for expected response length, lets find it
// by searching for the first returned <cr> char instead.
char *startbuf = 0;
char *endbuf = 0;
do
{
// According to protocol manual, it appears no query should ever exceed 150 byte size in response
n = read(fd, (void*)buf+i, 120 - i);
if (n < 0)
{
if (time(NULL) - started > 8) // Wait 8 secs before timeout
{
lprintf("SKYMAX: %s read timeout", cmd);
break;
}
else
{
usleep(10);
continue;
}
}
i += n;
startbuf = (char *)&buf[0];
endbuf = strchr(startbuf, '\r');
//lprintf("SKYMAX: %s Current buffer: %s", cmd, startbuf);
} while (endbuf == NULL); // Still haven't found end <cr> char as long as pointer is null
close(fd);
int replysize = endbuf - startbuf + 1;
lprintf("SKYMAX: Found <cr> at byte: %d", replysize);
if (buf[0]!='(' || buf[replysize-1]!=0x0d)
{
lprintf("SKYMAX: %s: incorrect start/stop bytes. Buffer: %s", cmd, buf);
return false;
}
if (!(CheckCRC(buf, replysize)))
{
lprintf("SKYMAX: %s: CRC Failed! Reply size: %d Buffer: %s", cmd, replysize, buf);
return false;
}
buf[replysize-3] = '\0'; // Null-terminating on first CRC byte
lprintf("SKYMAX: %s: %d bytes read: %s", cmd, i, buf);
lprintf("SKYMAX: %s query finished", cmd);
return true;
}
void cSkymax::poll()
{
int n,j;
while (true)
{
// Reading mode
if (!ups_qmod_changed)
{
if (query("QMOD"))
{
SetMode(buf[1]);
ups_qmod_changed = true;
}
}
// Reading QPIGS status
if (!ups_qpigs_changed)
{
if (query("QPIGS"))
{
m.lock();
strcpy(status1, (const char*)buf+1);
m.unlock();
ups_qpigs_changed = true;
}
}
// Reading QPIRI status
if (!ups_qpiri_changed)
{
if (query("QPIRI"))
{
m.lock();
strcpy(status2, (const char*)buf+1);
m.unlock();
ups_qpiri_changed = true;
}
}
sleep(5);
}
}
void cSkymax::ExecuteCmd(const string cmd)
{
// Sending any command raw
if (query(cmd.data()))
{
m.lock();
strcpy(status2, (const char*)buf+1);
m.unlock();
}
}
uint16_t cSkymax::cal_crc_half(uint8_t *pin, uint8_t len)
{
uint16_t crc;
uint8_t da;
uint8_t *ptr;
uint8_t bCRCHign;
uint8_t bCRCLow;
uint16_t crc_ta[16]=
{
0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef
};
ptr=pin;
crc=0;
while(len--!=0)
{
da=((uint8_t)(crc>>8))>>4;
crc<<=4;
crc^=crc_ta[da^(*ptr>>4)];
da=((uint8_t)(crc>>8))>>4;
crc<<=4;
crc^=crc_ta[da^(*ptr&0x0f)];
ptr++;
}
bCRCLow = crc;
bCRCHign= (uint8_t)(crc>>8);
if(bCRCLow==0x28||bCRCLow==0x0d||bCRCLow==0x0a)
bCRCLow++;
if(bCRCHign==0x28||bCRCHign==0x0d||bCRCHign==0x0a)
bCRCHign++;
crc = ((uint16_t)bCRCHign)<<8;
crc += bCRCLow;
return(crc);
}
bool cSkymax::CheckCRC(unsigned char *data, int len)
{
uint16_t crc = cal_crc_half(data, len-3);
return data[len-3]==(crc>>8) && data[len-2]==(crc&0xff);
}