Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 439 → Rev 440

/QMK-Groundstation/branches/libMK/libMK/Communication.cpp
0,0 → 1,5
#include "Communication.h"
 
bool Communication::is_connected() {
return connected;
}
/QMK-Groundstation/branches/libMK/libMK/Communication.h
0,0 → 1,23
#ifndef COMMUNICATION_H
#define COMMUNICATION_H
#include <string>
 
/**
* communication interface for Mikrokopter (MK) USART connection
*/
 
using namespace std;
 
class Communication{
protected:
bool connected;
public:
//connect to MK
virtual void connect_MK(string) {};
//send command to MK
virtual bool send_cmd(char, int, char[150],unsigned int, bool) { return false; };
virtual void stop_resend() {};
bool is_connected();
};
#endif
/QMK-Groundstation/branches/libMK/libMK/Handler.cpp
0,0 → 1,470
#include<Handler.h>
 
/**
* Constructor that gets a communication instance
*/
Handler::Handler(Communication * com, KopterData * data) {
this->com = com;
this->data = data;
}
 
//-------------FlightCtrl commands--------------------
/**
* read settings from FlightCtrl (settings index 0x00-0x05)
*/
void Handler::get_flightctrl_settings(int index) {
char tx_data[2] = {index, 0};
com->send_cmd('q', ADDRESS_FC, tx_data, 1, true);
}
 
/**
* write settings to FlightCtrl
*/
void Handler::set_flightctrl_settings(char * tx_data) {
com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true);
}
 
/**
* test one or more motors
*/
void Handler::motor_test(sMotor motor) {
char tx_data[12];
for (int z = 0; z<12; z++)
{
tx_data[z] = motor.Speed[z];
}
com->send_cmd('t', ADDRESS_FC, tx_data, 12, false);
}
 
void Handler::reset_motor() {
sMotor motor;
for (int z = 0; z<12; z++)
{
motor.Speed[z] = 0;
}
 
motor_test(motor);
}
 
/**
* read mixer values from FlightCtrl
*/
void Handler::read_motor_mixer() {
char tx_data[1] = {0};
//com->log("read motor mixer");
com->send_cmd('n', ADDRESS_FC, tx_data, 1, true);
}
 
/**
* write motor mixer values to FlightCtrl
*/
void Handler::write_motor_mixer(char * tx_data, int length) {
com->send_cmd('m', ADDRESS_FC, tx_data, length, true);
}
 
int Handler::get_motor_config(char * tx_data) {
return -1;
}
 
//-------------NaviCtrl commands--------------------
/**
* set debug values for NaviCtrl
*/
void Handler::set_navictrl_debug(int speed) {
char tx_data[1] = { speed };
com->send_cmd('o', ADDRESS_NC, tx_data, 1, false);
}
 
/**
* stop debug for NaviCtrl
*/
void Handler::stop_navictrl_debug() {
set_navictrl_debug(0);
}
 
/**
* send a waypoint to the NaviCtrl (the copter will fly to the position emidiately)
*/
void Handler::send_waypoint(Waypoint_t desired_pos) {
com->send_cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false);
}
 
/**
* add waypoint to waypoint list
*/
void Handler::add_waypoint(Waypoint_t wp) {
com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
}
 
/**
* clear waypoint list on MK
*/
void Handler::delete_waypoints() {
Waypoint_t wp;
wp.Position.Status = INVALID;
send_waypoint(wp);
}
//-------------switch between Hardware--------------------
void Handler::switch_navictrl() {
char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'};
com->send_cmd('#', ADDRESS_NC, tx_data, 6, false);
}
 
void Handler::switch_flightctrl() {
char tx_data[1] = { 0 };
com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
}
 
void Handler::switch_mk3mag() {
char tx_data[1] = { 1 };
com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
}
 
//-------------commands for MK3MAG-----------------
 
 
//-------------commands for all--------------------
 
/**
* set debug values for all components
*/
void Handler::set_all_debug(int speed) {
char tx_data[1] = { speed };
com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false);
}
 
/**
* stop debug for all components
*/
void Handler::stop_all_debug() {
set_all_debug(0);
}
 
/**
* get all analog labels
*/
void Handler::get_analog() {
char tx_data[1] = { 0 };
com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true);
}
 
/**
* get values from LCD / show LCD
*/
void Handler::show_lcd() {
char tx_data[1] = {0};
com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}
 
/**
* got to next LCD Page
*/
void Handler::lcd_up() {
char tx_data[2] = { 0, 0 };
if (data->lcd_cur != data->lcd_max)
tx_data[0] = data->lcd_cur+1;
com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}
 
/**
* got to previous LCD Page
*/
void Handler::lcd_down() {
char tx_data[2] = { 0, 0 };
if (data->lcd_cur != 0)
tx_data[0] = data->lcd_cur-1;
com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}
 
void Handler::get_version() {
//TODO: Check if is this correct or do we need data from switch_...
char tx_data[1] = { 0 };
com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true);
}
 
void Handler::get_ppm_channels() {
char tx_data[1] = { 0 };
com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
}
 
/**
* receive data
*/
void Handler::receive_data(sRxData RX) {
//extract hardware ID from received Data
int hardwareID = RX.input[1] - 'a';
switch(hardwareID)
{
case ADDRESS_FC :
switch(RX.input[2])
{
// Motor-Mixer
case 'N' :
if (Parser::decode64(RX))
{
com->stop_resend();
 
if (RX.decode[0] == VERSION_MIXER)
{
//f_MotorMixer->set_MotorConfig(RX);
}
}
break;
// Motor-Mixer Schreib-Bestätigung
case 'M' :
if (Parser::decode64(RX))
{
com->stop_resend();
 
if (RX.decode[0] == 1)
{
//lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
}
}
break;
 
// Stick-Belegung der Fernsteuerung
case 'P' : // DONE 0.71g
if (Parser::decode64(RX))
{
/*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 (Parser::decode64(RX))
{
com->stop_resend();
 
if (RX.decode[1] == VERSION_SETTINGS)
{
int Settings_ID = RX.decode[0];
/*for (int a = 0; a < MaxParameter; a++)
{
FCSettings[a] = RX.decode[a + 2];
}
f_Settings->show_FCSettings(Settings_ID, FCSettings);
f_Settings->pb_Read->setEnabled(true);
f_Settings->pb_Write->setEnabled(true);*/
}
else
{
/*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,
name, QMessageBox::Ok);*/
}
}
break;
// Settings written
case 'S' : // DONE 0.71g
com->stop_resend();
//TODO: QMessagebox("settings written successful") ?
break;
}
 
case ADDRESS_NC :
switch(RX.input[2])
{
// Navigationsdaten
case 'O' : // NOT DONE 0.12h
if (Parser::decode64(RX))
{
//new_NaviData(RX);
}
break;
}
// case ADDRESS_MK3MAG :
 
default :
switch(RX.input[2])
{
// LCD-Anzeige
case 'L' : // DONE 0.71g
if (Parser::decode64(RX))
{
com->stop_resend();
 
/*int LCD[150];
memcpy(LCD,RX.decode, sizeof(RX.decode));
 
f_LCD->show_Data(LCD);
 
LCD_Page = RX.decode[0];
LCD_MAX_Page = RX.decode[1];
*/
}
break;
// Analoglabels
case 'A' : // DONE 0.71g
if (Parser::decode64(RX))
{
com->stop_resend();
 
int Position = RX.decode[0];
if (Position != 31)
{
/*
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);
}
Position ++;
TX_Data[0] = Position;
o_Connection->send_Cmd('a', ADDRESS_ALL, TX_Data, 1, true);*/
}
if (Position == 31)
{
/*
for (int a = 0; a < MaxAnalog; a++)
{
lb_Analog[a]->setText(Settings->Analog1.Label[a]);
}
Settings->Analog1.Version = QString(Mode.Version);
Settings->write_Settings_AnalogLabels(HardwareID);
config_Plot();*/
}
}
break;
// Debug-Daten
case 'D' : // DONE 0.71g
if (Parser::decode64(RX))
{
for (int i = 0; i < MaxAnalog; i++)
{
//AnalogData[i] = Parser::dataToInt(RX.decode, (i * 2) + 2);
}
//show_DebugData();
}
break;
// Version
case 'V' : // DONE 0.71h
if (Parser::decode64(RX))
{
com->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.Hardware = HardwareType[Mode.ID];
//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().data;
setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " +
Mode.Hardware + " " +
Mode.Version);
 
if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR)
{
// AllowSend = false;
QMessageBox::warning(this, QA_NAME,
tr("Serielles Protokoll Inkompatibel. \nBitte neue Programmversion installieren,"), QMessageBox::Ok);
}
 
if (ac_NoDebug->isChecked())
{
TX_Data[0] = 0;
}
else
if (ac_FastDebug->isChecked())
{
TX_Data[0] = Settings->Data.Debug_Fast / 10;
}
else
{
TX_Data[0] = Settings->Data.Debug_Slow / 10;
}
 
o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false);
 
// Wenn MK3MAG dann andauernd Daten neu anfragen.
if (Mode.ID == ADDRESS_MK3MAG)
{
TickerEvent[3] = true;
rb_SelMag->setChecked(true);
}
 
// Wenn NaviCtrl dann hier.
if (Mode.ID == ADDRESS_NC)
{
rb_SelNC->setChecked(true);
 
if (ac_NoNavi->isChecked())
{
TX_Data[0] = 0;
}
else
if (ac_FastNavi->isChecked())
{
TX_Data[0] = Settings->Data.Navi_Fast / 10;
}
else
{
TX_Data[0] = Settings->Data.Navi_Slow / 10;
}
 
o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false);
}
 
 
// Wenn FlightCtrl dann Settings abfragen.
if (Mode.ID == ADDRESS_FC)
{
rb_SelFC->setChecked(true);
{
TX_Data[0] = 0xff;
TX_Data[1] = 0;
 
// DEP: Raus wenn Resend implementiert.
// ToolBox::Wait(SLEEP);
o_Connection->send_Cmd('q', ADDRESS_FC, TX_Data, 1, true);
qDebug("FC - Get Settings");
}
}
// Wenn nicht Lesen und Schreiben der Settings deaktivieren.
else
{
f_Settings->pb_Read->setDisabled(true);
f_Settings->pb_Write->setDisabled(true);
}
 
Settings->read_Settings_Analog(HardwareID);
Settings->read_Settings_AnalogLabels(HardwareID);
 
if (Settings->Analog1.Version != QString(Mode.Version))
{
lb_Status->setText(tr("Analoglabel-Version unterschiedlich. Lese Analoglabels neu aus."));
slot_ac_GetLabels();
}
else
for (int a = 0; a < MaxAnalog; a++)
{
lb_Analog[a]->setText(Settings->Analog1.Label[a]);
}
config_Plot();*/
}
break;
}
}
}
/QMK-Groundstation/branches/libMK/libMK/Handler.h
0,0 → 1,60
#ifndef HANDLER_H
#define HANDLER_H
#include <string>
#include "Parser.h"
#include "Communication.h"
#include "Kopter.h"
#include "../Parameter_Positions.h"
#include "../typedefs.h"
 
/**
* The Handler handels commands that are send from/to the Mikrokopter
* and parses them using the Parser-class.
*/
 
class Handler {
private:
Communication * com;
 
KopterData * data;
sRxData rxData;
public:
 
Handler(Communication * com, KopterData * data);
//FlightCtrl commands
void get_flightctrl_settings(int index);
void set_flightctrl_settings(char * tx_data);
void motor_test(sMotor motor);
void reset_motor();
void read_motor_mixer();
void write_motor_mixer(char * tx_data, int length);
int get_motor_config(char * tx_data);
 
//NaviCtrl commands
void set_navictrl_debug(int speed);
void stop_navictrl_debug();
void send_waypoint(Waypoint_t desired_pos);
void add_waypoint(Waypoint_t wp);
void delete_waypoints();
 
//switch between MK modules/components
void switch_navictrl();
void switch_flightctrl();
void switch_mk3mag();
 
//commands for MK3MAG
 
//commands for all modules/components
void set_all_debug(int speed);
void stop_all_debug();
void get_analog();
void show_lcd();
void lcd_up();
void lcd_down();
void get_version();
void get_ppm_channels();
 
void receive_data(sRxData rx);
};
 
#endif
/QMK-Groundstation/branches/libMK/libMK/Kopter.h
0,0 → 1,99
#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 MotorData
{
int motor[16][4];
string mixerName;
int mixerVersion;
};
 
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 sWayPoint
{
double Longitude;
double Latitude;
double Altitude;
int Time;
};
 
/**
* The KopterData class represents the current state of the MikroKopter.
* It containes all data that was sent from the Mikrokopter
*/
class KopterData {
public:
sMode mode;
sNaviData navi;
sMotor motor;
int analogData[MaxAnalog];
// current LCD page
int lcd_cur;
//max count of LCD pages
int lcd_max;
};
 
#endif
/QMK-Groundstation/branches/libMK/libMK/Parser.cpp
0,0 → 1,203
#include <Parser.h>
 
// Base64 Decoder
// see Parser.h for details about sRxData
bool Parser::decode64(sRxData &rx)
{
int length = rx.str.size();
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) {
return false;
//TODO: catch error to show that something went wrong during the decode process
//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 (length == 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
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 = pow(10, count);
 
float temp = value;
 
return temp / 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(string RXstr)
{
int length = RXstr.size();
int CRC = 0;
char *RX = (char *)RXstr.c_str();
 
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(string TX)
{
int length = TX.size();
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/libMK/libMK/Parser.h
0,0 → 1,38
#ifndef PARSER_H
#define PARSER_H
#include <string>
#include <cmath>
 
/**
* The Parser gets values from the Mikrokopter-USART interface
* and parses them into a sRxData-Struct
*/
 
using namespace std;
 
struct sRxData
{
char *input;
string str;
int decode[150];
int decLen;
};
 
class Parser {
public:
static bool decode64(sRxData &rx);
static string encode64(char data[150],unsigned int length);
 
static string add_CRC(string TX);
static bool check_CRC(string RX);
 
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/libMK/libMK/QTCommunication.cpp
0,0 → 1,23
#include <QTCommunication.h>
 
/**
* connect to Mikrokopter
*/
void QTCommunication::connect_MK(string addr) {
};
 
/**
* send command to Mikrokopter
*/
bool QTCommunication::send_cmd(char cmd, int address, char data[150], unsigned int length, bool resend) {
return true;
};
 
/**
* stop sending commands to Mikrokopter
* stop timer
*/
void QTCommunication::stop_resend() {
};
/QMK-Groundstation/branches/libMK/libMK/QTCommunication.h
0,0 → 1,21
#ifndef QT_COMMUNICATION_H
#define QT_COMMUNICATION_H
#include <Communication.h>
 
/**
* QT specific communication class
* based on the communication interface
* using the SerialPort implementation
* by VIANNEY-LIAUD Philippe
* ( philippe.vianney.liaud gmail.com )
*/
 
using namespace std;
 
class QTCommunication : public Communication {
public:
void connect_MK(string addr);
bool send_cmd(char cmd, int address, char data[150], unsigned int length, bool resend);
void stop_resend();
};
#endif