Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 673 → Rev 674

/QMK-Groundstation/trunk/Global/Class_HandlerMK/HandlerMK.cpp
0,0 → 1,232
/***************************************************************************
* Copyright (C) 2009 by Manuel Schrape *
* manuel.schrape@gmx.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include "HandlerMK.h"
 
HandlerMK::HandlerMK()
{
}
 
// Datensatz nach 16bit Integer
int HandlerMK::Data2Int(unsigned char Data[150] , int Start, bool is_signed)
{
int Out = (Data[Start+1]<<8) | (Data[Start+0]);
 
if ((Out > 32767) && (is_signed))
Out = Out - 65536;
 
return Out;
 
}
 
// Datensatz nach QString
QString HandlerMK::Data2QString(unsigned char Data[150], int Start, int End)
{
char String[150];
for (int a = Start; a < End; a++)
{
String[a - Start] = Data[a];
}
String[End - Start] = '\0';
 
return QString(String);
}
 
double HandlerMK::Int2Double(int32_t Wert, int Count)
{
QString Temp, s_Wert;
 
s_Wert = QString("%1").arg(Wert);
 
Temp = s_Wert.left(s_Wert.length() - Count) + QString(".") + s_Wert.right(Count);
 
return Temp.toDouble();
}
 
QString HandlerMK::add_CRC(QString TXString)
{
unsigned int tmpCRC = 0;
 
char *TXBuff;
char CRC[2];
 
TXBuff = TXString.toLatin1().data();
 
for(int i = 0; i < TXString.length(); i++)
{
tmpCRC += TXBuff[i];
}
 
tmpCRC %= 4096;
 
CRC[0] = '=' + tmpCRC / 64;
CRC[1] = '=' + tmpCRC % 64;
CRC[2] = '\0';
 
QString Return = TXString + QString(CRC);
 
return Return;
}
 
bool HandlerMK::Check_CRC(char *t_InData, int Length)
{
int CRC = 0;
 
if (t_InData[1] == 127)
t_InData[1] = 0;
 
for(int i=0; i < Length-2; i++)
CRC+=t_InData[i];
 
CRC = CRC % 4096;
 
if(t_InData[Length - 2] != ('=' + (CRC / 64)))
return false;
 
if(t_InData[Length - 1] != ('=' + CRC % 64))
return false;
 
return true;
}
 
int HandlerMK::Decode_64(char *t_InData, int Length, unsigned char *t_OutData)
{
unsigned char a,b,c,d;
unsigned char ptr = 0;
unsigned char x,y,z;
 
int Offset = 3;
 
if (t_InData[Offset] == 0)
{
return 0;
}
 
while(Length != 0)
{
a = t_InData[Offset++] - '=';
b = t_InData[Offset++] - '=';
c = t_InData[Offset++] - '=';
d = t_InData[Offset++] - '=';
 
// if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
 
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
 
if(Length--) t_OutData[ptr++] = x; else break;
if(Length--) t_OutData[ptr++] = y; else break;
if(Length--) t_OutData[ptr++] = z; else break;
}
 
return ptr;
}
 
// Base64 Encoder
QString HandlerMK::Encode64(char Data[150],unsigned int Length)
{
unsigned int pt = 0;
unsigned char a,b,c;
unsigned char ptr = 0;
 
char TX_Buff[150];
 
while(Length > 0)
{
if(Length) { a = Data[ptr++]; Length--;} else a = 0;
if(Length) { b = Data[ptr++]; Length--;} else b = 0;
if(Length) { c = Data[ptr++]; Length--;} else c = 0;
 
TX_Buff[pt++] = '=' + (a >> 2);
TX_Buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
TX_Buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
TX_Buff[pt++] = '=' + ( c & 0x3f);
}
TX_Buff[pt] = 0;
 
return QString(TX_Buff);
}
 
QString HandlerMK::make_Frame(char t_CMD, int t_Adress, char t_Data[150], unsigned int t_Length)
{
QString tx_Data = Encode64(t_Data, t_Length);
 
tx_Data = add_CRC(QString("#" + QString('a' + t_Adress) + QString(t_CMD) + tx_Data)) + "\r";
 
return tx_Data;
}
 
QString HandlerMK::get_SelectNC()
{
char t_Data[6];
 
t_Data[0] = 0x1B;
t_Data[1] = 0x1B;
t_Data[2] = 0x55;
t_Data[3] = 0xAA;
t_Data[4] = 0x00;
t_Data[5] = '\r';
 
QString tx_Data = QString(t_Data);
 
return tx_Data;
}
 
QString HandlerMK::get_SelectFC()
{
char t_Data[1];
 
t_Data[0] = 0;
 
QString tx_Data = Encode64(t_Data, 1);
 
tx_Data = add_CRC(QString("#" + QString('a' + ADDRESS_NC) + QString("u") + tx_Data)) + "\r";
 
return tx_Data;
}
 
QString HandlerMK::get_SelectMK3MAG()
{
char t_Data[1];
 
t_Data[0] = 1;
 
QString tx_Data = Encode64(t_Data, 1);
 
tx_Data = add_CRC(QString("#" + QString('a' + ADDRESS_NC) + QString("u") + tx_Data)) + "\r";
 
return tx_Data;
}
 
s_Hardware HandlerMK::parse_Version(unsigned char *t_Data, int Adress)
{
s_Hardware t_Hardware;
 
t_Hardware.ID = Adress;
t_Hardware.VERSION_MAJOR = t_Data[0];
t_Hardware.VERSION_MINOR = t_Data[1];
t_Hardware.VERSION_PATCH = t_Data[4];
t_Hardware.VERSION_SERIAL_MAJOR = t_Data[2];
t_Hardware.VERSION_SERIAL_MINOR = t_Data[3];
t_Hardware.Hardware = HardwareType[t_Hardware.ID];
t_Hardware.Version = QString("%1").arg(t_Data[0]) + "." + QString("%1").arg(t_Data[1]) + QString(t_Data[4] + 'a');
t_Hardware.VersionShort = QString("%1").arg(t_Data[0]) + QString("%1").arg(t_Data[1]) + QString(t_Data[4] + 'a');
 
return t_Hardware;
}