/QMK-Groundstation/branches/libMK/libMK/Communication.h |
---|
1,5 → 1,6 |
#ifndef COMMUNICATION_H |
#define COMMUNICATION_H |
#include <iostream> |
/** |
* communication interface for Mikrokopter (MK) USART connection |
/QMK-Groundstation/branches/libMK/libMK/FlightLog.cpp |
---|
13,10 → 13,11 |
#include <sys/time.h> |
void print_data(char * data, std::string type, char * color) { |
char buffer [100]; |
char buffer [20]; |
timeval timestamp; |
gettimeofday(×tamp, 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; |
33,10 → 34,38 |
#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,9 → 8,17 |
#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 { |
20,8 → 28,13 |
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,5 → 1,7 |
#include "Handler.h" |
#include "FlightLog.h" |
#include <sstream> |
#include <string> |
/** |
* Constructor that gets a communication instance |
14,6 → 16,7 |
* 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); |
} |
22,6 → 25,7 |
* 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); |
} |
34,6 → 38,7 |
{ |
tx_data[z] = motor.desired_speed[z]; |
} |
FlightLog::info_FC("testing motor"); |
com->send_cmd('t', ADDRESS_FC, tx_data, MAX_MOTORS, false); |
} |
50,6 → 55,7 |
* 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); |
59,6 → 65,7 |
* 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); |
} |
71,6 → 78,7 |
* 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); |
} |
86,6 → 94,7 |
* 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); |
} |
93,6 → 102,7 |
* 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); |
} |
106,16 → 116,19 |
} |
//-------------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); |
} |
129,6 → 142,7 |
* 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); |
} |
144,6 → 158,7 |
* 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); |
} |
152,6 → 167,7 |
* 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); |
} |
160,6 → 176,7 |
* 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; |
170,6 → 187,7 |
* 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; |
177,6 → 195,7 |
} |
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); |
183,24 → 202,41 |
} |
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]; |
//decode data |
unsigned char data[150]; |
//decoded data |
unsigned char data[MAX_DATA_SIZE]; |
Parser::decode64(incomming, length, data, 3); |
211,16 → 247,14 |
{ |
// Motor-Mixer |
case 'N' : |
//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); |
} |
//} |
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); |
} |
break; |
// Motor-Mixer Schreib-Bestätigung |
case 'M' : |
228,7 → 262,7 |
if (data[0] == 1) |
{ |
FlightLog::info("motor values written to FlightCtrl."); |
FlightLog::info_FC("motor values written to FlightCtrl."); |
//lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben.")); |
} else { |
FlightLog::warning("could not write motor values to FlightCtrl!"); |
237,7 → 271,14 |
// Stick-Belegung der Fernsteuerung |
case 'P' : // DONE 0.71g |
FlightLog::info("received stick-settings from FlightCtrl:"); |
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()); |
} |
/*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)); |
250,10 → 291,22 |
// Settings lesen |
case 'Q' : // DONE 0.71g |
com->stop_resend(); |
FlightLog::info("received settings from FlightCtrl"); |
FlightLog::info_FC("received settings"); |
if (data[1] == VERSION_SETTINGS) |
{ |
int Settings_ID = data[0]; |
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()); |
} |
/*for (int a = 0; a < MaxParameter; a++) |
{ |
FCSettings[a] = RX.decode[a + 2]; |
281,7 → 334,7 |
// Settings written |
case 'S' : // DONE 0.71g |
com->stop_resend(); |
FlightLog::info("settings written successfully to FlightCtrl"); |
FlightLog::info_FC("settings written successfully"); |
//TODO: QMessagebox("settings written successful") ? |
break; |
} |
292,7 → 345,7 |
// Navigationsdaten |
case 'O' : // NOT DONE 0.12h |
//new_NaviData(RX); |
FlightLog::info("received navigation data from NaviCtrl"); |
FlightLog::info_NC("received navigation data"); |
break; |
} |
// case ADDRESS_MK3MAG : |
304,7 → 357,7 |
case 'L' : // DONE 0.71g |
com->stop_resend(); |
FlightLog::info("received LCD page."); |
/*int LCD[150]; |
/*int LCD[MAX_DATA_SIZE]; |
memcpy(LCD,RX.decode, sizeof(RX.decode)); |
f_LCD->show_Data(LCD); |
/QMK-Groundstation/branches/libMK/libMK/Handler.h |
---|
54,6 → 54,7 |
void get_ppm_channels(); |
void receive_data(char * incomming, int length); |
void new_navi_data(char * data); |
}; |
#endif |
/QMK-Groundstation/branches/libMK/libMK/Makefile |
---|
0,0 → 1,30 |
TARGET = UNIX |
CPP = g++ |
RM = rm -f |
MAKE = make |
CXXINCS = -I"./" -I"../" |
CXXFLAGS = $(CXXINCS) -O3 |
LINKOBJ = Kopter.o FlightLog.o Parser.o Communication.o Handler.o |
all: all-before $(LINKOBJ) |
all-before: depend |
depend: |
@$(CPP) $(CXXINCS) -MM *.cpp > depend |
#kopter.o is only a header |
Kopter.o: Kopter.h |
#all other files have .cpp files |
%.o : %.cpp |
$(CPP) $(CXXFLAGS) -c -o $@ $< |
clean-custom: |
clean: clean-custom |
${RM} $(LINKOBJ) depend *~ *.a |
/QMK-Groundstation/branches/libMK/libMK/Parser.cpp |
---|
1,4 → 1,5 |
#include <Parser.h> |
#include <iostream> |
/** |
* create a frame that can be send to the MK using the |
9,7 → 10,7 |
* to look at the possible commands that are already coded |
* in data |
*/ |
void Parser::create_frame(char cmd, int address, char * data, unsigned int length) { |
void Parser::create_frame(char * send_data, 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 != '#') { |
23,7 → 24,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; |
50,14 → 51,15 |
unsigned char x,y,z; |
int decLen = 0; |
/* |
//FIXME: dies wieder einklammern! |
if (data[ptrIn] == 0) { |
return -1; |
std::cout << "decode64" << offset << " " << len << std::endl; |
FlightLog::log_data(data, len); |
if (data[offset] == 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++] - '='; |
199,26 → 201,18 |
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; |
} |
231,9 → 225,7 |
unsigned int tmpCRC = 0; |
for(int i = 0; i < length; i++) |
{ |
tmpCRC += tx[i]; |
} |
tmpCRC %= 4096; |
241,4 → 233,3 |
tx[length+1] = '=' + tmpCRC % 64; |
tx[length+2] = '\0'; |
} |
/QMK-Groundstation/branches/libMK/libMK/Parser.h |
---|
1,6 → 1,7 |
#ifndef PARSER_H |
#define PARSER_H |
#include "Kopter.h" |
#include "FlightLog.h" |
#include <cmath> |
#include <string> |
11,7 → 12,7 |
class Parser { |
public: |
static void create_frame(char cmd, int address, char * data, unsigned int length); |
static void create_frame(char * send_data, 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,24 → 44,21 |
}; |
/** |
* 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(cmd, address, data, length); |
Parser::create_frame(send_data, cmd, address, data, length); |
if (resend) { |
//resend every 2 seconds |
resendTimer.start(2000); |
resendData = data; |
resendData = send_data; |
} |
//TODO: save outgoing data for debug purpose |
std::cout << "send: " << data << std::endl; |
QByteArray temp(data); |
std::cout << "send: "; |
FlightLog::log_data(send_data, length+3); |
QByteArray temp(send_data); |
serial->sendData(temp); |
} else { |
//FIXME: Error message: not connected |
87,18 → 84,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 |
* handel received data from MK |
*/ |
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: " << data << std::endl; |
std::cout << "receive: "; |
FlightLog::log_data(data, length); |
//handle incomming data |
//FIXME: HIER! Hardware-Id = Mode.id |
handler->receive_data(data, length); |
} |
/QMK-Groundstation/branches/libMK/libMK/QTSerialCommunication.h |
---|
5,6 → 5,7 |
#include "Communication.h" |
#include "Handler.h" |
#include "../SerialPort/ManageSerialPort.h" |
#include "FlightLog.h" |
/** |
* QT specific communication class |
/QMK-Groundstation/branches/libMK/testing/HandlerTest.cpp |
---|
0,0 → 1,12 |
#include "HandlerTest.h" |
void HandlerTest::setUp(void) { |
handler = new Handler(&com, &data); |
}; |
void HandlerTest::get_flightctrl_settings_test(void) { |
handler->get_flightctrl_settings(0); |
}; |
void HandlerTest::tearDown(void) { |
}; |
/QMK-Groundstation/branches/libMK/testing/HandlerTest.h |
---|
0,0 → 1,31 |
#ifndef HANDLER_TEST_H |
#define HANDLER_TEST_H |
#include <cppunit/TestFixture.h> |
#include <cppunit/extensions/HelperMacros.h> |
#include "../libMK/Handler.h" |
#include "../libMK/Kopter.h" |
#include "../libMK/Communication.h" |
class HandlerTest : public CPPUNIT_NS :: TestFixture |
{ |
CPPUNIT_TEST_SUITE (HandlerTest); |
CPPUNIT_TEST (get_flightctrl_settings_test); |
CPPUNIT_TEST_SUITE_END (); |
public: |
void setUp (void); |
void tearDown (void); |
protected: |
void get_flightctrl_settings_test(void); |
private: |
KopterData data; |
Communication com; |
Handler * handler; |
}; |
CPPUNIT_TEST_SUITE_REGISTRATION( HandlerTest ); |
#endif |
/QMK-Groundstation/branches/libMK/testing/Makefile |
---|
0,0 → 1,30 |
CPP = g++ |
CCFLAGS = -c #-g -ansi |
LIBS = /usr/lib/libcppunit.a -L"../libMK" |
INCS = -I"./" -I"../" -I"../libMK" |
%.o : %.cpp |
$(CPP) $(CCFLAGS) -c $< |
LINKOBJ = ../libMK/*.o |
OBJ = HandlerTest.o main.o |
LIBMK = libMK |
LIBMK_DIR = ../libMK/ |
all: $(LIBMK) test |
test: $(OBJ) |
${CPP} -o test $(LINKOBJ) $(OBJ) ${LIBS} |
./test |
$(LIBMK): |
$(MAKE) -C $(LIBMK_DIR) |
main.o: main.cpp |
HandlerTest.o: HandlerTest.cpp |
#ParserTest.o: ParserTest.cpp |
clean: |
rm -f *.o test; \ |
$(MAKE) -C $(LIBMK_DIR) clean |
/QMK-Groundstation/branches/libMK/testing/main.cpp |
---|
0,0 → 1,32 |
#include <cppunit/CompilerOutputter.h> |
#include <cppunit/extensions/TestFactoryRegistry.h> |
#include <cppunit/TestResult.h> |
#include <cppunit/TestResultCollector.h> |
#include <cppunit/TestRunner.h> |
#include <cppunit/BriefTestProgressListener.h> |
int main( int argc, char* argv[] ) |
{ |
// Create the event manager and test controller |
CPPUNIT_NS::TestResult controller; |
// Add a listener that colllects test result |
CPPUNIT_NS::TestResultCollector result; |
controller.addListener( &result ); |
// Add a listener to show progress of single tests |
CPPUNIT_NS :: BriefTestProgressListener progress; |
controller.addListener (&progress); |
// Add the top suite to the test runner |
CPPUNIT_NS::TestRunner runner; |
runner.addTest( CPPUNIT_NS::TestFactoryRegistry::getRegistry().makeTest() ); |
runner.run( controller ); |
// Print test in a compiler compatible format. |
CPPUNIT_NS::CompilerOutputter outputter( &result, std::cerr ); |
outputter.write(); |
return result.wasSuccessful() ? 0 : 1; |
} |