/QMK-Groundstation/branches/own_com_lib/Classes/ToolBox.cpp |
---|
19,15 → 19,18 |
#include "ToolBox.h" |
ToolBox::ToolBox() |
{ |
} |
/** |
* specific data for QT |
*/ |
void ToolBox::Wait(int Time) |
//FIXME: search for something else than sleep |
void ToolBox::wait(int Time) |
{ |
#ifndef _WIN32_ |
usleep(Time); |
#else |
//FIXME: implement sleep for win_32 |
//(or kick this function completely) |
// sleep(Time); |
#endif |
} |
40,228 → 43,15 |
Temp = s_Wert.left(s_Wert.length() - Count) + QString(".") + s_Wert.right(Count); |
/* |
if (Wert > 0) |
Temp = ""; |
else |
Temp = "-"; |
Temp = Temp + QString("%1").arg(Wert / Count) + "." + QString("%1").arg(Wert % Count); |
*/ |
return Temp; |
} |
// Base64 Decoder |
bool ToolBox::Decode64(sRxData &RX, bool Long) |
{ |
unsigned char a,b,c,d; |
unsigned char ptr = 0; |
unsigned char x,y,z; |
int ptrOut[150]; |
int ptrIn = 3; |
int max = RX.String.length(); |
int len = RX.String.length(); |
int DecLen = 0; |
if (RX.Input[ptrIn] == 0) |
{ |
// qDebug("QString to Char ERROR...!!!!"); |
return false; |
} |
while(len != 0) |
{ |
a = RX.Input[ptrIn++] - '='; |
b = RX.Input[ptrIn++] - '='; |
c = RX.Input[ptrIn++] - '='; |
d = RX.Input[ptrIn++] - '='; |
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(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; |
} |
return true; |
} |
// Base64 Encoder |
QString ToolBox::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); |
} |
// Datensatz nach 8bit Integer |
int ToolBox::Data2Char(int *Data , int Start, bool is_signed) |
{ |
int Out = (Data[Start]); |
if ((Out > 128) && (is_signed)) |
Out = Out - 256; |
return Out; |
} |
// Datensatz nach 8bit Integer |
int ToolBox::Char2Data(int Data) |
{ |
if (Data < 0) |
{ |
return Data + 256; |
} |
return Data; |
} |
// Datensatz nach 16bit Integer |
int ToolBox::Data2Int(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; |
} |
// Datensatz nach 32bit Long |
long ToolBox::Data2Long(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; |
} |
// Datensatz nach QString |
QString ToolBox::Data2QString(int Data[150], int Start, int End) |
QString ToolBox::dataToQString(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 QString(String); |
return QString(Parser::dataToString(Data, Start, End)); |
} |
// Datensatz-CRC prüfen |
bool ToolBox::check_CRC(QString RXString) |
{ |
int CRC = 0; |
char *RX; |
int Length = RXString.length(); |
RX = RXString.toLatin1().data(); |
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; |
} |
// Datensatz-CRC hinzufügen |
QString ToolBox::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; |
} |
// Alle Icons |
QIcon ToolBox::Icon(int ID) |
{ |
/QMK-Groundstation/branches/own_com_lib/Classes/ToolBox.h |
---|
22,25 → 22,15 |
#include <QString> |
#include <QIcon> |
#include "../global.h" |
#include "../com/Kopter.h" |
class ToolBox |
{ |
public : |
ToolBox(); |
static bool Decode64(sRxData &RX, bool Long = true); |
static QString Encode64(char Data[150],unsigned int Length); |
static bool check_CRC(QString RXString); |
static QString add_CRC(QString TXString); |
static int Data2Char(int *Data , int Start, bool is_signed = true); |
static int Data2Int(int *Data , int Start, bool is_signed = true); |
static long Data2Long(int *Data , int Start, bool is_signed = true); |
static int Char2Data(int Data); |
static QString Data2QString(int Data[150], int Start = 0, int End = 150); |
static QString dataToQString(int Data[150], int Start = 0, int End = 150); |
static QIcon Icon(int ID); |
static QString get_Float(long Wert, int Count); |
static void Wait(int Time); |
static void wait(int Time); |
}; |
#endif // TOOLBOX_H |
/QMK-Groundstation/branches/own_com_lib/Classes/cConnection.h |
---|
26,6 → 26,7 |
#include "ToolBox.h" |
#include "../global.h" |
#include "../SerialPort/ManageSerialPort.h" |
#include "../com/Parser.h" |
#define C_TTY 1 |
#define C_IP 2 |
/QMK-Groundstation/branches/own_com_lib/Classes/cSettings.h |
---|
19,7 → 19,8 |
#ifndef CSETTINGS_H |
#define CSETTINGS_H |
#include "../global.h" |
//#include "../global.h" |
#include "../com/Kopter.h" |
struct set_TTY |
{ |
/QMK-Groundstation/branches/own_com_lib/Forms/dlg_MotorMixer.h |
---|
27,7 → 27,7 |
#include "../Classes/cSettings.h" |
#include "../Classes/cConnection.h" |
#include "../typedefs.h" |
#include "../Classes/ToolBox.h" |
//#include "../Classes/ToolBox.h" |
#include "../global.h" |
class dlg_MotorMixer : public QDialog, public Ui::dlg_MotorMixer_UI |
/QMK-Groundstation/branches/own_com_lib/Forms/mktool.cpp |
---|
36,6 → 36,7 |
#include "dlg_Preferences.h" |
#include "../global.h" |
#include "../Classes/ToolBox.h" |
#include "../com/Parser.h" |
#include <stdlib.h> |
380,9 → 381,9 |
{ |
sRxData RX; |
RX.String = IN->text(); |
RX.str = IN->text().toLatin1().data(); |
RX.Input = RX.String.toLatin1().data(); |
RX.input = (char *)RX.str.c_str(); |
slot_newData(RX); |
} |
449,8 → 450,8 |
//...und sende ihn an die NaviCtrl |
int max_radius = 10000; |
if (ok_lat && ok_lon && |
abs(Navi.Current.Longitude - desired_pos.Position.Longitude) < max_radius && |
abs(Navi.Current.Latitude - desired_pos.Position.Latitude) < max_radius) |
abs((double)(Navi.Current.Longitude - desired_pos.Position.Longitude)) < max_radius && |
abs((double)(Navi.Current.Latitude - desired_pos.Position.Latitude)) < max_radius) |
{ |
o_Connection->send_Cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false); |
} |
497,7 → 498,7 |
TX_Data[4] = 0x00; |
TX_Data[5] = '\r'; |
o_Connection->send_Cmd('#', ADDRESS_NC, TX_Data, 6, false); |
ToolBox::Wait(SLEEP); |
ToolBox::wait(SLEEP); |
} |
if (rb_SelFC->isChecked()) |
525,7 → 526,7 |
TX_Data[5] = '\r'; |
o_Connection->send_Cmd('#', ADDRESS_NC, TX_Data, 6, false); |
} |
ToolBox::Wait(SLEEP); |
ToolBox::wait(SLEEP); |
// qDebug("Select RB Hardware"); |
o_Connection->send_Cmd('v', ADDRESS_ALL, TX_Data, 0, true); |
766,7 → 767,7 |
// Waypoint-Liste löschen |
WayPoint.Position.Status = INVALID; |
o_Connection->send_Cmd('w', ADDRESS_NC, (char *)&WayPoint, sizeof(WayPoint), false); |
ToolBox::Wait(SLEEP); |
ToolBox::wait(SLEEP); |
for (int z = 0; z < l_WayPoints.count(); z++) |
{ |
1369,7 → 1370,7 |
{ |
// qDebug("Navi-Data"); |
switch(RX.Decode[N_NC_FLAGS]) |
switch(RX.decode[N_NC_FLAGS]) |
{ |
case 0x01 : lb_Mode->setText(tr("Free")); break; |
case 0x02 : lb_Mode->setText(tr("Position Hold")); break; |
1380,25 → 1381,25 |
case 0x40 : lb_Mode->setText(tr("Manual Control")); break; |
} |
Navi.Current.Longitude = ToolBox::Data2Long(RX.Decode, N_CUR_LONGITUDE, true); |
Navi.Current.Latitude = ToolBox::Data2Long(RX.Decode, N_CUR_LATITUDE, true); |
Navi.Current.Altitude = ToolBox::Data2Long(RX.Decode, N_CUR_ALTITUDE, true); |
Navi.Target.Longitude = ToolBox::Data2Long(RX.Decode, N_TAR_LONGITUDE, true); |
Navi.Target.Latitude = ToolBox::Data2Long(RX.Decode, N_TAR_LATITUDE, true); |
Navi.Target.Altitude = ToolBox::Data2Long(RX.Decode, N_TAR_ALTITUDE, true); |
Navi.Current.Longitude = Parser::dataToLong(RX.decode, N_CUR_LONGITUDE, true); |
Navi.Current.Latitude = Parser::dataToLong(RX.decode, N_CUR_LATITUDE, true); |
Navi.Current.Altitude = Parser::dataToLong(RX.decode, N_CUR_ALTITUDE, true); |
Navi.Target.Longitude = Parser::dataToLong(RX.decode, N_TAR_LONGITUDE, true); |
Navi.Target.Latitude = Parser::dataToLong(RX.decode, N_TAR_LATITUDE, true); |
Navi.Target.Altitude = Parser::dataToLong(RX.decode, N_TAR_ALTITUDE, true); |
le_CDistance->setText(QString("%1 cm").arg(ToolBox::Data2Int(RX.Decode, N_HOME_DISTANCE))); |
le_CWPA->setText(QString("%1").arg(RX.Decode[N_WP_INDEX])); |
le_CWPT->setText(QString("%1").arg(RX.Decode[N_WP_NUMBER])); |
le_CSats->setText(QString("%1").arg(RX.Decode[N_SATS_IN_USER])); |
le_CDistance->setText(QString("%1 cm").arg(Parser::dataToInt(RX.decode, N_HOME_DISTANCE))); |
le_CWPA->setText(QString("%1").arg(RX.decode[N_WP_INDEX])); |
le_CWPT->setText(QString("%1").arg(RX.decode[N_WP_NUMBER])); |
le_CSats->setText(QString("%1").arg(RX.decode[N_SATS_IN_USER])); |
qwt_Rate->setValue(double(ToolBox::Data2Int(RX.Decode, N_VARIOMETER, true))); |
qwt_Rate->setValue(double(Parser::dataToInt(RX.decode, N_VARIOMETER, true))); |
le_CTime->setText(QString("%1 sec.").arg(ToolBox::Data2Int(RX.Decode, N_FLYING_TIME))); |
le_CTime->setText(QString("%1 sec.").arg(Parser::dataToInt(RX.decode, N_FLYING_TIME))); |
bar_UBAT->setValue(RX.Decode[N_UBAT]); |
bar_UBAT->setValue(RX.decode[N_UBAT]); |
double Speed = double((ToolBox::Data2Int(RX.Decode, N_GROUND_SPEED)) / 10.0); |
double Speed = double((Parser::dataToLong(RX.decode, N_GROUND_SPEED)) / 10.0); |
if ((Speed > 4.5) && SpeedMeter->property("END") == 5) |
{ |
1416,12 → 1417,12 |
SpeedMeter->setValue(Speed); |
Compass->setValue(ToolBox::Data2Int(RX.Decode, N_COMAPSS_HEADING)); //(68) |
Compass->setValue(Parser::dataToInt(RX.decode, N_COMAPSS_HEADING)); //(68) |
bar_RX->setValue(RX.Decode[N_RC_QUALITY]); |
bar_RX->setValue(RX.decode[N_RC_QUALITY]); |
int Nick = RX.Decode[N_ANGLE_NICK]; |
int Roll = RX.Decode[N_ANGLE_ROLL]; |
int Nick = RX.decode[N_ANGLE_NICK]; |
int Roll = RX.decode[N_ANGLE_ROLL]; |
if (Roll > 128) |
Roll = Roll - 255; |
1434,16 → 1435,16 |
sNaviString NaviString; |
NaviString.Longitude = ToolBox::get_Float(Navi.Current.Longitude,7); |
NaviString.Latitude = ToolBox::get_Float(Navi.Current.Latitude,7); |
NaviString.Altitude = ToolBox::get_Float(Navi.Current.Altitude,3); |
NaviString.Longitude = Parser::getFloat(Navi.Current.Longitude,7); |
NaviString.Latitude = Parser::getFloat(Navi.Current.Latitude,7); |
NaviString.Altitude = Parser::getFloat(Navi.Current.Altitude,3); |
le_CurLong->setText(NaviString.Longitude); |
le_CurLat->setText(NaviString.Latitude); |
le_CurLong->setText(QString::number(NaviString.Longitude)); |
le_CurLat->setText(QString::number(NaviString.Latitude)); |
KML_Server->store_NaviString(NaviString); |
f_Map->add_Position(NaviString.Longitude.toDouble(), NaviString.Latitude.toDouble()); |
f_Map->add_Position(NaviString.Longitude, NaviString.Latitude); |
if ((QMK_Server->property("Connect")) == true) |
{ |
1461,20 → 1462,20 |
if (LastSend.length() > 2) |
{ |
} |
int HardwareID = RX.Input[1] - 'a'; |
int HardwareID = RX.input[1] - 'a'; |
switch(HardwareID) |
{ |
case ADDRESS_FC : |
switch(RX.Input[2]) |
switch(RX.input[2]) |
{ |
// Motor-Mixer |
case 'N' : |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
o_Connection->stop_ReSend(); |
if (RX.Decode[0] == VERSION_MIXER) |
if (RX.decode[0] == VERSION_MIXER) |
{ |
f_MotorMixer->set_MotorConfig(RX); |
} |
1482,11 → 1483,11 |
break; |
// Motor-Mixer Schreib-Bestätigung |
case 'M' : |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
o_Connection->stop_ReSend(); |
if (RX.Decode[0] == 1) |
if (RX.decode[0] == 1) |
{ |
lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben.")); |
} |
1495,30 → 1496,30 |
// Stick-Belegung der Fernsteuerung |
case 'P' : // DONE 0.71g |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
f_Settings->pb_K1->setValue(ToolBox::Data2Int(RX.Decode, 2,true)); |
f_Settings->pb_K2->setValue(ToolBox::Data2Int(RX.Decode, 4,true)); |
f_Settings->pb_K3->setValue(ToolBox::Data2Int(RX.Decode, 6,true)); |
f_Settings->pb_K4->setValue(ToolBox::Data2Int(RX.Decode, 8,true)); |
f_Settings->pb_K5->setValue(ToolBox::Data2Int(RX.Decode, 10 ,true)); |
f_Settings->pb_K6->setValue(ToolBox::Data2Int(RX.Decode, 12,true)); |
f_Settings->pb_K7->setValue(ToolBox::Data2Int(RX.Decode, 14,true)); |
f_Settings->pb_K8->setValue(ToolBox::Data2Int(RX.Decode, 16,true)); |
f_Settings->pb_K1->setValue(Parser::dataToInt(RX.decode, 2,true)); |
f_Settings->pb_K2->setValue(Parser::dataToInt(RX.decode, 4,true)); |
f_Settings->pb_K3->setValue(Parser::dataToInt(RX.decode, 6,true)); |
f_Settings->pb_K4->setValue(Parser::dataToInt(RX.decode, 8,true)); |
f_Settings->pb_K5->setValue(Parser::dataToInt(RX.decode, 10 ,true)); |
f_Settings->pb_K6->setValue(Parser::dataToInt(RX.decode, 12,true)); |
f_Settings->pb_K7->setValue(Parser::dataToInt(RX.decode, 14,true)); |
f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true)); |
} |
break; |
// Settings lesen |
case 'Q' : // DONE 0.71g |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
o_Connection->stop_ReSend(); |
if (RX.Decode[1] == VERSION_SETTINGS) |
if (RX.decode[1] == VERSION_SETTINGS) |
{ |
int Settings_ID = RX.Decode[0]; |
int Settings_ID = RX.decode[0]; |
for (int a = 0; a < MaxParameter; a++) |
{ |
FCSettings[a] = RX.Decode[a + 2]; |
FCSettings[a] = RX.decode[a + 2]; |
} |
f_Settings->show_FCSettings(Settings_ID, FCSettings); |
f_Settings->pb_Read->setEnabled(true); |
1529,8 → 1530,14 |
f_Settings->pb_Read->setDisabled(true); |
f_Settings->pb_Write->setDisabled(true); |
QString name = QString("Versionen inkompatibel.\n") + |
QString("Version von GroundStation benoetigt: ") + |
QString(VERSION_SETTINGS) + |
QString("\nVersion auf der FlightCtrl: ") + |
QString(RX.decode[1]) + |
QString("\nParameterbearbeitung nicht moeglich."); |
QMessageBox::warning(this, QA_NAME, |
tr("Versionen inkompatibel. \nParameterbearbeitung nicht moeglich."), QMessageBox::Ok); |
name, QMessageBox::Ok); |
} |
} |
break; |
1541,11 → 1548,11 |
} |
case ADDRESS_NC : |
switch(RX.Input[2]) |
switch(RX.input[2]) |
{ |
// Navigationsdaten |
case 'O' : // NOT DONE 0.12h |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
new_NaviData(RX); |
} |
1554,33 → 1561,33 |
// case ADDRESS_MK3MAG : |
default : |
switch(RX.Input[2]) |
switch(RX.input[2]) |
{ |
// LCD-Anzeige |
case 'L' : // DONE 0.71g |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
o_Connection->stop_ReSend(); |
int LCD[150]; |
memcpy(LCD,RX.Decode, sizeof(RX.Decode)); |
memcpy(LCD,RX.decode, sizeof(RX.decode)); |
f_LCD->show_Data(LCD); |
LCD_Page = RX.Decode[0]; |
LCD_MAX_Page = RX.Decode[1]; |
LCD_Page = RX.decode[0]; |
LCD_MAX_Page = RX.decode[1]; |
} |
break; |
// Analoglabels |
case 'A' : // DONE 0.71g |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
o_Connection->stop_ReSend(); |
int Position = RX.Decode[0]; |
int Position = RX.decode[0]; |
if (Position != 31) |
{ |
Settings->Analog1.Label[Position] = ToolBox::Data2QString(RX.Decode,1,17).trimmed(); |
Settings->Analog1.Label[Position] = ToolBox::dataToQString(RX.decode,1,17).trimmed(); |
if (Settings->Analog1.Label[Position] == "") |
{ |
Settings->Analog1.Label[Position] = "A-" + QString("%1").arg(Position); |
1603,11 → 1610,11 |
break; |
// Debug-Daten |
case 'D' : // DONE 0.71g |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
for (int i = 0; i < MaxAnalog; i++) |
{ |
AnalogData[i] = ToolBox::Data2Int(RX.Decode, (i * 2) + 2); |
AnalogData[i] = Parser::dataToInt(RX.decode, (i * 2) + 2); |
} |
show_DebugData(); |
} |
1614,22 → 1621,26 |
break; |
// Version |
case 'V' : // DONE 0.71h |
if (ToolBox::Decode64(RX)) |
if (Parser::decode64(RX)) |
{ |
o_Connection->stop_ReSend(); |
Mode.ID = HardwareID; |
Mode.VERSION_MAJOR = RX.Decode[0]; |
Mode.VERSION_MINOR = RX.Decode[1]; |
Mode.VERSION_PATCH = RX.Decode[4]; |
Mode.VERSION_SERIAL_MAJOR = RX.Decode[2]; |
Mode.VERSION_SERIAL_MINOR = RX.Decode[3]; |
Mode.VERSION_MAJOR = RX.decode[0]; |
Mode.VERSION_MINOR = RX.decode[1]; |
Mode.VERSION_PATCH = RX.decode[4]; |
Mode.VERSION_SERIAL_MAJOR = RX.decode[2]; |
Mode.VERSION_SERIAL_MINOR = RX.decode[3]; |
Mode.Hardware = HardwareType[Mode.ID]; |
Mode.Version = QString("%1").arg(RX.Decode[0]) + "." + QString("%1").arg(RX.Decode[1]) + QString(RX.Decode[4] + 'a'); |
//TODO: Funktion im Handler get_version() oder sowas |
QString version = QString("%1").arg(RX.decode[0]) + "." + |
QString("%1").arg(RX.decode[1]) + |
QString(RX.decode[4] + 'a'); |
Mode.Version = version.toLatin1(); |
setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " + |
QString(Mode.Hardware) + " " + Mode.Version); |
setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " + Mode.Hardware + " " + Mode.Version); |
if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR) |
{ |
// AllowSend = false; |
1729,7 → 1740,7 |
// QMK_Server->send_RawData(RX.String); |
} |
slot_showTerminal(1, RX.String); |
slot_showTerminal(1, RX.str); |
} |
void MKTool::slot_showTerminal(int Typ, QString Text) |
1776,13 → 1787,13 |
{ |
TX_Data[0] = Settings->Data.Debug_Off / 10; |
o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false); |
ToolBox::Wait(SLEEP); |
ToolBox::wait(SLEEP); |
if (Mode.ID == ADDRESS_NC) |
{ |
TX_Data[0] = Settings->Data.Navi_Off / 10; |
o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false); |
ToolBox::Wait(SLEEP); |
ToolBox::wait(SLEEP); |
} |
if (Mode.ID == ADDRESS_FC) |
1792,7 → 1803,7 |
TX_Data[2] = 0; |
TX_Data[3] = 0; |
o_Connection->send_Cmd('t', ADDRESS_FC, TX_Data, 4, false); |
ToolBox::Wait(SLEEP); |
ToolBox::wait(SLEEP); |
} |
o_Connection->Close(); |
/QMK-Groundstation/branches/own_com_lib/Forms/mktool.h |
---|
52,6 → 52,9 |
#include "../Logger/Logger.h" |
#include "../typedefs.h" |
//quadcopter lib stuff |
#include "../com/Parser.h" |
class QextSerialPort; |
class MKTool : public QMainWindow, public Ui::dlg_mktool_UI |
/QMK-Groundstation/branches/own_com_lib/README |
---|
0,0 → 1,2 |
QMK-Groundstation with seperated communication and Mikrokopter-Library that can be used in other programs. |
Based on SVN version r390 |
/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 |
/QMK-Groundstation/branches/own_com_lib/eeepc.pro |
---|
6,4 → 6,4 |
LIBS += -lqwt-qt4 |
INCLUDEPATH += $(HOME)/include /usr/include/qwt-qt4 |
include(global.pri) |
include(global.pri)HEADERS += com/Handler.h com/Parser.h com/Kopter.h |
/QMK-Groundstation/branches/own_com_lib/global.h |
---|
25,6 → 25,9 |
#include <QColor> |
#include <QBitArray> |
//copter settings from kopter module |
#include "com/Kopter.h" |
#include "Parameter_Positions.h" |
#ifdef _WIN32_ |
41,23 → 44,13 |
static const int MeterSize = 160; |
#endif |
// Version des Seriellen Protokoll |
static const int VERSION_SERIAL_MAJOR = 10; |
static const int VERSION_SERIAL_MINOR = 0; |
static const QString QA_NAME = "QMK-Groundstation"; |
static const QString QA_VERSION_NR = "0.8.5 - own flight lib"; |
// Basis-Addressen der verschiedenen Hardware |
static const int ADDRESS_ALL = 0; |
static const int ADDRESS_FC = 1; |
static const int ADDRESS_NC = 2; |
static const int ADDRESS_MK3MAG = 3; |
static const int SETTINGS_ID = 2; |
// sleep time - time to wait until next command is received |
// FIXME: delete this / rewrite to use interrupts, mutex or something elsebetter than timeouts |
static const int SLEEP = 500000; |
static const QString QA_NAME = "QMK-Groundstation"; |
static const QString QA_VERSION_NR = "0.8.5"; |
#ifdef _BETA_ |
static const QString QA_VERSION = QA_VERSION_NR + " (BETA)"; |
static const QString QA_HWVERSION = "FlightCtrl v0.73d & NaviCtrl v0.15c"; |
106,72 → 99,4 |
static const bool Def_Plot_Show[] = {1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0}; |
static const bool Def_Log[] = {1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0}; |
static const QString 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; |
QString Hardware; |
QString Version; |
}; |
struct sRxData |
{ |
char *Input; |
QString String; |
int Decode[150]; |
int DecLen; |
}; |
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 |
{ |
QString Longitude; |
QString Latitude; |
QString Altitude; |
}; |
struct sWayPoint |
{ |
double Longitude; |
double Latitude; |
double Altitude; |
int Time; |
}; |
#endif |