WebSVN - Projects - Blame - Rev 1471 - /Transportables_Koptertool/branch/GPL_PKT_V3_5_8a

Rev	Author	Line No.	Line
1471	-	1	/*****************************************************************************
		2	* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
		3	* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
		4	* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
		5	* Copyright (C) 2011 Harald Bongartz *
		6	* *
		7	* This program is free software; you can redistribute it and/or modify *
		8	* it under the terms of the GNU General Public License as published by *
		9	* the Free Software Foundation; either version 2 of the License. *
		10	* *
		11	* This program is distributed in the hope that it will be useful, *
		12	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
		13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
		14	* GNU General Public License for more details. *
		15	* *
		16	* You should have received a copy of the GNU General Public License *
		17	* along with this program; if not, write to the *
		18	* Free Software Foundation, Inc., *
		19	* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
		20	* *
		21	* *
		22	* Credits to: *
		23	* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
		24	* http://www.mikrokopter.de *
		25	* Gregor "killagreg" Stobrawa for his version of the MK code *
		26	* Thomas Kaiser "thkais" for the original project. See *
		27	* http://www.ft-fanpage.de/mikrokopter/ *
		28	* http://forum.mikrokopter.de/topic-4061-1.html *
		29	* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
		30	* http://www.mylifesucks.de/oss/c-osd/ *
		31	* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
		32	*****************************************************************************/
		33
		34
		35	#include "cpu.h"
		36	#include <avr/io.h>
		37	#include <avr/interrupt.h>
		38	#include <avr/pgmspace.h>
		39	#include <string.h>
		40	#include <stdlib.h>
		41	#include "main.h"
		42	#include "motortest.h"
		43	#include "lcd.h"
		44	#include "timer.h"
		45	#include "twimaster.h"
		46	//#include "menu.h"
		47	#include "uart1.h"
		48	#include "usart.h"
		49	#include "messages.h"
		50
		51
		52
		53
		54	uint8_t m;
		55	uint8_t mmode; // 0=Value 1=Motor
		56	uint8_t v;
		57
		58	volatile uint8_t i2c_state;
		59	volatile uint8_t motor_addr = 0;
		60
		61
		62	//--------------------------------------------------------------
		63	// Senden der Motorwerte per I2C-Bus
		64	//
		65	void SendMotorData(uint8_t m,uint8_t v)
		66	{
		67	if (m==0)
		68
		69	for(m=0;m<MAX_MOTORS;m++) // alle Motoren
		70	{
		71	// Motor[m].SetPoint = MotorTest[m];
		72	Motor[m].SetPoint = v;
		73	Motor[m].SetPointLowerBits = 0;
		74
		75	// Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution
		76	// Motor[i].SetPointLowerBits = MotorTest[i] % 4;
		77
		78	}
		79	else
		80	{
		81	Motor[m-1].SetPoint = v;
		82	Motor[m-1].SetPointLowerBits = 0;
		83	}
		84
		85
		86
		87	if(I2C_TransferActive)
		88	I2C_TransferActive = 0; // enable for the next time
		89	else
		90	{
		91	motor_write = 0;
		92	I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode
		93	}
		94	}
		95
		96
		97	//--------------------------------------------------------------
		98	//
		99	void Search_BL (void)
		100	{
		101	uint8_t i = 0;
		102	unsigned int timer;
		103	lcd_cls ();
		104	MotorenEin =0;
		105	MotorTest[i] = 0;
		106
		107	lcd_printp (PSTR("Suche BL-Ctrl"), 0);
		108
		109	// Check connected BL-Ctrls
		110	BLFlags \|= BLFLAG_READ_VERSION;
		111	motor_read = 0; // read the first I2C-Data
		112
		113	SendMotorData(0,0);
		114	timer = SetDelay(1);
		115	while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
		116
		117	timer = SetDelay(1);
		118	for(i=0; i < MAX_MOTORS; i++)
		119	{
		120
		121	SendMotorData(i,0);
		122
		123
		124	while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
		125
		126
		127	if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
		128	{
		129
		130	lcd_printp_at (0, 1, PSTR("Found BL-Ctrl:"), 0);
		131	lcd_print_hex_at (18,1,i,0);
		132
		133	lcd_printp_at (0, 2, PSTR("Version:"), 0);
		134	lcd_print_hex_at (8,2,Motor[i].Version,0);
		135	lcd_printp_at (11, 2, PSTR("SetPoi:"), 0);
		136	lcd_print_hex_at (18,2,Motor[i].SetPoint,0);
		137
		138	lcd_printp_at (0, 3, PSTR("SetPoiL:"), 0);
		139	lcd_print_hex_at (8,3,Motor[i].SetPointLowerBits,0);
		140	lcd_printp_at (11, 3, PSTR("State :"), 0);
		141	lcd_print_hex_at (18,3,Motor[i].State,0);
		142
		143	lcd_printp_at (0, 4, PSTR("ReadMod:"), 0);
		144	lcd_print_hex_at (8,4,Motor[i].ReadMode,0);
		145	lcd_printp_at (11, 4, PSTR("Currnt:"), 0);
		146	lcd_print_hex_at (18,4,Motor[i].Current,0);
		147
		148	lcd_printp_at (0, 5, PSTR("MaxPWM :"), 0);
		149	lcd_print_hex_at (8,5,Motor[i].MaxPWM,0);
		150	lcd_printp_at (11, 5, PSTR("Temp :"), 0);
		151	write_ndigit_number_u (18,5,Motor[i].Temperature,3,1);
		152	}
		153
		154	} //End For I
		155	}
		156
		157
		158	//--------------------------------------------------------------
		159	//
		160	void motor (uint8_t m,uint8_t v)
		161	{
		162	memset (buffer, 0, 16);
		163
		164	if(m == 0)
		165	{
		166	memset (buffer, v, 16);
		167	}
		168	else
		169	{
		170	buffer[m-1] = v;
		171	}
		172
		173	SendOutData('t', ADDRESS_FC, 1, buffer, 16);
		174	}
		175
		176	//--------------------------------------------------------------
		177	//
		178	void motor_test (uint8_t MotorMode)
		179	{
		180
		181	lcd_cls ();
		182	mmode = 1; // 1=Motor
		183	m = 1;
		184	v = 0;
		185	char buffer[7];
		186	unsigned int SerLoop;
		187	SerLoop = 10;
		188
		189	if (MotorMode == I2C_Mode)
		190	{
		191	Search_BL();
		192	do
		193	{
		194	lcd_printp_at (11, 7, PSTR("Ende Check"), 0);
		195	if (get_key_press (1 << KEY_ESC))
		196	{
		197	get_key_press(KEY_ALL);
		198	return;
		199	}
		200	}
		201	while (!get_key_press (1 << KEY_ENTER));
		202	}
		203
		204	lcd_cls();
		205	lcd_printp (PSTR(" BL-Ctrl Test "), 2);
		206	lcd_printp_at (2, 2, PSTR("Motor: 1"), 0);
		207	lcd_printp_at (2, 3, PSTR("Value: 0"), 0);
		208	lcd_frect ((81), (85), (0 * (14*8)) / 255, 6, 1);
		209	// lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0);
		210	lcd_puts_at(0, 7, strGet(KEYLINE3), 0);
		211	lcd_printp_at (18, 7, PSTR("\x1a \x1b"), 0);
		212	lcd_printp_at (0, 2, PSTR("\x1d"), 0);
		213
		214	#if defined HWVERSION1_3W \|\| defined HWVERSION1_3 \|\| defined HWVERSION3_9
		215	if (MotorMode == I2C_Mode)
		216	uart1_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r");
		217	#endif
		218	#if defined HWVERSION1_2W \|\| defined HWVERSION1_2
		219	if (MotorMode == I2C_Mode)
		220	USART_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r");
		221	#endif
		222
		223	if (MotorMode == FC_Mode)
		224	{
		225	if (hardware == NC && current_hardware == NC)
		226	{
		227	SwitchToFC();
		228	}
		229	}
		230	do
		231	{
		232	// mmode 0=Value 1=Motor
		233
		234	if ((mmode == 0) && (get_key_press (1 << KEY_PLUS) \|\| get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (v < 254))
		235	{
		236	v++;
		237	write_ndigit_number_u (9, 3, v, 3, 0);
		238	if (MotorMode == FC_Mode)
		239	lcd_frect ((81), (85), (v * (14*8)) / 255, 6, 1);
		240	}
		241
		242	if ((mmode == 0) && (get_key_press (1 << KEY_MINUS) \|\| get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (v > 0))
		243	{
		244	if (MotorMode == FC_Mode)
		245	lcd_frect (((v * (148) / 255) + 8), (85), ((14*8) / 255), 6, 0);
		246
		247	v--;
		248	write_ndigit_number_u (9, 3, v, 3, 0);
		249	if (MotorMode == FC_Mode)
		250	lcd_frect ((81), (85), (v * (14*8)) / 255, 6, 1);
		251	}
		252
		253	if ((mmode == 1) && (get_key_press (1 << KEY_PLUS) \|\| get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (m < 16))
		254	{
		255	m++;
		256	write_ndigit_number_u (9, 2, m, 3, 0);
		257	}
		258
		259	if ((mmode == 1) && (get_key_press (1 << KEY_MINUS) \|\| get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (m > 0))
		260	{
		261	m--;
		262	if(m > 0)
		263	write_ndigit_number_u (9, 2, m, 3, 0);
		264	if(m == 0)
		265	lcd_printp_at (9, 2, PSTR("All"), 0);
		266	}
		267
		268	if (get_key_press (1 << KEY_ENTER))
		269	{
		270	if (MotorMode == I2C_Mode)
		271	{
		272	if (v > 0)
		273	{
		274	m = 0;
		275	v=0;
		276	lcd_frect ((81), (85), (0 * (14*8)) / 255, 6, 1);
		277	lcd_cls_line (0, 5, 21);
		278	if(m > 0) write_ndigit_number_u (9, 2, m, 3, 0);
		279	if(m == 0) lcd_printp_at (9, 2, PSTR("All"), 0);
		280	write_ndigit_number_u (9, 3, v, 3, 0);
		281	SendMotorData(m,v);
		282	timer = SetDelay(1);
		283	while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
		284	}
		285	}
		286
		287	if(mmode == 0) // 0=Value
		288	{
		289	lcd_printp_at (0, 2, PSTR("\x1d"), 0);
		290	lcd_printp_at (0, 3, PSTR(" "), 0);
		291	mmode = 1; // 1=Motor
		292	}
		293	else
		294	{
		295	lcd_printp_at (0, 2, PSTR(" "), 0);
		296	lcd_printp_at (0, 3, PSTR("\x1d"), 0);
		297	mmode = 0; // 0=Value
		298	}
		299	}
		300	//if (get_key_press (1 << KEY_ENTER))//
		301
		302	if (MotorMode == I2C_Mode)
		303	{
		304	SendMotorData(m,v);
		305	timer = SetDelay(1);
		306	lcd_printp_at (0, 3, PSTR("SetPoint :"), 0);
		307
		308	write_ndigit_number_u (13,3,Motor[m-1].SetPoint,3,0);
		309	lcd_printp_at (0, 4, PSTR("Current :"), 0);
		310	lcd_print_hex_at (13,4,Motor[m-1].Current,0);
		311	write_ndigit_number_u (13,4,Motor[m-1].Current,3,0);
		312	lcd_printp_at (0, 5, PSTR("Temperature:"), 0);
		313	write_ndigit_number_u (13,5,Motor[m-1].Temperature,3,0);
		314	lcd_printp_at (0, 6, PSTR("Version:"), 0);
		315	lcd_print_hex_at (8,6,Motor[m-1].Version,0);
		316	lcd_printp_at (11, 6, PSTR("State :"), 0);
		317	lcd_print_hex_at (18,6,Motor[m-1].State,0);
		318
		319	#if defined HWVERSION1_3W \|\| defined HWVERSION1_3 \|\| defined HWVERSION3_9
		320
		321	if (Motor[m-1].SetPoint > 0)
		322	{
		323	if (SerLoop == 0)
		324	{
		325	itoa( m-1, buffer, 10); // convert interger into string (decimal format)
		326	uart1_puts(buffer); // and transmit string to UART
		327	uart1_puts(";");
		328	itoa( Motor[m-1].Version, buffer, 10); //
		329	uart1_puts(buffer);
		330	uart1_puts(";");
		331	itoa( Motor[m-1].SetPoint, buffer, 10); //
		332	uart1_puts(buffer);
		333	uart1_puts(";");
		334	itoa( Motor[m-1].SetPointLowerBits, buffer, 10); //
		335	uart1_puts(buffer);
		336	uart1_puts(";");
		337	itoa( Motor[m-1].State, buffer, 10); //
		338	uart1_puts(buffer);
		339	uart1_puts(";");
		340	itoa( Motor[m-1].ReadMode, buffer, 10); //
		341	uart1_puts(buffer);
		342	uart1_puts(";");
		343	itoa( Motor[m-1].Current, buffer, 10); //
		344	uart1_puts(buffer);
		345	uart1_puts(";");
		346	itoa( Motor[m-1].MaxPWM, buffer, 10); //
		347	uart1_puts(buffer);
		348	uart1_puts(";");
		349	itoa( Motor[m-1].Temperature, buffer, 10); //
		350	uart1_puts(buffer);
		351	uart1_puts("\r");
		352	uart1_puts("\n");
		353	SerLoop =200;
		354	}
		355	else
		356	SerLoop--;
		357	}
		358
		359	#endif
		360
		361	#if defined HWVERSION1_2W \|\| defined HWVERSION1_2
		362
		363	if (Motor[m-1].SetPoint > 0)
		364	{
		365	if (SerLoop == 0)
		366	{
		367	itoa( m-1, buffer, 10); // convert interger into string (decimal format)
		368	USART_puts(buffer); // and transmit string to UART
		369	USART_puts(";");
		370	itoa( Motor[m-1].Version, buffer, 10); //
		371	USART_puts(buffer);
		372	USART_puts(";");
		373	itoa( Motor[m-1].SetPoint, buffer, 10); //
		374	USART_puts(buffer);
		375	USART_puts(";");
		376	itoa( Motor[m-1].SetPointLowerBits, buffer, 10); //
		377	USART_puts(buffer);
		378	USART_puts(";");
		379	itoa( Motor[m-1].State, buffer, 10); //
		380	USART_puts(buffer);
		381	USART_puts(";");
		382	itoa( Motor[m-1].ReadMode, buffer, 10); //
		383	USART_puts(buffer);
		384	USART_puts(";");
		385	itoa( Motor[m-1].Current, buffer, 10); //
		386	USART_puts(buffer);
		387	USART_puts(";");
		388	itoa( Motor[m-1].MaxPWM, buffer, 10); //
		389	USART_puts(buffer);
		390	USART_puts(";");
		391	itoa( Motor[m-1].Temperature, buffer, 10); //
		392	USART_puts(buffer);
		393	USART_puts("\r");
		394	USART_puts("\n");
		395	SerLoop =200;
		396	}
		397	else
		398	SerLoop--;
		399	}
		400
		401	#endif
		402
		403	while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
		404	}
		405	else
		406	motor (m,v); //if (MotorMode == I2C_Mode)//
		407
		408	}
		409	while (!get_key_press (1 << KEY_ESC));
		410
		411	get_key_press(KEY_ALL);
		412
		413	if (MotorMode == FC_Mode)
		414	{
		415	motor(0,0); // switch all engines off at exit
		416	}
		417
		418	}
		419

Subversion Repositories Projects

(root)/Transportables_Koptertool/branch/GPL_PKT_V3_5_8a_FC086/motortest.c @ 1497 - Rev 1471