WebSVN - Projects - Blame - Rev 399 - /QMK-Groundstation/branches/libMK/libMK/Handler.cpp

Rev	Author	Line No.	Line
391	Brean	1	#include<Handler.h>
		2
		3	/**
392	Brean	4	* Constructor that gets a communication instance
		5	*/
		6	Handler::Handler(Communication * com) {
		7	this->com = com;
		8	}
		9
399	Brean	10	//-------------FlightCtrl commands--------------------
392	Brean	11	/**
399	Brean	12	* read settings from FlightCtrl (settings index 0x00-0x05)
		13	*/
		14	void Handler::get_flightctrl_settings(int index) {
		15	char tx_data[2] = {index, 0};
		16	com->send_cmd('q', ADDRESS_FC, tx_data, 1, true);
		17	}
		18
		19	/**
		20	* write settings to FlightCtrl
		21	*/
		22	void Handler::set_flightctrl_settings(char * tx_data) {
		23	com->send_cmd('s', ADDRESS_FC, tx_data, MaxParameter+2, true);
		24	}
		25
		26	/**
		27	* test one or more motors
		28	*/
		29	void Handler::motor_test(sMotor motor) {
		30	char tx_data[12];
		31	for (int z = 0; z<12; z++)
		32	{
		33	tx_data[z] = motor.Speed[z];
		34	}
		35	com->send_cmd('t', ADDRESS_FC, tx_data, 12, false);
		36	}
		37
		38	/**
391	Brean	39	* read mixer values from FlightCtrl
		40	*/
		41	void Handler::read_mixer() {
396	Brean	42	char tx_data[1] = {0};
		43	//com->log("read motor mixer");
397	Brean	44	com->send_cmd('n', ADDRESS_FC, tx_data, 1, true);
393	Brean	45	}
		46
399	Brean	47	/**
		48	* write motor mixer values to FlightCtrl
		49	*/
		50	void Handler::write_mixer(char * tx_data, int length) {
		51	com->send_cmd('m', ADDRESS_FC, tx_data, length, true);
		52	}
		53
396	Brean	54	void Handler::get_motor_config() {
		55	}
		56
399	Brean	57	//-------------NaviCtrl commands--------------------
		58	/**
		59	* set debug values for NaviCtrl
		60	*/
		61	void Handler::set_navictrl_debug(int speed) {
		62	char tx_data[1] = { speed };
		63	com->send_cmd('o', ADDRESS_NC, tx_data, 1, false);
		64	}
		65
		66	/**
		67	* stop debug for NaviCtrl
		68	*/
		69	void Handler::stop_navictrl_debug() {
		70	set_navictrl_debug(0);
		71	}
		72
		73	/**
		74	* send a waypoint to the NaviCtrl (the copter will fly to the position emidiately)
		75	*/
		76	void Handler::send_waypoint(Waypoint_t desired_pos) {
		77	com->send_cmd('s', ADDRESS_NC, (char *)&desired_pos, sizeof(desired_pos), false);
		78	}
		79
		80	/**
		81	* add waypoint to waypoint list
		82	*/
		83	void Handler::add_waypoint(Waypoint_t wp) {
		84	com->send_cmd('w', ADDRESS_NC, (char *)&wp, sizeof(wp), false);
		85	}
		86
		87	/**
		88	* clear waypoint list on MK
		89	*/
		90	void Handler::delete_waypoints() {
		91	Waypoint_t wp;
		92	wp.Position.Status = INVALID;
		93	send_waypoint(wp);
		94	}
		95	//-------------switch between Hardware--------------------
		96	void Handler::switch_navictrl() {
		97	char tx_data[6] = { 0x1B, 0x1B, 0x55, 0xAA, 0x00, '\r'};
		98	com->send_cmd('#', ADDRESS_NC, tx_data, 6, false);
		99	}
		100
		101	void Handler::switch_flightctrl() {
		102	char tx_data[1] = { 0 };
		103	com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
		104	}
		105
		106	void Handler::switch_mk3mag() {
		107	char tx_data[1] = { 1 };
		108	com->send_cmd('u', ADDRESS_NC, tx_data, 1, false);
		109	}
		110
		111	//-------------commands for MK3MAG-----------------
		112
		113
		114	//-------------commands for all--------------------
		115
		116	/**
		117	* set debug values for all components
		118	*/
		119	void Handler::set_all_debug(int speed) {
		120	char tx_data[1] = { speed };
		121	com->send_cmd('d', ADDRESS_ALL, tx_data, 1, false);
		122	}
		123
		124	/**
		125	* stop debug for all components
		126	*/
		127	void Handler::stop_all_debug() {
		128	set_all_debug(0);
		129	}
		130
		131	/**
		132	* get all analog labels
		133	*/
		134	void Handler::get_analog() {
		135	char tx_data[1] = { 0 };
		136	com->send_cmd('a', ADDRESS_ALL, tx_data, 1, true);
		137	}
		138
		139	/**
		140	* get values from LCD / show LCD
		141	*/
		142	void Handler::show_lcd() {
		143	char tx_data[1] = {0};
		144	com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
		145	}
		146
		147	/**
		148	* got to next LCD Page
		149	*/
		150	void Handler::lcd_up() {
		151	char tx_data[2] = { 0, 0 };
		152	if (lcd_cur != lcd_max)
		153	tx_data[0] = lcd_cur+1;
		154	com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
		155	}
		156
		157	/**
		158	* got to previous LCD Page
		159	*/
		160	void Handler::lcd_down() {
		161	char tx_data[2] = { 0, 0 };
		162	if (lcd_cur != 0)
		163	tx_data[0] = lcd_cur-1;
		164	com->send_cmd('l', ADDRESS_ALL, tx_data, 1, true);
		165	}
		166
		167	void Handler::get_version() {
		168	//TODO: Check if is this correct or do we need data from switch_...
		169	char tx_data[1] = { 0 };
		170	com->send_cmd('v', ADDRESS_ALL, tx_data, 0, true);
		171	}
		172
		173	void Handler::get_ppm_channels() {
		174	char tx_data[1] = { 0 };
		175	com->send_cmd('p', ADDRESS_ALL, tx_data, 0, false);
		176	}
		177
		178	/**
		179	* receive data
		180	*/
393	Brean	181	void Handler::receive_data(sRxData RX) {
		182	//extract hardware ID from received Data
		183	int hardwareID = RX.input[1] - 'a';
		184	switch(hardwareID)
		185	{
		186	case ADDRESS_FC :
		187	switch(RX.input[2])
		188	{
		189	// Motor-Mixer
		190	case 'N' :
		191	if (Parser::decode64(RX))
		192	{
397	Brean	193	com->stop_resend();
393	Brean	194
		195	if (RX.decode[0] == VERSION_MIXER)
		196	{
396	Brean	197	//f_MotorMixer->set_MotorConfig(RX);
393	Brean	198	}
		199	}
		200	break;
		201	// Motor-Mixer Schreib-Bestätigung
		202	case 'M' :
		203	if (Parser::decode64(RX))
		204	{
397	Brean	205	com->stop_resend();
393	Brean	206
		207	if (RX.decode[0] == 1)
		208	{
396	Brean	209	//lb_Status->setText(tr("MotorMixer-Daten in FC geschrieben."));
393	Brean	210	}
		211	}
		212	break;
		213
		214	// Stick-Belegung der Fernsteuerung
		215	case 'P' : // DONE 0.71g
		216	if (Parser::decode64(RX))
		217	{
396	Brean	218	/*f_Settings->pb_K1->setValue(Parser::dataToInt(RX.decode, 2,true));
393	Brean	219	f_Settings->pb_K2->setValue(Parser::dataToInt(RX.decode, 4,true));
		220	f_Settings->pb_K3->setValue(Parser::dataToInt(RX.decode, 6,true));
		221	f_Settings->pb_K4->setValue(Parser::dataToInt(RX.decode, 8,true));
		222	f_Settings->pb_K5->setValue(Parser::dataToInt(RX.decode, 10 ,true));
		223	f_Settings->pb_K6->setValue(Parser::dataToInt(RX.decode, 12,true));
		224	f_Settings->pb_K7->setValue(Parser::dataToInt(RX.decode, 14,true));
396	Brean	225	f_Settings->pb_K8->setValue(Parser::dataToInt(RX.decode, 16,true));*/
393	Brean	226	}
		227	break;
		228	// Settings lesen
		229	case 'Q' : // DONE 0.71g
		230	if (Parser::decode64(RX))
		231	{
397	Brean	232	com->stop_resend();
393	Brean	233
		234	if (RX.decode[1] == VERSION_SETTINGS)
		235	{
		236	int Settings_ID = RX.decode[0];
396	Brean	237	/*for (int a = 0; a < MaxParameter; a++)
393	Brean	238	{
		239	FCSettings[a] = RX.decode[a + 2];
		240	}
		241	f_Settings->show_FCSettings(Settings_ID, FCSettings);
		242	f_Settings->pb_Read->setEnabled(true);
396	Brean	243	f_Settings->pb_Write->setEnabled(true);*/
393	Brean	244	}
		245	else
		246	{
396	Brean	247	/*f_Settings->pb_Read->setDisabled(true);
393	Brean	248	f_Settings->pb_Write->setDisabled(true);
		249
		250	QString name = QString("Versionen inkompatibel.\n") +
		251	QString("Version von GroundStation benoetigt: ") +
		252	QString(VERSION_SETTINGS) +
		253	QString("\nVersion auf der FlightCtrl: ") +
		254	QString(RX.decode[1]) +
		255	QString("\nParameterbearbeitung nicht moeglich.");
		256	QMessageBox::warning(this, QA_NAME,
396	Brean	257	name, QMessageBox::Ok);*/
393	Brean	258	}
		259	}
		260	break;
396	Brean	261	// Settings written
393	Brean	262	case 'S' : // DONE 0.71g
397	Brean	263	com->stop_resend();
396	Brean	264	//TODO: QMessagebox("settings written successful") ?
393	Brean	265	break;
		266	}
		267
		268	case ADDRESS_NC :
		269	switch(RX.input[2])
		270	{
		271	// Navigationsdaten
		272	case 'O' : // NOT DONE 0.12h
		273	if (Parser::decode64(RX))
		274	{
396	Brean	275	//new_NaviData(RX);
393	Brean	276	}
		277	break;
		278	}
		279	// case ADDRESS_MK3MAG :
		280
		281	default :
		282	switch(RX.input[2])
		283	{
		284	// LCD-Anzeige
		285	case 'L' : // DONE 0.71g
		286	if (Parser::decode64(RX))
		287	{
397	Brean	288	com->stop_resend();
393	Brean	289
396	Brean	290	/*int LCD[150];
393	Brean	291	memcpy(LCD,RX.decode, sizeof(RX.decode));
		292
		293	f_LCD->show_Data(LCD);
		294
		295	LCD_Page = RX.decode[0];
		296	LCD_MAX_Page = RX.decode[1];
396	Brean	297	*/
393	Brean	298	}
		299	break;
		300	// Analoglabels
		301	case 'A' : // DONE 0.71g
		302	if (Parser::decode64(RX))
		303	{
397	Brean	304	com->stop_resend();
393	Brean	305
		306	int Position = RX.decode[0];
		307	if (Position != 31)
		308	{
396	Brean	309	/*
393	Brean	310	Settings->Analog1.Label[Position] = ToolBox::dataToQString(RX.decode,1,17).trimmed();
		311	if (Settings->Analog1.Label[Position] == "")
		312	{
		313	Settings->Analog1.Label[Position] = "A-" + QString("%1").arg(Position);
		314	}
		315	Position ++;
		316	TX_Data[0] = Position;
396	Brean	317	o_Connection->send_Cmd('a', ADDRESS_ALL, TX_Data, 1, true);*/
393	Brean	318	}
		319	if (Position == 31)
		320	{
396	Brean	321	/*
393	Brean	322	for (int a = 0; a < MaxAnalog; a++)
		323	{
		324	lb_Analog[a]->setText(Settings->Analog1.Label[a]);
		325	}
		326	Settings->Analog1.Version = QString(Mode.Version);
		327	Settings->write_Settings_AnalogLabels(HardwareID);
396	Brean	328	config_Plot();*/
393	Brean	329	}
		330	}
		331	break;
		332	// Debug-Daten
		333	case 'D' : // DONE 0.71g
		334	if (Parser::decode64(RX))
		335	{
		336	for (int i = 0; i < MaxAnalog; i++)
		337	{
396	Brean	338	//AnalogData[i] = Parser::dataToInt(RX.decode, (i * 2) + 2);
393	Brean	339	}
396	Brean	340	//show_DebugData();
393	Brean	341	}
		342	break;
		343	// Version
		344	case 'V' : // DONE 0.71h
		345	if (Parser::decode64(RX))
		346	{
397	Brean	347	com->stop_resend();
396	Brean	348	/*
393	Brean	349	Mode.ID = HardwareID;
		350	Mode.VERSION_MAJOR = RX.decode[0];
		351	Mode.VERSION_MINOR = RX.decode[1];
		352	Mode.VERSION_PATCH = RX.decode[4];
		353	Mode.VERSION_SERIAL_MAJOR = RX.decode[2];
		354	Mode.VERSION_SERIAL_MINOR = RX.decode[3];
		355
		356	Mode.Hardware = HardwareType[Mode.ID];
		357	//TODO: Funktion im Handler get_version() oder sowas
		358	QString version = QString("%1").arg(RX.decode[0]) + "." +
		359	QString("%1").arg(RX.decode[1]) +
		360	QString(RX.decode[4] + 'a');
		361	Mode.Version = version.toLatin1().data;
		362	setWindowTitle(QA_NAME + " v" + QA_VERSION + " - " +
		363	Mode.Hardware + " " +
		364	Mode.Version);
		365
		366	if (Mode.VERSION_SERIAL_MAJOR != VERSION_SERIAL_MAJOR)
		367	{
		368	// AllowSend = false;
		369	QMessageBox::warning(this, QA_NAME,
		370	tr("Serielles Protokoll Inkompatibel. \nBitte neue Programmversion installieren,"), QMessageBox::Ok);
		371	}
		372
		373	if (ac_NoDebug->isChecked())
		374	{
		375	TX_Data[0] = 0;
		376	}
		377	else
		378	if (ac_FastDebug->isChecked())
		379	{
		380	TX_Data[0] = Settings->Data.Debug_Fast / 10;
		381	}
		382	else
		383	{
		384	TX_Data[0] = Settings->Data.Debug_Slow / 10;
		385	}
		386
		387	o_Connection->send_Cmd('d', ADDRESS_ALL, TX_Data, 1, false);
		388
		389	// Wenn MK3MAG dann andauernd Daten neu anfragen.
		390	if (Mode.ID == ADDRESS_MK3MAG)
		391	{
		392	TickerEvent[3] = true;
		393	rb_SelMag->setChecked(true);
		394	}
		395
		396	// Wenn NaviCtrl dann hier.
		397	if (Mode.ID == ADDRESS_NC)
		398	{
		399	rb_SelNC->setChecked(true);
		400
		401	if (ac_NoNavi->isChecked())
		402	{
		403	TX_Data[0] = 0;
		404	}
		405	else
		406	if (ac_FastNavi->isChecked())
		407	{
		408	TX_Data[0] = Settings->Data.Navi_Fast / 10;
		409	}
		410	else
		411	{
		412	TX_Data[0] = Settings->Data.Navi_Slow / 10;
		413	}
		414
		415	o_Connection->send_Cmd('o', ADDRESS_NC, TX_Data, 1, false);
		416	}
		417
		418
		419	// Wenn FlightCtrl dann Settings abfragen.
		420	if (Mode.ID == ADDRESS_FC)
		421	{
		422	rb_SelFC->setChecked(true);
		423	{
		424	TX_Data[0] = 0xff;
		425	TX_Data[1] = 0;
		426
		427	// DEP: Raus wenn Resend implementiert.
		428	// ToolBox::Wait(SLEEP);
		429	o_Connection->send_Cmd('q', ADDRESS_FC, TX_Data, 1, true);
		430	qDebug("FC - Get Settings");
		431	}
		432	}
		433	// Wenn nicht Lesen und Schreiben der Settings deaktivieren.
		434	else
		435	{
		436	f_Settings->pb_Read->setDisabled(true);
		437	f_Settings->pb_Write->setDisabled(true);
		438	}
		439
		440	Settings->read_Settings_Analog(HardwareID);
		441	Settings->read_Settings_AnalogLabels(HardwareID);
		442
		443	if (Settings->Analog1.Version != QString(Mode.Version))
		444	{
		445	lb_Status->setText(tr("Analoglabel-Version unterschiedlich. Lese Analoglabels neu aus."));
		446	slot_ac_GetLabels();
		447	}
		448	else
		449	for (int a = 0; a < MaxAnalog; a++)
		450	{
		451	lb_Analog[a]->setText(Settings->Analog1.Label[a]);
		452	}
396	Brean	453	config_Plot();*/
393	Brean	454	}
		455	break;
		456	}
		457	}
391	Brean	458	}

Subversion Repositories Projects

(root)/QMK-Groundstation/branches/libMK/libMK/Handler.cpp - Rev 399