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

Subversion Repositories Projects

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