Rev 674 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 674 | Rev 801 | ||
---|---|---|---|
1 | /*************************************************************************** |
1 | /*************************************************************************** |
2 | * Copyright (C) 2009 by Manuel Schrape * |
2 | * Copyright (C) 2009 by Manuel Schrape * |
3 | * manuel.schrape@gmx.de * |
3 | * manuel.schrape@gmx.de * |
4 | * * |
4 | * * |
5 | * This program is free software; you can redistribute it and/or modify * |
5 | * This program is free software; you can redistribute it and/or modify * |
6 | * it under the terms of the GNU General Public License as published by * |
6 | * it under the terms of the GNU General Public License as published by * |
7 | * the Free Software Foundation; either version 2 of the License. * |
7 | * the Free Software Foundation; either version 2 of the License. * |
8 | * * |
8 | * * |
9 | * This program is distributed in the hope that it will be useful, * |
9 | * This program is distributed in the hope that it will be useful, * |
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
12 | * GNU General Public License for more details. * |
12 | * GNU General Public License for more details. * |
13 | * * |
13 | * * |
14 | * You should have received a copy of the GNU General Public License * |
14 | * You should have received a copy of the GNU General Public License * |
15 | * along with this program; if not, write to the * |
15 | * along with this program; if not, write to the * |
16 | * Free Software Foundation, Inc., * |
16 | * Free Software Foundation, Inc., * |
17 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
17 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
18 | ***************************************************************************/ |
18 | ***************************************************************************/ |
19 | #include "HandlerMK.h" |
19 | #include "HandlerMK.h" |
20 | 20 | ||
21 | HandlerMK::HandlerMK() |
21 | HandlerMK::HandlerMK() |
22 | { |
22 | { |
23 | } |
23 | } |
24 | 24 | ||
25 | // Datensatz nach 16bit Integer |
25 | // Datensatz nach 16bit Integer |
26 | int HandlerMK::Data2Int(unsigned char Data[150] , int Start, bool is_signed) |
26 | int HandlerMK::Data2Int(unsigned char Data[160] , int Start, bool is_signed) |
27 | { |
27 | { |
28 | int Out = (Data[Start+1]<<8) | (Data[Start+0]); |
28 | int Out = (Data[Start+1]<<8) | (Data[Start+0]); |
29 | 29 | ||
30 | if ((Out > 32767) && (is_signed)) |
30 | if ((Out > 32767) && (is_signed)) |
31 | Out = Out - 65536; |
31 | Out = Out - 65536; |
32 | 32 | ||
33 | return Out; |
33 | return Out; |
34 | 34 | ||
35 | } |
35 | } |
36 | 36 | ||
37 | // Datensatz nach QString |
37 | // Datensatz nach QString |
38 | QString HandlerMK::Data2QString(unsigned char Data[150], int Start, int End) |
38 | QString HandlerMK::Data2QString(unsigned char Data[150], int Start, int End) |
39 | { |
39 | { |
40 | char String[150]; |
40 | char String[150]; |
41 | for (int a = Start; a < End; a++) |
41 | for (int a = Start; a < End; a++) |
42 | { |
42 | { |
43 | String[a - Start] = Data[a]; |
43 | String[a - Start] = Data[a]; |
44 | } |
44 | } |
45 | String[End - Start] = '\0'; |
45 | String[End - Start] = '\0'; |
46 | 46 | ||
47 | return QString(String); |
47 | return QString(String); |
48 | } |
48 | } |
49 | 49 | ||
50 | double HandlerMK::Int2Double(int32_t Wert, int Count) |
50 | double HandlerMK::Int2Double(int32_t Wert, int Count) |
51 | { |
51 | { |
52 | QString Temp, s_Wert; |
52 | QString Temp, s_Wert; |
53 | 53 | ||
54 | s_Wert = QString("%1").arg(Wert); |
54 | s_Wert = QString("%1").arg(Wert); |
55 | 55 | ||
56 | Temp = s_Wert.left(s_Wert.length() - Count) + QString(".") + s_Wert.right(Count); |
56 | Temp = s_Wert.left(s_Wert.length() - Count) + QString(".") + s_Wert.right(Count); |
57 | 57 | ||
58 | return Temp.toDouble(); |
58 | return Temp.toDouble(); |
59 | } |
59 | } |
60 | 60 | ||
61 | QString HandlerMK::add_CRC(QString TXString) |
61 | QString HandlerMK::add_CRC(QString TXString) |
62 | { |
62 | { |
63 | unsigned int tmpCRC = 0; |
63 | unsigned int tmpCRC = 0; |
64 | 64 | ||
65 | char *TXBuff; |
65 | char *TXBuff; |
66 | char CRC[2]; |
66 | char CRC[2]; |
67 | 67 | ||
68 | TXBuff = TXString.toLatin1().data(); |
68 | TXBuff = TXString.toLatin1().data(); |
69 | 69 | ||
70 | for(int i = 0; i < TXString.length(); i++) |
70 | for(int i = 0; i < TXString.length(); i++) |
71 | { |
71 | { |
72 | tmpCRC += TXBuff[i]; |
72 | tmpCRC += TXBuff[i]; |
73 | } |
73 | } |
74 | 74 | ||
75 | tmpCRC %= 4096; |
75 | tmpCRC %= 4096; |
76 | 76 | ||
77 | CRC[0] = '=' + tmpCRC / 64; |
77 | CRC[0] = '=' + tmpCRC / 64; |
78 | CRC[1] = '=' + tmpCRC % 64; |
78 | CRC[1] = '=' + tmpCRC % 64; |
79 | CRC[2] = '\0'; |
79 | CRC[2] = '\0'; |
80 | 80 | ||
81 | QString Return = TXString + QString(CRC); |
81 | QString Return = TXString + QString(CRC); |
82 | 82 | ||
83 | return Return; |
83 | return Return; |
84 | } |
84 | } |
85 | 85 | ||
86 | bool HandlerMK::Check_CRC(char *t_InData, int Length) |
86 | bool HandlerMK::Check_CRC(char *t_InData, int Length) |
87 | { |
87 | { |
88 | int CRC = 0; |
88 | int CRC = 0; |
89 | 89 | ||
90 | if (t_InData[1] == 127) |
90 | if (t_InData[1] == 127) |
91 | t_InData[1] = 0; |
91 | t_InData[1] = 0; |
92 | 92 | ||
93 | for(int i=0; i < Length-2; i++) |
93 | for(int i=0; i < Length-2; i++) |
94 | CRC+=t_InData[i]; |
94 | CRC+=t_InData[i]; |
95 | 95 | ||
96 | CRC = CRC % 4096; |
96 | CRC = CRC % 4096; |
97 | 97 | ||
98 | if(t_InData[Length - 2] != ('=' + (CRC / 64))) |
98 | if(t_InData[Length - 2] != ('=' + (CRC / 64))) |
99 | return false; |
99 | return false; |
100 | 100 | ||
101 | if(t_InData[Length - 1] != ('=' + CRC % 64)) |
101 | if(t_InData[Length - 1] != ('=' + CRC % 64)) |
102 | return false; |
102 | return false; |
103 | 103 | ||
104 | return true; |
104 | return true; |
105 | } |
105 | } |
106 | 106 | ||
107 | int HandlerMK::Decode_64(char *t_InData, int Length, unsigned char *t_OutData) |
107 | int HandlerMK::Decode_64(char *t_InData, int Length, unsigned char *t_OutData) |
108 | { |
108 | { |
109 | unsigned char a,b,c,d; |
109 | unsigned char a,b,c,d; |
110 | unsigned char ptr = 0; |
110 | unsigned char ptr = 0; |
111 | unsigned char x,y,z; |
111 | unsigned char x,y,z; |
112 | 112 | ||
113 | int Offset = 3; |
113 | int Offset = 3; |
114 | 114 | ||
115 | if (t_InData[Offset] == 0) |
115 | if (t_InData[Offset] == 0) |
116 | { |
116 | { |
117 | return 0; |
117 | return 0; |
118 | } |
118 | } |
119 | 119 | ||
120 | while(Length != 0) |
120 | while(Length != 0) |
121 | { |
121 | { |
122 | a = t_InData[Offset++] - '='; |
122 | a = t_InData[Offset++] - '='; |
123 | b = t_InData[Offset++] - '='; |
123 | b = t_InData[Offset++] - '='; |
124 | c = t_InData[Offset++] - '='; |
124 | c = t_InData[Offset++] - '='; |
125 | d = t_InData[Offset++] - '='; |
125 | d = t_InData[Offset++] - '='; |
126 | 126 | ||
127 | // if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
127 | // if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden |
128 | 128 | ||
129 | x = (a << 2) | (b >> 4); |
129 | x = (a << 2) | (b >> 4); |
130 | y = ((b & 0x0f) << 4) | (c >> 2); |
130 | y = ((b & 0x0f) << 4) | (c >> 2); |
131 | z = ((c & 0x03) << 6) | d; |
131 | z = ((c & 0x03) << 6) | d; |
132 | 132 | ||
133 | if(Length--) t_OutData[ptr++] = x; else break; |
133 | if(Length--) t_OutData[ptr++] = x; else break; |
134 | if(Length--) t_OutData[ptr++] = y; else break; |
134 | if(Length--) t_OutData[ptr++] = y; else break; |
135 | if(Length--) t_OutData[ptr++] = z; else break; |
135 | if(Length--) t_OutData[ptr++] = z; else break; |
136 | } |
136 | } |
137 | 137 | ||
138 | return ptr; |
138 | return ptr; |
139 | } |
139 | } |
140 | 140 | ||
141 | // Base64 Encoder |
141 | // Base64 Encoder |
142 | QString HandlerMK::Encode64(char Data[150],unsigned int Length) |
142 | QString HandlerMK::Encode64(char Data[160],unsigned int Length) |
143 | { |
143 | { |
144 | unsigned int pt = 0; |
144 | unsigned int pt = 0; |
145 | unsigned char a,b,c; |
145 | unsigned char a,b,c; |
146 | unsigned char ptr = 0; |
146 | unsigned char ptr = 0; |
147 | 147 | ||
148 | char TX_Buff[150]; |
148 | char TX_Buff[160]; |
149 | 149 | ||
150 | while(Length > 0) |
150 | while(Length > 0) |
151 | { |
151 | { |
152 | if(Length) { a = Data[ptr++]; Length--;} else a = 0; |
152 | if(Length) { a = Data[ptr++]; Length--;} else a = 0; |
153 | if(Length) { b = Data[ptr++]; Length--;} else b = 0; |
153 | if(Length) { b = Data[ptr++]; Length--;} else b = 0; |
154 | if(Length) { c = Data[ptr++]; Length--;} else c = 0; |
154 | if(Length) { c = Data[ptr++]; Length--;} else c = 0; |
155 | 155 | ||
156 | TX_Buff[pt++] = '=' + (a >> 2); |
156 | TX_Buff[pt++] = '=' + (a >> 2); |
157 | TX_Buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
157 | TX_Buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
158 | TX_Buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
158 | TX_Buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
159 | TX_Buff[pt++] = '=' + ( c & 0x3f); |
159 | TX_Buff[pt++] = '=' + ( c & 0x3f); |
160 | } |
160 | } |
161 | TX_Buff[pt] = 0; |
161 | TX_Buff[pt] = 0; |
162 | 162 | ||
163 | return QString(TX_Buff); |
163 | return QString(TX_Buff); |
164 | } |
164 | } |
165 | 165 | ||
166 | QString HandlerMK::make_Frame(char t_CMD, int t_Adress, char t_Data[150], unsigned int t_Length) |
166 | QString HandlerMK::make_Frame(char t_CMD, int t_Adress, char t_Data[160], unsigned int t_Length) |
167 | { |
167 | { |
168 | QString tx_Data = Encode64(t_Data, t_Length); |
168 | QString tx_Data = Encode64(t_Data, t_Length); |
169 | 169 | ||
170 | tx_Data = add_CRC(QString("#" + QString('a' + t_Adress) + QString(t_CMD) + tx_Data)) + "\r"; |
170 | tx_Data = add_CRC(QString("#" + QString('a' + t_Adress) + QString(t_CMD) + tx_Data)) + "\r"; |
171 | 171 | ||
172 | return tx_Data; |
172 | return tx_Data; |
173 | } |
173 | } |
174 | 174 | ||
175 | QString HandlerMK::get_SelectNC() |
175 | QString HandlerMK::get_SelectNC() |
176 | { |
176 | { |
177 | char t_Data[6]; |
177 | char t_Data[6]; |
178 | 178 | ||
179 | t_Data[0] = 0x1B; |
179 | t_Data[0] = 0x1B; |
180 | t_Data[1] = 0x1B; |
180 | t_Data[1] = 0x1B; |
181 | t_Data[2] = 0x55; |
181 | t_Data[2] = 0x55; |
182 | t_Data[3] = 0xAA; |
182 | t_Data[3] = 0xAA; |
183 | t_Data[4] = 0x00; |
183 | t_Data[4] = 0x00; |
184 | t_Data[5] = '\r'; |
184 | t_Data[5] = '\r'; |
185 | 185 | ||
186 | QString tx_Data = QString(t_Data); |
186 | QString tx_Data = QString(t_Data); |
187 | 187 | ||
188 | return tx_Data; |
188 | return tx_Data; |
189 | } |
189 | } |
190 | 190 | ||
191 | QString HandlerMK::get_SelectFC() |
191 | QString HandlerMK::get_SelectFC() |
192 | { |
192 | { |
193 | char t_Data[1]; |
193 | char t_Data[1]; |
194 | 194 | ||
195 | t_Data[0] = 0; |
195 | t_Data[0] = 0; |
196 | 196 | ||
197 | QString tx_Data = Encode64(t_Data, 1); |
197 | QString tx_Data = Encode64(t_Data, 1); |
198 | 198 | ||
199 | tx_Data = add_CRC(QString("#" + QString('a' + ADDRESS_NC) + QString("u") + tx_Data)) + "\r"; |
199 | tx_Data = add_CRC(QString("#" + QString('a' + ADDRESS_NC) + QString("u") + tx_Data)) + "\r"; |
200 | 200 | ||
201 | return tx_Data; |
201 | return tx_Data; |
202 | } |
202 | } |
203 | 203 | ||
204 | QString HandlerMK::get_SelectMK3MAG() |
204 | QString HandlerMK::get_SelectMK3MAG() |
205 | { |
205 | { |
206 | char t_Data[1]; |
206 | char t_Data[1]; |
207 | 207 | ||
208 | t_Data[0] = 1; |
208 | t_Data[0] = 1; |
209 | 209 | ||
210 | QString tx_Data = Encode64(t_Data, 1); |
210 | QString tx_Data = Encode64(t_Data, 1); |
211 | 211 | ||
212 | tx_Data = add_CRC(QString("#" + QString('a' + ADDRESS_NC) + QString("u") + tx_Data)) + "\r"; |
212 | tx_Data = add_CRC(QString("#" + QString('a' + ADDRESS_NC) + QString("u") + tx_Data)) + "\r"; |
213 | 213 | ||
214 | return tx_Data; |
214 | return tx_Data; |
215 | } |
215 | } |
216 | 216 | ||
217 | s_Hardware HandlerMK::parse_Version(unsigned char *t_Data, int Adress) |
217 | s_Hardware HandlerMK::parse_Version(unsigned char *t_Data, int Adress) |
218 | { |
218 | { |
219 | s_Hardware t_Hardware; |
219 | s_Hardware t_Hardware; |
220 | 220 | ||
221 | t_Hardware.ID = Adress; |
221 | t_Hardware.ID = Adress; |
222 | t_Hardware.VERSION_MAJOR = t_Data[0]; |
222 | t_Hardware.VERSION_MAJOR = t_Data[0]; |
223 | t_Hardware.VERSION_MINOR = t_Data[1]; |
223 | t_Hardware.VERSION_MINOR = t_Data[1]; |
224 | t_Hardware.VERSION_PATCH = t_Data[4]; |
224 | t_Hardware.VERSION_PATCH = t_Data[4]; |
225 | t_Hardware.VERSION_SERIAL_MAJOR = t_Data[2]; |
225 | t_Hardware.VERSION_SERIAL_MAJOR = t_Data[2]; |
226 | t_Hardware.VERSION_SERIAL_MINOR = t_Data[3]; |
226 | t_Hardware.VERSION_SERIAL_MINOR = t_Data[3]; |
227 | t_Hardware.Hardware = HardwareType[t_Hardware.ID]; |
227 | t_Hardware.Hardware = HardwareType[t_Hardware.ID]; |
228 | t_Hardware.Version = QString("%1").arg(t_Data[0]) + "." + QString("%1").arg(t_Data[1]) + QString(t_Data[4] + 'a'); |
228 | t_Hardware.Version = QString("%1").arg(t_Data[0]) + "." + QString("%1").arg(t_Data[1]) + QString(t_Data[4] + 'a'); |
229 | t_Hardware.VersionShort = QString("%1").arg(t_Data[0]) + QString("%1").arg(t_Data[1]) + QString(t_Data[4] + 'a'); |
229 | t_Hardware.VersionShort = QString("%1").arg(t_Data[0]) + QString("%1").arg(t_Data[1]) + QString(t_Data[4] + 'a'); |
230 | 230 | ||
231 | return t_Hardware; |
231 | return t_Hardware; |
232 | } |
232 | } |
233 | 233 |