Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 513 → Rev 512

/QMK-Groundstation/branches/libMK/testing/HandlerTest.cpp
File deleted
/QMK-Groundstation/branches/libMK/testing/HandlerTest.h
File deleted
/QMK-Groundstation/branches/libMK/testing/main.cpp
File deleted
/QMK-Groundstation/branches/libMK/testing/Makefile
File deleted
/QMK-Groundstation/branches/libMK/libMK/Makefile
File deleted
/QMK-Groundstation/branches/libMK/libMK/Communication.h
1,6 → 1,5
#ifndef COMMUNICATION_H
#define COMMUNICATION_H
#include <iostream>
 
/**
* communication interface for Mikrokopter (MK) USART connection
/QMK-Groundstation/branches/libMK/libMK/FlightLog.cpp
13,11 → 13,10
#include <sys/time.h>
 
void print_data(char * data, std::string type, char * color) {
char buffer [20];
char buffer [100];
timeval timestamp;
gettimeofday(&timestamp, 0);
sprintf(buffer, "%i.%03i", timestamp.tv_sec, (timestamp.tv_usec+500)/1000);
//alternative to show human readable output
/*
time_t rawtime;
struct tm * timeinfo;
34,38 → 33,10
#endif
}
 
void FlightLog::log_data(char * data, int length) {
printf("raw data:");
for( int i = 0 ; i < length; i++ ){
printf("%c", data[i]);
}
printf(" hex:");
for( int i = 0 ; i < length ; i++ ){
printf( "%02hhx " , data[i] );
}
printf( "\n" );
}
 
void FlightLog::info(char * data) {
print_data(data, "INFO", INFO_COLOR);
}
 
void FlightLog::info_FC(char * data) {
print_data(data, "INFO FC", INFO_FC_COLOR);
}
 
void FlightLog::info_NC(char * data) {
print_data(data, "INFO NC", INFO_NC_COLOR);
}
 
void FlightLog::info_GPS(char * data) {
print_data(data, "INFO GPS", INFO_GPS_COLOR);
}
 
void FlightLog::info_MK3(char * data) {
print_data(data, "INFO MK3", INFO_MK3_COLOR);
}
 
void FlightLog::warning(char * data) {
print_data(data, "WARNING", WARNING_COLOR);
}
/QMK-Groundstation/branches/libMK/libMK/FlightLog.h
8,17 → 8,9
#define ERROR_COLOR COLOR_FG_DARKRED
#define WARNING_COLOR COLOR_BG_LIGHTYELLOW
#define INFO_COLOR COLOR_NORMAL
//FlightCtrl
#define INFO_FC_COLOR COLOR_NORMAL
//NaviCtrl
#define INFO_NC_COLOR COLOR_FG_DARKBLUE
//GPS
#define INFO_GPS_COLOR COLOR_FG_DARKGREY
//MK3MAG
#define INFO_MK3_COLOR COLOR_FG_DARKGREEN
 
/**
* The FlightLog saves a human-readable log, so that we can see what happened.
* The FlightLog saves a human-readable log, so that we can see, what happened.
*/
 
class FlightLog {
28,13 → 20,8
 
public:
static void info(char * dat);
static void info_FC(char * dat);
static void info_NC(char * dat);
static void info_GPS(char * dat);
static void info_MK3(char * dat);
static void warning(char * dat);
static void error(char * dat);
static void log_data(char * data, int length);
};
 
#endif
/QMK-Groundstation/branches/libMK/libMK/Handler.cpp
1,7 → 1,5
#include "Handler.h"
#include "FlightLog.h"
#include <sstream>
#include <string>
 
/**
* Constructor that gets a communication instance
16,7 → 14,6
* read settings from FlightCtrl (settings index 0x00-0x05)
*/
void Handler::get_flightctrl_settings(int index) {
FlightLog::info_FC("getting FlightCtrl settings");
char tx_data[2] = {index, 0};
com->send_cmd('q', ADDRESS_FC, tx_data, 1, true);
}
25,7 → 22,6
* write settings to FlightCtrl
*/
void Handler::set_flightctrl_settings(char * tx_data) {
FlightLog::info_FC("setting FlightCtrl settings");
com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true);
}
 
38,7 → 34,6
{
tx_data[z] = motor.desired_speed[z];
}
FlightLog::info_FC("testing motor");
com->send_cmd('t', ADDRESS_FC, tx_data, MAX_MOTORS, false);
}
 
55,7 → 50,6
* read mixer values from FlightCtrl
*/
void Handler::read_motor_mixer() {
FlightLog::info_FC("reading motor mixer values");
char tx_data[1] = {0};
//com->log("read motor mixer");
com->send_cmd('n', ADDRESS_FC, tx_data, 1, true);
65,7 → 59,6
* write motor mixer values to FlightCtrl
*/
void Handler::write_motor_mixer(char * tx_data, int length) {
FlightLog::info_FC("setting motor mixer values");
com->send_cmd('m', ADDRESS_FC, tx_data, length, true);
}
 
78,7 → 71,6
* set debug values for NaviCtrl
*/
void Handler::set_navictrl_debug(int speed) {
FlightLog::info_NC("setting NaviCtrl debug speed");
char tx_data[1] = { speed };
com->send_cmd('o', ADDRESS_NC, tx_data, 1, false);
}
94,7 → 86,6
* send a waypoint to the NaviCtrl (the MikroKopter will fly to the position emidiately)
*/
void Handler::send_waypoint(Waypoint_t desired_pos) {
FlightLog::info_GPS("sending Waypoint");
com->send_cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false);
}
 
102,7 → 93,6
* add waypoint to waypoint list
*/
void Handler::add_waypoint(Waypoint_t wp) {
FlightLog::info_GPS("adding Waypoint");
com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
}
 
116,19 → 106,16
}
//-------------switch between Hardware--------------------
void Handler::switch_navictrl() {
FlightLog::info("switching to NaviCtrl");
char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'};
com->send_cmd('#', ADDRESS_NC, tx_data, 6, false);
}
 
void Handler::switch_flightctrl() {
FlightLog::info("switching to FlightCtrl");
char tx_data[1] = { 0 };
com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
}
 
void Handler::switch_mk3mag() {
FlightLog::info("switching to MK3MAG");
char tx_data[1] = { 1 };
com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
}
142,7 → 129,6
* set debug values for all components
*/
void Handler::set_all_debug(int speed) {
FlightLog::info("setting debug speed");
char tx_data[1] = { speed };
com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false);
}
158,7 → 144,6
* get all analog labels
*/
void Handler::get_analog() {
FlightLog::info("get analog values");
char tx_data[1] = { 0 };
com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true);
}
167,7 → 152,6
* get values from LCD / show LCD
*/
void Handler::show_lcd() {
FlightLog::info("show LCD");
char tx_data[1] = {0};
com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
}
176,7 → 160,6
* got to next LCD Page
*/
void Handler::lcd_up() {
FlightLog::info("going up one LCD page");
char tx_data[2] = { 0, 0 };
if (data->lcd_cur != data->lcd_max)
tx_data[0] = data->lcd_cur+1;
187,7 → 170,6
* got to previous LCD Page
*/
void Handler::lcd_down() {
FlightLog::info("going down one LCD page");
char tx_data[2] = { 0, 0 };
if (data->lcd_cur != 0)
tx_data[0] = data->lcd_cur-1;
195,7 → 177,6
}
 
void Handler::get_version() {
FlightLog::info("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);
202,41 → 183,24
}
 
void Handler::get_ppm_channels() {
FlightLog::info("get PPM channels");
char tx_data[1] = { 0 };
com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
}
 
void Handler::new_navi_data(char * data) {
std::string mode;
switch(data[N_NC_FLAGS])
{
case 0x01 : mode = "Free"; break;
case 0x02 : mode = "Position Hold"; break;
case 0x04 : mode = "Coming Home"; break;
case 0x08 : mode = "Range Limit"; break;
case 0x10 : mode = "Serial Error"; break;
case 0x20 : mode = "Target reached"; break;
case 0x40 : mode = "Manual Control"; break;
}
}
 
/**
* receive data
*/
void Handler::receive_data(char * incomming, int length) {
if (incomming[0] != '#') {
if (incomming[0] != '#')
FlightLog::error("this frame is not correct");
FlightLog::info(incomming);
return;
}
int hardwareID = incomming[1] - 'a';
 
//The cmd is also known as ID-Byte (or ID for short) in the wiki-dokumentation
char cmd = incomming[2];
//decoded data
unsigned char data[MAX_DATA_SIZE];
//decode data
unsigned char data[150];
 
Parser::decode64(incomming, length, data, 3);
 
247,14 → 211,16
{
// Motor-Mixer
case 'N' :
com->stop_resend();
//decoded data
FlightLog::info_FC("received motortest values from FlightCtrl");
if (data[0] == VERSION_MIXER)
{
//TODO: Handler::receivedMotorConfig(incomming)
//f_MotorMixer->set_MotorConfig(RX);
}
//if (Parser::decode64(RX))
//{
com->stop_resend();
//decoded data
FlightLog::info("received motortest values from FlightCtrl");
if (data[0] == VERSION_MIXER)
{
//f_MotorMixer->set_MotorConfig(RX);
}
//}
break;
// Motor-Mixer Schreib-Best├Ątigung
case 'M' :
262,7 → 228,7
 
if (data[0] == 1)
{
FlightLog::info_FC("motor values written to FlightCtrl.");
FlightLog::info("motor values written to FlightCtrl.");
//lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
} else {
FlightLog::warning("could not write motor values to FlightCtrl!");
271,14 → 237,7
 
// Stick-Belegung der Fernsteuerung
case 'P' : // DONE 0.71g
FlightLog::info_FC("received stick-settings:");
for (int i = 1; i <= 8; i++) {
std::stringstream d;
d << "Stickvalue for Stick nr. " << i << ": " << Parser::dataToInt((char *)data, i*2, true);;
std::string val;
d >> val;
FlightLog::info_FC((char *)val.c_str());
}
FlightLog::info("received stick-settings from FlightCtrl:");
/*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));
291,22 → 250,10
// Settings lesen
case 'Q' : // DONE 0.71g
com->stop_resend();
FlightLog::info_FC("received settings");
FlightLog::info("received settings from FlightCtrl");
if (data[1] == VERSION_SETTINGS)
{
int settings_id = data[0];
std::stringstream d;
d << "Settings-ID: " << settings_id;
std::string val;
d >> val;
FlightLog::info_FC((char *)val.c_str());
for (int i = 0; i < MaxParameter; i++) {
d.flush();
d << "Value for Parameter nr. " << i << ": " << (int)data[i+2];
d >> val;
FlightLog::info_FC((char *)val.c_str());
}
int Settings_ID = data[0];
/*for (int a = 0; a < MaxParameter; a++)
{
FCSettings[a] = RX.decode[a + 2];
334,7 → 281,7
// Settings written
case 'S' : // DONE 0.71g
com->stop_resend();
FlightLog::info_FC("settings written successfully");
FlightLog::info("settings written successfully to FlightCtrl");
//TODO: QMessagebox("settings written successful") ?
break;
}
345,7 → 292,7
// Navigationsdaten
case 'O' : // NOT DONE 0.12h
//new_NaviData(RX);
FlightLog::info_NC("received navigation data");
FlightLog::info("received navigation data from NaviCtrl");
break;
}
// case ADDRESS_MK3MAG :
357,7 → 304,7
case 'L' : // DONE 0.71g
com->stop_resend();
FlightLog::info("received LCD page.");
/*int LCD[MAX_DATA_SIZE];
/*int LCD[150];
memcpy(LCD,RX.decode, sizeof(RX.decode));
 
f_LCD->show_Data(LCD);
/QMK-Groundstation/branches/libMK/libMK/Handler.h
54,7 → 54,6
void get_ppm_channels();
 
void receive_data(char * incomming, int length);
void new_navi_data(char * data);
};
 
#endif
/QMK-Groundstation/branches/libMK/libMK/Parser.cpp
1,5 → 1,4
#include <Parser.h>
#include <iostream>
 
/**
* create a frame that can be send to the MK using the
10,7 → 9,7
* to look at the possible commands that are already coded
* in data
*/
void Parser::create_frame(char * send_data, char cmd, int address, char * data, unsigned int length) {
void Parser::create_frame(char cmd, int address, char * data, unsigned int length) {
//if # is cmd we do not touch anything, because
// the command is already encoded in data
if (cmd != '#') {
24,7 → 23,7
char * send_data = (char *)malloc(buf_len);
*/
Parser::encode64(data, length);
char send_data[150];
send_data[0]='#';
send_data[1]=(char)address+'a';
send_data[2]=cmd;
51,15 → 50,14
unsigned char x,y,z;
 
int decLen = 0;
 
std::cout << "decode64" << offset << " " << len << std::endl;
FlightLog::log_data(data, len);
if (data[offset] == 0) {
//return -1;
/*
//FIXME: dies wieder einklammern!
if (data[ptrIn] == 0) {
return -1;
//TODO: catch error to show that something went wrong during the decode process
//throw "Nothing received";
FlightLog::warning("incorrect data received");
}
*/
//decode data
while(len) {
a = data[offset++] - '=';
201,18 → 199,26
int CRC = 0;
 
if (rx[1] == 127)
{
rx[1] = 0;
}
 
for(int i=0; i < length-2; i++)
CRC+=rx[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;
}
225,7 → 231,9
unsigned int tmpCRC = 0;
 
for(int i = 0; i < length; i++)
{
tmpCRC += tx[i];
}
 
tmpCRC %= 4096;
 
233,3 → 241,4
tx[length+1] = '=' + tmpCRC % 64;
tx[length+2] = '\0';
}
 
/QMK-Groundstation/branches/libMK/libMK/Parser.h
1,7 → 1,6
#ifndef PARSER_H
#define PARSER_H
#include "Kopter.h"
#include "FlightLog.h"
#include <cmath>
#include <string>
 
12,7 → 11,7
 
class Parser {
public:
static void create_frame(char * send_data, char cmd, int address, char * data, unsigned int length);
static void create_frame(char cmd, int address, char * data, unsigned int length);
 
static int decode64(char * data, int len, unsigned char *ptrOut, int offset);
static void encode64(char data[150],unsigned int length);
/QMK-Groundstation/branches/libMK/libMK/QTSerialCommunication.cpp
44,21 → 44,24
};
 
/**
* received new data from MK
*/
 
 
/**
* send command to Mikrokopter
*/
void QTSerialCommunication::send_cmd(char cmd, int address, char data[150], unsigned int length, bool resend) {
char send_data[150];
if (is_connected()) {
Parser::create_frame(send_data, cmd, address, data, length);
Parser::create_frame(cmd, address, data, length);
if (resend) {
//resend every 2 seconds
resendTimer.start(2000);
resendData = send_data;
resendData = data;
}
//TODO: save outgoing data for debug purpose
std::cout << "send: ";
FlightLog::log_data(send_data, length+3);
QByteArray temp(send_data);
std::cout << "send: " << data << std::endl;
QByteArray temp(data);
serial->sendData(temp);
} else {
//FIXME: Error message: not connected
84,18 → 87,18
* and just give it to the handler.
*/
void QTSerialCommunication::slot_received_data(const QByteArray & data) {
std::cout << "receive something" << std::endl;
received_data((char *)data.data(), data.length());
}
 
/**
* handel received data from MK
* handel received data
*/
void QTSerialCommunication::received_data(char * data, int length) {
//Debug: print oud debug data
//TODO: save data in a file for debug purposes
std::cout << "receive: ";
FlightLog::log_data(data, length);
std::cout << "receive: " << data << std::endl;
//handle incomming data
//FIXME: HIER! Hardware-Id = Mode.id
 
handler->receive_data(data, length);
}
/QMK-Groundstation/branches/libMK/libMK/QTSerialCommunication.h
5,7 → 5,6
#include "Communication.h"
#include "Handler.h"
#include "../SerialPort/ManageSerialPort.h"
#include "FlightLog.h"
 
/**
* QT specific communication class