Subversion Repositories Projects

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

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