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

Subversion Repositories Projects

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