Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 390 → Rev 391

/QMK-Groundstation/branches/own_com_lib/com/Handler.cpp
0,0 → 1,9
#include<Handler.h>
 
/**
* read mixer values from FlightCtrl
*/
void Handler::read_mixer() {
TX_Data[0] = 0;
o_Connection->send_Cmd('n', ADDRESS_FC, TX_Data, 1, true);
}
/QMK-Groundstation/branches/own_com_lib/com/Handler.h
0,0 → 1,16
#ifndef HANDLER_H
#define HANDLER_H
#include <string>
#include <Parser.h>
 
/**
* The Handler handels commands that are send from/to the Mikrokopter
* and parses them using the Parser-class.
*/
 
class Handler {
public:
void read_mixer();
};
 
#endif
/QMK-Groundstation/branches/own_com_lib/com/Kopter.h
0,0 → 1,83
#ifndef COPTER_H
#define COPTER_H
#include <string>
 
using namespace std;
 
/**
* This file contains informations and configurations from the Mikrokopter
*/
 
// version information for the serial connection
static const int VERSION_SERIAL_MAJOR = 10;
static const int VERSION_SERIAL_MINOR = 0;
 
// Basis-Adresses for different Hardware components
static const int ADDRESS_ALL = 0;
static const int ADDRESS_FC = 1;
static const int ADDRESS_NC = 2;
static const int ADDRESS_MK3MAG = 3;
 
// settings ID
static const int SETTINGS_ID = 2;
 
static const string HardwareType[] = {"Default", "FlightCtrl", "NaviCtrl", "MK3Mag"};
 
static const int MaxTickerEvents = 5;
 
static const int MaxAnalog = 32;
static const int MaxPlot = 50000;
 
static const int MaxNaviPos = 2000;
 
struct sMotor
{
int Speed[12];
};
 
struct sMode
{
int ID;
int VERSION_MAJOR;
int VERSION_MINOR;
int VERSION_PATCH;
int VERSION_SERIAL_MAJOR;
int VERSION_SERIAL_MINOR;
string Hardware;
string Version;
};
 
struct sGPS_Pos
{
long Longitude;
long Latitude;
long Altitude;
};
 
struct sNaviData
{
sGPS_Pos Current;
sGPS_Pos Target;
sGPS_Pos Home;
 
long Longitude;
long Latitude;
long Altitude;
};
 
struct sNaviString
{
float Longitude;
float Latitude;
float Altitude;
};
 
struct sWayPoint
{
double Longitude;
double Latitude;
double Altitude;
int Time;
};
 
#endif
/QMK-Groundstation/branches/own_com_lib/com/Parser.cpp
0,0 → 1,197
#include <Parser.h>
 
// Base64 Decoder
// see Parser.h for details about sRxData
void Parser::decode64(sRxData &rx, unsigned int length)
{
unsigned char a,b,c,d;
unsigned char ptr = 0;
unsigned char x,y,z;
int ptrOut[150];
 
int ptrIn = 3;
int max = length;
int len = length;
int decLen = 0;
 
if (rx.Input[ptrIn] == 0) {
throw "Nothing received";
}
 
while(len != 0) {
a = RX.input[ptrIn++] - '=';
b = RX.input[ptrIn++] - '=';
c = RX.input[ptrIn++] - '=';
d = RX.input[ptrIn++] - '=';
 
if(ptrIn > max - 2) break;
 
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
 
if(len--) ptrOut[ptr++] = x; else break;
if(len--) ptrOut[ptr++] = y; else break;
if(len--) ptrOut[ptr++] = z; else break;
}
 
for (int a=0; a<ptr; a++) {
if (Long == false) {
int b1, b2, b3;
 
b1 = ptrOut[a++];
b2 = ptrOut[a];
 
b3 = (b2 << 8) | b1;
 
if (b3 > 32767)
b3 = b3 - 65536;
 
RX.decode[DecLen] = b3;
DecLen++;
} else {
RX.decode[DecLen] = ptrOut[a];
DecLen++;
}
RX.DecLen = DecLen;
}
}
 
// base64 encoder
string Parser::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 string(tx_buff);
}
 
// Datensatz nach 8bit Integer
int Parser::dataToChar(int *data , int start, bool is_signed) {
int out = (data[start]);
 
if ((out > 128) && (is_signed))
out = out - 256;
 
return out;
}
 
// received char to 8 bit integer
int Parser::charToData(int data) {
if (data < 0)
return data + 256;
return data;
}
 
// convert data to 16bit Integer
int Parser::dataToInt(int *Data , int Start, bool is_signed)
{
int Out = (Data[Start+1]<<8) | (Data[Start+0]);
 
if ((Out > 32767) && (is_signed))
Out = Out - 65536;
 
return Out;
 
}
 
// convert data to 32bit Long
long Parser::dataToLong(int *Data , int Start, bool is_signed)
{
long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
 
if ((Out > 32767) && (is_signed))
Out = Out;
 
return Out;
}
 
float Parser::getFloat(long value, int count)
{
long num = math.pow(10, count);
 
float temp = value;
 
return value / num;
}
 
string Parser::dataToString(int 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 string(String);
}
 
// check CRC
bool Parser::check_CRC(char * RX, int length)
{
int CRC = 0;
char *RX;
 
if (RX[1] == 127)
{
RX[1] = 0;
}
 
for(int i=0; i < length-2; i++)
{
CRC+=RX[i];
}
 
CRC = CRC % 4096;
 
if(RX[length - 2] != ('=' + (CRC / 64)))
{
return false;
}
 
if(RX[length - 1] != ('=' + CRC % 64))
{
return false;
}
 
return true;
}
 
// add CRC
string Parser::add_CRC(char * TX, int length)
{
unsigned int tmpCRC = 0;
 
char CRC[2];
 
for(int i = 0; i < length; i++)
{
tmpCRC += TX[i];
}
 
tmpCRC %= 4096;
 
CRC[0] = '=' + tmpCRC / 64;
CRC[1] = '=' + tmpCRC % 64;
CRC[2] = '\0';
 
return string(TX) + string(CRC);
}
 
/QMK-Groundstation/branches/own_com_lib/com/Parser.h
0,0 → 1,35
#ifndef PARSER_H
#define PARSER_H
#include <string>
 
/**
* The Parser gets values from the Mikrokopter-USART interface
* and parses them into a sRxData-Struct
*/
 
struct sRxData
{
char *input;
string str;
int decode[150];
int decLen;
};
 
class Parser {
public:
bool decode64(sRxData &rx, unsigned int length);
string encode64(char data[150],unsigned int length);
 
string add_CRC(char * TX, int length);
string check_CRC(char * RX, int length);
 
static float getFloat(long value, int count);
 
static int dataToInt(int *Data , int Start, bool is_signed = true);
static long dataToLong(int *Data , int Start, bool is_signed = true);
static int dataToChar(int *data , int start, bool is_signed = true);
static string dataToString(int Data[150], int Start = 0, int End = 150);
static int charToData(int data);
};
 
#endif