WebSVN - Projects - Blame - /Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/servo

Rev	Author	Line No.	Line
2136	-	1
		2	/****************************************************************/
		3	/* */
		4	/* NG-Video 5,8GHz */
		5	/* */
		6	/* Copyright (C) 2011 - gebad */
		7	/* */
		8	/* This code is distributed under the GNU Public License */
		9	/* which can be found at http://www.gnu.org/licenses/gpl.txt */
		10	/* */
		11	/****************************************************************/
		12
		13	//############################################################################
		14	//# HISTORY servo_setup.c
		15	//#
		16	//# 27.06.2014 OG
		17	//# - del: #include "tools.h"
		18	//# - del: #include "mymath.h"
		19	//#
		20	//# 30.05.2014 OG
		21	//# - chg: etliche Edit-Anzeigen/Funktionen auf neues Layout umgeschrieben;
		22	//# einige Optimierungen an Aufruffunktionen (unnoetige Menue-
		23	//# Interfacefunktionen entfernt)
		24	//#
		25	//# 13.05.2014 OG
		26	//# - chg: Menu_Test_Start() - Variable 'range' auskommentiert
		27	//# wegen Warning: variable set but not used
		28	//# - chg: Displ_PulseWidth() - Variable 'me' auskommentiert
		29	//# wegen Warning: variable set but not used
		30	//# - chg: Change_Value_plmi() - Variable 'tmp_val' entfernt
		31	//# wegen Warning: variable set but not used
		32	//#
		33	//# 11.05.2014 OG
		34	//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem()
		35	//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt
		36	//#
		37	//# 30.03.2014 OG
		38	//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war
		39	//# - chg: Sprache Hollaendisch vollstaendig entfernt
		40	//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P()
		41	//#
		42	//# 25.08.2013 Cebra
		43	//# - add: #ifdef USE_TRACKING
		44	//#
		45	//# 30.05.2013 cebra
		46	//# - chg: doppelte Texte auf #define umgestellt
		47	//#
		48	//# 24.05.2013 OG
		49	//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P()
		50	//#
		51	//# 22.05.2013 OG
		52	//# - umgestellt auf menuctrl: Setup_ServoTracking(), Setup_ServoTest(),
		53	//# Setup_ServoTestCont(), Setup_ServoAdjust()
		54	//# - del: include utils/menuold.h
		55	//#
		56	//# 17.05.2013 OG
		57	//# - chg: umgestellt auf utils/menuold.h
		58	//#
		59	//# 16.04.2013 Cebra
		60	//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated
		61	//#
		62	//# 04.04.2013 Cebra
		63	//# - chg: Texttuning
		64	//#
		65	//# 03.04.2013 Cebra
		66	//# - chg: Holl�ndische Men�texte
		67	//#
		68	//# 27.03.2013 Cebra
		69	//# - chg: Fehler bei der Men�steuerung behoben
		70	//############################################################################
		71
		72	#include <stdio.h>
		73	#include <stdlib.h>
		74	#include <string.h>
		75	#include <math.h>
		76	#include "../cpu.h"
		77	#include <util/delay.h>
		78	#include <avr/pgmspace.h>
		79	#include <avr/interrupt.h>
		80	#include "../main.h"
		81	#include "../timer/timer.h"
		82	#include "servo_setup.h"
		83	#include "tracking.h"
		84	#include "../lcd/lcd.h"
		85	#include "../timer/timer.h"
		86	#include "../menu.h"
		87	#include "../messages.h"
		88	#include "../mk-data-structs.h"
		89	#include "../uart/usart.h"
		90	#include "../osd/osd.h"
		91	#include "../eeprom/eeprom.h"
		92	#include "../setup/setup.h"
		93	#include "../utils/menuctrl.h"
		94	#include "../pkt/pkt.h"
		95
		96
		97	#ifdef USE_TRACKING
		98
		99
		100	//#include "ng_usart.h"
		101	//#include "ng_config.h"
		102	//#include "servo.h"
		103	//#include "tools.h"
		104	//#include "mk.h"
		105	//#include "keys.h"
		106	//#include "mymath.h"
		107	//
		108
		109	//GPS_Pos_t last5pos[7];
		110	//uint8_t error1 = 0;
		111	//NaviData_t *naviData;
		112	//HomePos_t MK_pos; // Home position of station
		113	//GPS_Pos_t currentPos; // Current position of flying object
		114	//int8_t satsInUse; // Number of satelites currently in use
		115
		116	//uint8_t tracking = TRACKING_MIN;
		117	//uint8_t track_hyst = TRACKING_HYSTERESE;
		118	//uint8_t track_tx = 0;
		119
		120	//geo_t geo;
		121	//int16_t anglePan, angleTilt; // Servo Winkel
		122	//uint8_t coldstart = 1;
		123
		124	uint8_t servo_nr; // zwischen Servo 1 und 2 wird nur mit global servo_nr unterschieden
		125	//uint8_t FCStatusFlags;
		126
		127
		128	//############################################################################
		129
		130	//-----------------------------
		131	// Setup_ServoTracking()
		132	//-----------------------------
		133	#define MSERVO_STEPS 1
		134	#define MSERVO_SERVO1 2
		135	#define MSERVO_SERVO2 3
		136	#define MSERVO_SERVOTEST 4
		137
		138	static const char MSERVO_STEPS_de[] PROGMEM = "Servoschritte";
		139	static const char MSERVO_STEPS_en[] PROGMEM = "servo steps";
		140
		141	static const char MSERVO_SERVO1_de[] PROGMEM = "Servo 1";
		142	#define MSERVO_SERVO1_en MSERVO_SERVO1_de
		143
		144	static const char MSERVO_SERVO2_de[] PROGMEM = "Servo 2";
		145	#define MSERVO_SERVO2_en MSERVO_SERVO2_de
		146
		147	static const char MSERVO_SERVOTEST_de[] PROGMEM = "Servotest";
		148	#define MSERVO_SERVOTEST_en MSERVO_SERVOTEST_de
		149
		150
		151	//############################################################################
		152
		153	//-----------------------------
		154	// Setup_ServoTest()
		155	//-----------------------------
		156	#define MSERVOT_PULS 1
		157	#define MSERVOT_CONT 2
		158	#define MSERVOT_SERVO 3
		159	#define MSERVOT_FRAME 4
		160
		161	static const char MSERVOT_PULS_de[] PROGMEM = "Test Pulsl�nge";
		162	static const char MSERVOT_PULS_en[] PROGMEM = "test puls width";
		163
		164	static const char MSERVOT_CONT_de[] PROGMEM = "Test Fortlfd.";
		165	static const char MSERVOT_CONT_en[] PROGMEM = "test cont.";
		166
		167	static const char MSERVOT_SERVO_de[] PROGMEM = "Servo";
		168	#define MSERVOT_SERVO_en MSERVOT_SERVO_de
		169
		170	static const char MSERVOT_FRAME_de[] PROGMEM = "Periode";
		171	static const char MSERVOT_FRAME_en[] PROGMEM = "frame";
		172
		173	//############################################################################
		174
		175	//-----------------------------
		176	// Setup_ServoTestCont()
		177	//-----------------------------
		178	#define MSERVOTC_START 1
		179	#define MSERVOTC_SINGLE 2
		180	#define MSERVOTC_COUNT 3
		181	#define MSERVOTC_PAUSEEND 4
		182	#define MSERVOTC_PAUSEINC 5
		183
		184	static const char MSERVOTC_START_de[] PROGMEM = "Start Test";
		185	static const char MSERVOTC_START_en[] PROGMEM = "start test";
		186
		187	static const char MSERVOTC_SINGLE_de[] PROGMEM = "Einzelschritt";
		188	static const char MSERVOTC_SINGLE_en[] PROGMEM = "single step";
		189
		190	static const char MSERVOTC_COUNT_de[] PROGMEM = "Anzahl Test";
		191	static const char MSERVOTC_COUNT_en[] PROGMEM = "number of test";
		192
		193	static const char MSERVOTC_PAUSEEND_de[] PROGMEM = "Pause Endpos.";
		194	static const char MSERVOTC_PAUSEEND_en[] PROGMEM = "pause end pos";
		195
		196	static const char MSERVOTC_PAUSEINC_de[] PROGMEM = "Pause pro Inc.";
		197	static const char MSERVOTC_PAUSEINC_en[] PROGMEM = "pause proc inc.";
		198
		199	//############################################################################
		200
		201	//-----------------------------
		202	// Setup_ServoAdjust()
		203	//-----------------------------
		204	#define MSERVOADJ_REV 1
		205	#define MSERVOADJ_LEFT 2
		206	#define MSERVOADJ_RIGHT 3
		207	#define MSERVOADJ_MID 4
		208
		209	static const char MSERVOADJ_REV_de[] PROGMEM = "Reverse";
		210	static const char MSERVOADJ_REV_en[] PROGMEM = "reverse";
		211
		212	static const char MSERVOADJ_LEFT_de[] PROGMEM = "Links";
		213	static const char MSERVOADJ_LEFT_en[] PROGMEM = "left";
		214
		215	static const char MSERVOADJ_RIGHT_de[] PROGMEM = "Rechts";
		216	static const char MSERVOADJ_RIGHT_en[] PROGMEM = "right";
		217
		218	static const char MSERVOADJ_MID_de[] PROGMEM = "Mitte";
		219	static const char MSERVOADJ_MID_en[] PROGMEM = "middle";
		220
		221	//############################################################################
		222
		223
		224
		225	//--------------------------------------------------------------
		226	//--------------------------------------------------------------
		227	void draw_input_screen( void )
		228	{
		229	lcd_cls ();
		230	PKT_TitleFromMenuItem( true );
		231	lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM)
		232	lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL);
		233	}
		234
		235
		236
		237	///************************************************************************************/
		238	///* */
		239	///* �ndern der Werte mit Tasten +,- und Anzeige */
		240	///* z.B. f�r U-Offset, Batterie leer Eingabe ... */
		241	///* */
		242	///* Parameter: */
		243	///* uint16_t val :zu �ndernter Wert */
		244	///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */
		245	///* uint8_t posX, posY :Darstellung Wert xPos, YPos auf LCD */
		246	///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */
		247	///* uint8_t cycle :0 begrenzt Anzeige bei man_val, bzw. max_val */
		248	///* :1 springt nach max_val auf min_val und umgedreht */
		249	///* uint8_t vrepeat :beschleunigte Repeat-Funktion aus/ein */
		250	///* uint16_t Change_Value_plmi(...) :R�ckgabe ge�nderter Wert */
		251	///* */
		252	///************************************************************************************/
		253
		254	void Servo_tmp_Original(uint8_t track)
		255	{
		256	servoSetDefaultPos();
		257	// tracking = track; // urspr�nglicher Wert Tracking aus, RSSI oder GPS
		258	// NoTracking_ServosOff(); // Servos sind nur zum Tracking oder bei Kalibrierung eingeschaltet
		259	// Jump_Menu(pmenu);
		260	}
		261
		262	uint8_t Servo_tmp_on(uint8_t servo_period)
		263	{
		264	// uint8_t tmp_tracking = tracking;
		265
		266	// tracking = 0; // Servopositionierung durch tracking abschalten
		267	// if (tracking == TRACKING_MIN) servoInit(servo_period); // falls aus, Servos einschalten
		268	servoInit(servo_period);
		269	// lcdGotoXY(0, 0); // lcd Cursor vorpositionieren
		270	// return(tmp_tracking);
		271	return (0);
		272	}
		273
		274
		275	void Displ_Off_On(uint16_t val)
		276	{
		277	if (val == 0) lcd_printp_at(17, 2, strGet(OFF), 0); else lcd_printp_at(17, 2, strGet(ON), 0);
		278	}
		279
		280
		281
		282	uint16_t Change_Value_plmi(uint16_t val, uint16_t min_val, uint16_t max_val, uint8_t posX, uint8_t posY,Displ_Fnct_t Displ_Fnct)
		283	{
		284	// >> Menueauswahl nach oben
		285
		286	if (get_key_press (1 << KEY_PLUS) \|\| get_key_long_rpt_sp ((1 << KEY_PLUS), 3))
		287	{
		288	if (val < max_val) {
		289	edit = 1;
		290	val++;
		291
		292	}
		293	else
		294	{
		295	val = min_val;
		296	}
		297	Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt
		298
		299
		300	}
		301	// >> Menueauswahl nach unten
		302
		303	if (get_key_press (1 << KEY_MINUS) \|\| get_key_long_rpt_sp ((1 << KEY_MINUS), 3))
		304	{
		305	if (val > min_val) {
		306	val--;
		307	edit = 1;
		308
		309	}
		310	else
		311	{
		312	val = max_val;
		313	}
		314	Displ_Fnct(val); // ge�nderten Wert darstellen, je nach Men�punkt
		315
		316
		317	}
		318
		319	return(val);
		320	}
		321	//
		322	///************************************************************************************/
		323	///* */
		324	///* �ndern der Werte mit Tasten +,- repetierend; (long)Entertaste und Anzeige */
		325	///* z.B. f�r U-Offset, Batterie leer Eingabe ... */
		326	///* */
		327	///* Parameter: */
		328	///* uint16_t val :zu �ndernter Wert /
		329	///* uint16_t min_val, max_val :min, max Grenze Wert �ndern darf */
		330	///* uint8_t fl_pos :Bit 7 beschleunigte Repeat-Funktion aus/ein */
		331	///* Bit 6 zyklische Werte�nderung aus/ein */
		332	///* Bit 4-5 z.Z. ohne Funktion */
		333	///* Bit 0-3 Wert xPos auf LCD */
		334	///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */
		335	///* uint8_t Change_Value(...) :R�ckgabe ge�ndert ergibt TRUE */
		336	///* */
		337	///************************************************************************************/
		338	//// Bei Bedarf k�nnte einfach innerhalp fl_pos auch noch pos_y (Bit 4-5) �bergeben werden
		339	uint8_t Change_Value(uint16_t *val, uint16_t min_val, uint16_t max_val,Displ_Fnct_t Displ_Fnct)
		340	{
		341	uint16_t tmp_val;
		342
		343	tmp_val = *val;
		344	Displ_Fnct(tmp_val); // initiale Wertdarstellung, je nach Men�punkt
		345	while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC))
		346	val = Change_Value_plmi(val, min_val, max_val, 16,2, Displ_Fnct);
		347
		348
		349	if (*val == tmp_val) {
		350	edit = 0;
		351	// lcd_printp_at (0, 5, PSTR("Edit=0"), 0);
		352	// _delay_ms(500);
		353	//// return (*val);
		354	}
		355	//
		356	else
		357	{
		358	edit = 1;
		359	// lcd_printp_at (0, 5, PSTR("Edit=1"), 0);
		360	// _delay_ms(500);
		361	}
		362
		363	return (tmp_val != *val);
		364
		365
		366	}
		367
		368	uint16_t calc_range(int16_t PosProzent, int16_t min, int16_t max, int16_t mid)
		369	{ uint16_t range;
		370
		371	if (PosProzent < 0) {
		372	range = mid - min;
		373	// if (chrxs == CHRRS) { // falls Richtung ge�ndert, anderen Zeichensatz laden
		374	// chrxs = CHRLS;
		375	//// lcdWriteCGRAM_Array(lcdSpecialChrLs, 5);// LCD-Char mit Rahmensymbole vom Graph
		376	// }
		377	}
		378	else {
		379	range = max - mid;
		380	// if (chrxs == CHRLS) { // falls Richtung ge�ndert, anderen Zeichensatz laden
		381	//// lcdWriteCGRAM_Array(lcdSpecialChrRs, 5);// LCD-Char mit Rahmensymbole vom Graph
		382	// chrxs = CHRRS;
		383	// }
		384	}
		385	return(range);
		386	}
		387
		388
		389
		390	/************************************************************************************/
		391	/* zeigt einen max. 3-stelligen Integerwert auf Display an */
		392	/* Parameter: */
		393	/* uint16_t val :anzuzeigender Wert, */
		394	/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
		395	/* */
		396	/************************************************************************************/
		397	void Displ_Format_Int( uint16_t val )
		398	{
		399	lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), val );
		400	}
		401
		402
		403
		404	//--------------------------------------------------------------
		405	//--------------------------------------------------------------
		406	void Displ_PulseWidth( uint16_t val )
		407	{
		408	int16_t PosProzent, range;
		409
		410	servoSetPositionRaw( servo_nr, val);
		411
		412	PosProzent = val - steps_pw[Config.sIdxSteps].mid;
		413	range = calc_range(PosProzent, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, steps_pw[Config.sIdxSteps].mid);
		414	PosProzent = (int32_t)1000 * PosProzent / range;
		415
		416	lcdx_printf_at_P( 15, 2, MNORMAL, 0,0, PSTR("%4.1d"), PosProzent );
		417	}
		418
		419
		420
		421	/************************************************************************************/
		422	/* zeigt Pausenl�nge der Links-, Mittel- und Rechtsposition auf Display an */
		423	/* Parameter: */
		424	/* uint16_t val : Zeit in 1ms * 100 */
		425	/* */
		426	/************************************************************************************/
		427	void Displ_Pause(uint16_t val)
		428	{
		429	lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%1.1d"), val );
		430	//lcdPuts("s ");
		431	}
		432
		433
		434
		435	/************************************************************************************/
		436	/* zeigt aus oder Integerwert auf Display an */
		437	/* Parameter: */
		438	/* uint16_t val : val = 0 ==> aus, sont Integerwert */
		439	/* */
		440	/************************************************************************************/
		441	void Displ_Off_Format_Int( uint16_t val )
		442	{
		443	if( val == 0 )
		444	{
		445	lcd_printp_at( 17, 2, strGet(OFF), MNORMAL);
		446	}
		447	else
		448	{
		449	Displ_Format_Int( val );
		450	}
		451	}
		452
		453
		454
		455	/************************************************************************************/
		456	/* zeigt aus oder Pausenzeit zwischen 2 Servoschritte auf Display an */
		457	/* Parameter: */
		458	/* uint16_t val : val = 0 ==> aus, sont Integerwert */
		459	/* */
		460	/************************************************************************************/
		461	void Displ_Pause_Step( uint16_t val )
		462	{
		463	Displ_Off_Format_Int( val );
		464	}
		465
		466
		467
		468	/************************************************************************************/
		469	/* zeigt zu testende Servonummer zur Auswahl auf Display an */
		470	/* Parameter: */
		471	/* uint16_t val :0 = Servo 1 oder 1 = Servo 2, */
		472	/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
		473	/* */
		474	/************************************************************************************/
		475	void Displ_ServoNr( uint16_t val )
		476	{
		477	Displ_Format_Int( val+1 );
		478	}
		479
		480
		481
		482	/**************************/
		483	/* */
		484	/* Servos-Tests */
		485	/* */
		486	/**************************/
		487	//void Menu_Servo_Test(void)
		488	//{ uint8_t scr_sub_menu[SCROLL_MAX_6] = {SCROLL_MAX_6 - 2, MSG_RETURN, MSG_PULSE_WIDTH, MSG_CONTINOUS, MSG_SERVO, MSG_FRAME};
		489	//
		490	// Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global
		491	// servo_nr = eeprom_read_byte(&ep_servo_nr);
		492	// Jump_Menu(pmenu);
		493	//}
		494
		495
		496
		497	//--------------------------------------------------------------
		498	//--------------------------------------------------------------
		499	void Menu_Test_Frame( void )
		500	{
		501	uint16_t tmp_val;
		502
		503	draw_input_screen();
		504
		505	tmp_val = Config.servo_frame;
		506	if( Change_Value( &tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX, Displ_Format_Int) )
		507	{
		508	Config.servo_frame = tmp_val;
		509	}
		510	}
		511
		512
		513
		514	//--------------------------------------------------------------
		515	//--------------------------------------------------------------
		516	void Menu_Test_ServoNr( void )
		517	{
		518	uint16_t tmp_val;
		519
		520	draw_input_screen();
		521
		522	tmp_val = servo_nr;
		523
		524	if( Change_Value( &tmp_val, 0, 1, Displ_ServoNr) )
		525	{ // pmenu global; es gibt nur 0=Servo1, 1=Servo2
		526	servo_nr = tmp_val;
		527	}
		528	}
		529
		530
		531
		532	//--------------------------------------------------------------
		533	// Dieser Test im raw-Modus ohne Anschlagkalibrierung (normiert) z.B.: f�r Modelleinstellungen ohne Empf�nger
		534	//--------------------------------------------------------------
		535	void Menu_Test_PulseWidth( void )
		536	{
		537	uint8_t tmp_tracking;
		538	uint16_t tmp_val;
		539
		540	draw_input_screen();
		541
		542	// 30.05.2014 OG: macht dieser Code Sinn?
		543	// Change_Value() -> 'tmp_val' und 'tmp_val' ist local - was soll mir das sagen?
		544
		545	tmp_tracking = Servo_tmp_on(Config.servo_frame);
		546	tmp_val = steps_pw[Config.sIdxSteps].mid;
		547	Change_Value( &tmp_val, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, Displ_PulseWidth); // pmenu global
		548	cli();
		549	servoInit( SERVO_PERIODE );
		550	sei();
		551	Servo_tmp_Original(tmp_tracking);
		552	}
		553
		554
		555
		556	//--------------------------------------------------------------
		557	//--------------------------------------------------------------
		558	void Menu_Test_SingleStep(void)
		559	{
		560	uint16_t tmp_val;
		561
		562	draw_input_screen();
		563
		564	tmp_val = Config.single_step;
		565
		566	if( Change_Value( &tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int) )
		567	{ // pmenu global
		568	Config.single_step = tmp_val;
		569	}
		570	}
		571
		572
		573
		574	//--------------------------------------------------------------
		575	//--------------------------------------------------------------
		576	void Menu_Test_Repeat( void )
		577	{
		578	uint16_t tmp_val;
		579
		580	draw_input_screen();
		581
		582	tmp_val = Config.repeat;
		583
		584	if( Change_Value( &tmp_val, REPEAT_MIN, REPEAT_MAX, Displ_Format_Int) )
		585	{
		586	Config.repeat = tmp_val;
		587	}
		588	}
		589
		590
		591
		592	//--------------------------------------------------------------
		593	//--------------------------------------------------------------
		594	void Menu_Test_Pause( void )
		595	{
		596	uint16_t tmp_val;
		597
		598	draw_input_screen();
		599
		600	tmp_val = Config.pause;
		601
		602	if( Change_Value( &tmp_val, PAUSE_MIN, PAUSE_MAX, Displ_Pause) )
		603	{
		604	Config.pause = tmp_val;
		605	}
		606	}
		607
		608
		609
		610	//--------------------------------------------------------------
		611	//--------------------------------------------------------------
		612	void Menu_Test_Pause_Step( void )
		613	{
		614	uint16_t tmp_val;
		615
		616	draw_input_screen();
		617
		618	tmp_val = Config.pause_step;
		619
		620	if( Change_Value( &tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX, Displ_Pause_Step) )
		621	{
		622	Config.pause_step = tmp_val;
		623	}
		624	}
		625
		626
		627
		628	int8_t calc_dir(uint8_t idx, int16_t *Position)
		629	{ uint8_t nextIdx;
		630	int8_t nextDir = 1;
		631
		632	nextIdx = idx;
		633	if ((idx + 1) < POSIDX_MAX)
		634	nextIdx++;
		635	else
		636	nextIdx = 0;
		637	if (Position[PosIdx[idx]] > Position[PosIdx[nextIdx]]) nextDir = -1;
		638	return(nextDir);
		639	}
		640
		641
		642
		643	void Displ_LoopCounter( uint8_t val )
		644	{
		645	lcdx_printf_at_P( 16, 2, MNORMAL, 0,0, PSTR("%4d"), val );
		646	//lcd_puts_at(16,2,my_itoa(val, 4, 0, 0),0);
		647	}
		648
		649
		650
		651
		652	// Test �ber Scalierung der Servos mit Anschlagkalibrierung
		653	void Menu_Test_Start(void)
		654	{
		655	uint8_t tmp_tracking, idx, rep;
		656	int8_t dir;
		657	int16_t sPos;
		658	int16_t Position[3];
		659	//int16_t range;
		660
		661	tmp_tracking = Servo_tmp_on(Config.servo_frame);
		662	// lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph
		663	// chrxs = CHRLS; // Flag, welche K�stchensymbole geladen
		664	// Displ_Title(MSG_CONTINOUS);
		665
		666	lcd_cls ();
		667	PKT_TitleFromMenuItem( true );
		668	lcd_printp_at(0, 7, strGet(KEYLINE2), 0);
		669
		670	Displ_LoopCounter(Config.repeat);
		671	Position[0] = 0; // skalierte Servoposition aber unterschiedliche Schrittanzahl m�glich
		672	Position[1] = ServoSteps()/2;
		673	Position[2] = ServoSteps();
		674	// init Einzelschritt
		675	idx = 0;
		676	dir = calc_dir(idx, Position);
		677	sPos = Position[PosIdx[idx]];
		678	idx++;
		679	rep = Config.repeat;
		680
		681	// Test bis Ende der Wiederholungen oder irgendein Enter
		682	while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) {
		683	calc_range(sPos - Position[1], Position[0], Position[2], Position[1]);
		684	//range = calc_range(sPos - Position[1], Position[0], Position[2], Position[1]);
		685	// draw_bar(sPos - Position[1], range, 1); // eingerahmter Balkengraph auf 2. Display-Zeile
		686	servoSetPosition(servo_nr, sPos);
		687
		688	if ( sPos != Position[PosIdx[idx]]) { // Links-, Mittel- oder Rechtsposotion erreicht?
		689	sPos += (Config.single_step * dir); // variable Schrittweite subtrahieren oder addieren
		690	if (((dir < 0) && (sPos < Position[PosIdx[idx]])) \|\| ((dir > 0) && (sPos > Position[PosIdx[idx]])) \|\| !(Config.single_step))
		691	sPos = Position[PosIdx[idx]]; // �berlauf bei variabler Schrittweite ber�cksichtigen oder Einzelschritt
		692	Delay_ms(Config.servo_frame + 1 + Config.pause_step);// Bei Schrittweite um 1 w�rden welche �bersprungen, zus�tzlich pro Servoschritt verz�gert
		693	}
		694	else {
		695	dir = calc_dir(idx, Position); // Richtungs�nderung
		696	if (idx < (POSIDX_MAX - 1)) {
		697	if (idx == 0) {
		698	rep--; // bei jeden vollen Durchlauf Wiederholz�hler verringern
		699	Displ_LoopCounter(rep);
		700	}
		701	idx++; // Index f�r n�chsten Positionswert ==> Array PosIdx[] bestimmt Anschlagreihenfolge
		702	}
		703	else
		704	idx = 0;
		705
		706	_delay_ms( Config.pause*100 ); // variable Pause bei Links-, Mittel- und Rechtsposotion Mindestzeit 400ms (Servolauf)
		707	}
		708	}
		709
		710	// lcdClear();
		711	// if (pmenu[0] == '\0')
		712	// Displ_Main_Disp();
		713	// else
		714	// return_m_pkt(strlen(pmenu)); // um bei R�cksprung auf urspr�nglichen Men�punkt zeigen oder Displ_Main_Disp()
		715	// lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char f�r Bargraph zur�ckschreiben
		716	cli();
		717	servoInit(SERVO_PERIODE);
		718	sei();
		719	Servo_tmp_Original(tmp_tracking);
		720	}
		721
		722
		723
		724
		725
		726	/********************************************************************************/
		727	/* zeigt Servo-Anschlagposition links auf Display an */
		728	/* mit sofortiger Wirkung auf Servo */
		729	/* Parameter: */
		730	/* uint16_t val :anzuzeigender Wert, */
		731	/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
		732	/* */
		733	/********************************************************************************/
		734	void Displ_Servo_Min(uint16_t val)
		735	{
		736	uint16_t steps = 0;
		737
		738	write_ndigit_number_s( 15, 2, val, 5, 0,0);
		739	servoSet_min(servo_nr, val); // Wert setzen damit nachfolgend die
		740	if (Config.servo[servo_nr].rev) steps = ServoSteps();
		741	servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist
		742	}
		743
		744
		745	/************************************************************************************/
		746	/* zeigt Servo-Anschlagposition rechts auf Display an */
		747	/* mit sofortiger Wirkung auf Servo */
		748	/* Parameter: */
		749	/* uint16_t val :anzuzeigender Wert, */
		750	/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
		751	/* */
		752	/************************************************************************************/
		753	void Displ_Servo_Max(uint16_t val)
		754	{
		755	uint16_t steps = ServoSteps();
		756
		757	write_ndigit_number_u( 15, 2, val, 5, 0,0);
		758	servoSet_max(servo_nr, val); // Wert setzen damit nachfolgend die
		759	if (Config.servo[servo_nr].rev) steps = 0;
		760	servoSetPosition(servo_nr, steps); // �nderung direkt am Servo sichtbar ist
		761	}
		762
		763
		764
		765	/************************************************************************************/
		766	/* zeigt Servo-Anschlagposition Mitte auf Display an */
		767	/* mit sofortiger Wirkung auf Servo */
		768	/* Parameter: */
		769	/* uint16_t val :anzuzeigender Wert, */
		770	/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
		771	/* */
		772	/************************************************************************************/
		773	void Displ_Servo_Mid( uint16_t val )
		774	{
		775	int16_t mid_val;
		776
		777	mid_val = val - ServoSteps()/2;
		778
		779	lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), mid_val );
		780
		781	servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die
		782	servoSetPosition(servo_nr, ServoSteps()/2); // �nderung direkt am Servo sichtbar ist
		783	}
		784
		785
		786
		787	//--------------------------------------------------------------
		788	//--------------------------------------------------------------
		789	void Servo_rev( void )
		790	{
		791	uint16_t tmp_val;
		792	uint8_t tmp_tracking;
		793
		794	draw_input_screen();
		795
		796	tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
		797	tmp_val = Config.servo[servo_nr].rev;
		798
		799	if( Change_Value(&tmp_val , 0, 1, Displ_Off_On) )
		800	{ //reverse gibt es nur 0=off, 1=on
		801	Config.servo[servo_nr].rev = tmp_val ;
		802	servoSet_rev(servo_nr, tmp_val );
		803	}
		804	Servo_tmp_Original(tmp_tracking);
		805	}
		806
		807
		808
		809	//--------------------------------------------------------------
		810	//--------------------------------------------------------------
		811	void Servo_left( void )
		812	{
		813	uint16_t tmp_val;
		814	uint8_t tmp_tracking;
		815
		816	draw_input_screen();
		817
		818	tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
		819	servoSetPosition(servo_nr, ServoSteps()); // Linkssanschlag um Kalibrierung am Servo zu sehen
		820	tmp_val = Config.servo[servo_nr].max;
		821
		822	if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max) )
		823	{
		824	Config.servo[servo_nr].max = tmp_val;
		825	servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben
		826	}
		827	Servo_tmp_Original(tmp_tracking);
		828	}
		829
		830
		831
		832	//--------------------------------------------------------------
		833	//--------------------------------------------------------------
		834	void Servo_right( void )
		835	{
		836	uint16_t tmp_val;
		837	uint8_t tmp_tracking;
		838
		839	draw_input_screen();
		840
		841	tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
		842	servoSetPosition(servo_nr, 0); // Rechtsanschlag um Kalibrierung am Servo zu sehen
		843	tmp_val = Config.servo[servo_nr].min;
		844
		845	if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min) )
		846	{
		847	Config.servo[servo_nr].min = tmp_val;
		848	servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben
		849	}
		850	Servo_tmp_Original(tmp_tracking);
		851	}
		852
		853
		854
		855	//--------------------------------------------------------------
		856	//--------------------------------------------------------------
		857	void Servo_middle( void )
		858	{
		859	uint16_t tmp_val;
		860	uint8_t tmp_tracking;
		861
		862	draw_input_screen();
		863
		864	tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
		865	servoSetPosition(servo_nr, ServoSteps()/2); // Mittelposition um Kalibrierung am Servo zu sehen
		866	tmp_val = Config.servo[servo_nr].mid;
		867
		868	if( Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid) )
		869	{
		870	Config.servo[servo_nr].mid = tmp_val;
		871	}
		872	Servo_tmp_Original(tmp_tracking);
		873	}
		874
		875
		876
		877	void Servo_NewValues(uint8_t idx_presc)
		878	{
		879	for (uint8_t i = 0; i < SERVO_NUM_CHANNELS; i++) {
		880	if (idx_presc == STEPS_255) { // Werte umrechnen f�r Prescaler = 256
		881	Config.servo[i].min /= 4;
		882	Config.servo[i].max /= 4;
		883	Config.servo[i].mid /= 4;
		884	}
		885	else { // Werte umrechnen f�r Prescaler = 64
		886	Config.servo[i].min *= 4;
		887	Config.servo[i].max *= 4;
		888	Config.servo[i].mid = (Config.servo[i].mid + 1) * 4 - 1;
		889	}
		890	servoSet_min(i, Config.servo[i].min);
		891	servoSet_max(i, Config.servo[i].max);
		892	servoSet_mid(i, Config.servo[i].mid);
		893	// eeprom_write_block(&servo[i],&ep_servo[i],sizeof(servo_t));
		894	}
		895	// Vorberechnung von ServoChannels[channel].duty
		896	servoSetDefaultPos(); // Ausgangsstellung beider Servos
		897
		898	}
		899
		900	/************************************************************************************/
		901	/* zeigt Servoschritte zur Auswahl auf Display an */
		902	/* Parameter: */
		903	/* uint16_t val :0 = 255 oder 1 = 1023, */
		904	/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
		905	/* */
		906	/************************************************************************************/
		907	void Displ_Servo_Steps(uint16_t val)
		908	{
		909	if (val==0)
		910	lcd_puts_at(16,2,INTERNAT_STEPS_255,0 );
		911	else
		912	lcd_puts_at(16,2,INTERNAT_STEPS_1023,0 );
		913
		914	}
		915
		916
		917
		918	//--------------------------------------------------------------
		919	//--------------------------------------------------------------
		920	void Menu_Servo_Steps(void)
		921	{
		922	uint16_t tmp_val;
		923
		924	draw_input_screen();
		925
		926	tmp_val = Config.sIdxSteps;
		927
		928	if( Change_Value( &tmp_val, STEPS_255, STEPS_1023,Displ_Servo_Steps) )
		929	{
		930	cli();
		931	Config.sIdxSteps = tmp_val;
		932	Servo_NewValues( Config.sIdxSteps ); // hier ist der neue Index anzugeben!
		933	servoInit(SERVO_PERIODE);
		934	sei();
		935
		936
		937	}
		938	}
		939
		940
		941
		942	//--------------------------------------------------------------
		943	// Setup_ServoAdjust()
		944	//--------------------------------------------------------------
		945	void Setup_ServoAdjust( uint8_t servo )
		946	{
		947	servo_nr = servo;
		948
		949	//---------------
		950	// Create
		951	//---------------
		952	MenuCtrl_Create();
		953	MenuCtrl_SetTitleFromParentItem(); // "Servo 1" oder "Servo 1"
		954	//MenuCtrl_SetTitle_P( PSTR("Servo Adjust") );
		955
		956
		957	//---------------
		958	// Menuitems
		959	//---------------
		960	MenuCtrl_PushML2_P( MSERVOADJ_REV , MENU_ITEM, &Servo_rev , MSERVOADJ_REV_de , MSERVOADJ_REV_en );
		961	MenuCtrl_PushML2_P( MSERVOADJ_LEFT , MENU_ITEM, &Servo_left , MSERVOADJ_LEFT_de , MSERVOADJ_LEFT_en );
		962	MenuCtrl_PushML2_P( MSERVOADJ_RIGHT , MENU_ITEM, &Servo_right , MSERVOADJ_RIGHT_de , MSERVOADJ_RIGHT_en );
		963	MenuCtrl_PushML2_P( MSERVOADJ_MID , MENU_ITEM, &Servo_middle , MSERVOADJ_MID_de , MSERVOADJ_MID_en );
		964
		965	//---------------
		966	// Control
		967	//---------------
		968	MenuCtrl_Control( MENUCTRL_EVENT );
		969
		970
		971	//---------------
		972	// Destroy
		973	//---------------
		974	MenuCtrl_Destroy();
		975	}
		976
		977
		978
		979	//--------------------------------------------------------------
		980	// Setup_ServoTestCont()
		981	//--------------------------------------------------------------
		982	void Setup_ServoTestCont( void )
		983	{
		984	//---------------
		985	// Create
		986	//---------------
		987	MenuCtrl_Create();
		988	MenuCtrl_SetTitleFromParentItem(); // "Test fortlfd."
		989	//MenuCtrl_SetTitle_P( PSTR("Servotest Cont.") );
		990
		991	//---------------
		992	// Menuitems
		993	//---------------
		994	MenuCtrl_PushML2_P( MSERVOTC_START , MENU_ITEM, &Menu_Test_Start , MSERVOTC_START_de , MSERVOTC_START_en );
		995	MenuCtrl_PushML2_P( MSERVOTC_SINGLE , MENU_ITEM, &Menu_Test_SingleStep , MSERVOTC_SINGLE_de , MSERVOTC_SINGLE_en );
		996	MenuCtrl_PushML2_P( MSERVOTC_COUNT , MENU_ITEM, &Menu_Test_Repeat , MSERVOTC_COUNT_de , MSERVOTC_COUNT_en );
		997	MenuCtrl_PushML2_P( MSERVOTC_PAUSEEND , MENU_ITEM, &Menu_Test_Pause , MSERVOTC_PAUSEEND_de , MSERVOTC_PAUSEEND_en );
		998	MenuCtrl_PushML2_P( MSERVOTC_PAUSEINC , MENU_ITEM, &Menu_Test_Pause_Step , MSERVOTC_PAUSEINC_de , MSERVOTC_PAUSEINC_en );
		999
		1000	//---------------
		1001	// Control
		1002	//---------------
		1003	MenuCtrl_Control( MENUCTRL_EVENT );
		1004
		1005	//---------------
		1006	// Destroy
		1007	//---------------
		1008	MenuCtrl_Destroy();
		1009	}
		1010
		1011
		1012
		1013	//--------------------------------------------------------------
		1014	// Setup_ServoTest()
		1015	//--------------------------------------------------------------
		1016	void Setup_ServoTest( void )
		1017	{
		1018	//---------------
		1019	// Create
		1020	//---------------
		1021	MenuCtrl_Create();
		1022	MenuCtrl_SetTitleFromParentItem(); // "Servotest"
		1023
		1024	//---------------
		1025	// Menuitems
		1026	//---------------
		1027	MenuCtrl_PushML2_P( MSERVOT_PULS , MENU_ITEM, &Menu_Test_PulseWidth , MSERVOT_PULS_de , MSERVOT_PULS_en );
		1028	MenuCtrl_PushML2_P( MSERVOT_CONT , MENU_SUB , &Setup_ServoTestCont , MSERVOT_CONT_de , MSERVOT_CONT_en );
		1029	MenuCtrl_PushML2_P( MSERVOT_SERVO , MENU_ITEM, &Menu_Test_ServoNr , MSERVOT_SERVO_de , MSERVOT_SERVO_en );
		1030	MenuCtrl_PushML2_P( MSERVOT_FRAME , MENU_ITEM, &Menu_Test_Frame , MSERVOT_FRAME_de , MSERVOT_FRAME_en );
		1031
		1032	//---------------
		1033	// Control
		1034	//---------------
		1035	MenuCtrl_Control( MENUCTRL_EVENT );
		1036
		1037	//---------------
		1038	// Destroy
		1039	//---------------
		1040	MenuCtrl_Destroy();
		1041	}
		1042
		1043
		1044
		1045	//--------------------------------------------------------------
		1046	// Setup_ServoTracking()
		1047	//--------------------------------------------------------------
		1048	void Setup_ServoTracking( void )
		1049	{
		1050	uint8_t itemid;
		1051
		1052	//---------------
		1053	// Create
		1054	//---------------
		1055	MenuCtrl_Create();
		1056	MenuCtrl_SetTitleFromParentItem(); // "Tracking"
		1057
		1058	//---------------
		1059	// Menuitems
		1060	//---------------
		1061	MenuCtrl_PushML2_P( MSERVO_STEPS , MENU_ITEM, &Menu_Servo_Steps , MSERVO_STEPS_de , MSERVO_STEPS_en );
		1062	MenuCtrl_PushML2_P( MSERVO_SERVO1 , MENU_SUB , NOFUNC , MSERVO_SERVO1_de , MSERVO_SERVO1_en );
		1063	MenuCtrl_PushML2_P( MSERVO_SERVO2 , MENU_SUB , NOFUNC , MSERVO_SERVO2_de , MSERVO_SERVO2_en );
		1064	MenuCtrl_PushML2_P( MSERVO_SERVOTEST , MENU_SUB , &Setup_ServoTest , MSERVO_SERVOTEST_de , MSERVO_SERVOTEST_en );
		1065
		1066
		1067	//---------------
		1068	// Control
		1069	//---------------
		1070	while( true )
		1071	{
		1072	MenuCtrl_Control( MENUCTRL_EVENT );
		1073
		1074	if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE
		1075
		1076	itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID)
		1077
		1078	if( itemid == MSERVO_SERVO1 ) { Setup_ServoAdjust(0); }
		1079	if( itemid == MSERVO_SERVO2 ) { Setup_ServoAdjust(1); }
		1080	}
		1081
		1082	//---------------
		1083	// Destroy
		1084	//---------------
		1085	MenuCtrl_Destroy();
		1086	}
		1087
		1088
		1089
		1090
		1091	//--------------------------------------------------------------
		1092
		1093	//void start_tracking(void)
		1094	//{
		1095	// #define TIMEOUT 200 // 2 sec
		1096	//
		1097	// uint16_t old_anglePan = 0;
		1098	// uint16_t old_angleTilt = 0;
		1099	//
		1100	// //uint16_t old_hh = 0;
		1101	// uint8_t flag;
		1102	// uint8_t tmp_dat;
		1103	//
		1104	// lcd_cls ();
		1105	// //lcd_printpns_at(0, 0, PSTR("start_tracking "), 2);
		1106	//
		1107	// //lcd_printpns_at(0, 1, PSTR("ab jetzt Tracking"), 0);
		1108	//
		1109	// lcd_ecircle(22, 35, 16, 1);
		1110	// lcd_ecircle(88, 35, 16, 1);
		1111	// lcd_putc (10, 1, 0x1e, 0); // degree symbol
		1112	// lcd_putc (20, 1, 0x1e, 0); // degree symbol
		1113	//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
		1114	// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
		1115	// SwitchToNC();
		1116	//
		1117	// mode = 'O';
		1118	//
		1119	// // disable debug...
		1120	// // RS232_request_mk_data (0, 'd', 0);
		1121	// tmp_dat = 0;
		1122	// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
		1123	//
		1124	// // request OSD Data from NC every 100ms
		1125	// // RS232_request_mk_data (1, 'o', 100);
		1126	// tmp_dat = 10;
		1127	// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
		1128	//
		1129	// if (rxd_buffer_locked)
		1130	// {
		1131	// timer = TIMEOUT;
		1132	// Decode64 ();
		1133	// naviData = (NaviData_t *) pRxData;
		1134	//
		1135	// if(error1 == 1)
		1136	// lcd_cls();
		1137	// }
		1138	//
		1139	// GPS_Pos_t currpos;
		1140	// currpos.Latitude = naviData->CurrentPosition.Latitude;
		1141	// currpos.Longitude = naviData->CurrentPosition.Longitude;
		1142	//
		1143	// flag = 0;
		1144	// timer = TIMEOUT;
		1145	// abo_timer = ABO_TIMEOUT;
		1146	//
		1147	// coldstart = 1;
		1148	//
		1149	// do
		1150	// {
		1151	// if (rxd_buffer_locked)
		1152	// {
		1153	// timer = TIMEOUT;
		1154	// Decode64 ();
		1155	// naviData = (NaviData_t *) pRxData;
		1156	//
		1157	//
		1158	////CB uint8_t FCStatusFlag = naviData->FCFlags;
		1159	// uint8_t FCStatusFlag = naviData->FCStatusFlags;
		1160	// //write_ndigit_number_u (0, 0, FCStatusFlag);
		1161	//
		1162	// Tracking_GPS();
		1163	//
		1164	// //uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360;
		1165	//
		1166	// // alte Linien l�schen
		1167	// //lcd_ecirc_line (22, 35, 15, old_hh, 0);
		1168	// //old_hh = heading_home;
		1169	// lcd_ecirc_line (22, 35, 15, old_anglePan, 0);
		1170	// old_anglePan = anglePan;
		1171	// lcd_ecirc_line (88, 35, 15, old_angleTilt, 0);
		1172	// old_angleTilt = angleTilt;
		1173	//
		1174	// lcd_ecirc_line (22, 35, 15, anglePan, 1);
		1175	// write_ndigit_number_u (7, 1, anglePan, 3, 0,0);
		1176	// lcd_ecirc_line (88, 35, 15, angleTilt, 1);
		1177	// write_ndigit_number_u (17, 1, angleTilt, 3, 0,0);
		1178	//
		1179	// rxd_buffer_locked = FALSE;
		1180	//
		1181	// if (!abo_timer)
		1182	// { // renew abo every 3 sec
		1183	// // request OSD Data from NC every 100ms
		1184	// // RS232_request_mk_data (1, 'o', 100);
		1185	// tmp_dat = 10;
		1186	// SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
		1187	//
		1188	// abo_timer = ABO_TIMEOUT;
		1189	// }
		1190	// }
		1191	//
		1192	// if (!timer)
		1193	// {
		1194	// OSD_Timeout(flag);
		1195	// flag = 0;
		1196	// }
		1197	// }
		1198	// while(!get_key_press (1 << KEY_ESC));
		1199	//
		1200	// //lcd_cls();
		1201	// //return;
		1202	//}
		1203
		1204	//--------------------------------------------------------------
		1205	//
		1206	//void conect2at_unit(void)
		1207	//{
		1208	// lcd_cls ();
		1209	// lcd_printpns_at(0, 0, PSTR("conect2at_unit "), 2);
		1210	//
		1211	// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2);
		1212	//
		1213	//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
		1214	// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
		1215	//
		1216	// while(!get_key_press (1 << KEY_ESC));
		1217	//
		1218	// lcd_cls();
		1219	// return;
		1220	//}
		1221	//
		1222	////--------------------------------------------------------------
		1223	//
		1224	//void conect2gps_ser (void)
		1225	//{
		1226	// lcd_cls ();
		1227	// lcd_printpns_at(0, 0, PSTR("conect2gps_ser "), 2);
		1228	//
		1229	// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2);
		1230	//
		1231	//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
		1232	// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
		1233	// while(!get_key_press (1 << KEY_ESC));
		1234	//
		1235	// lcd_cls();
		1236	// return;
		1237	//}
		1238	//
		1239	////--------------------------------------------------------------
		1240	//
		1241	//void conect2gps_bt (void)
		1242	//{
		1243	// lcd_cls ();
		1244	// lcd_printpns_at(0, 0, PSTR("conect2gps_bt "), 2);
		1245	//
		1246	// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2);
		1247	//
		1248	//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
		1249	// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
		1250	// while(!get_key_press (1 << KEY_ESC));
		1251	//
		1252	// lcd_cls();
		1253	// return;
		1254	//}
		1255
		1256	//--------------------------------------------------------------
		1257
		1258	//void conect2gps_menu(void)
		1259	//{
		1260	// uint8_t ii = 0;
		1261	// uint8_t Offset = 0;
		1262	// uint8_t size = ITEMS_CONECT_GPS;
		1263	// uint8_t dmode = 0;
		1264	// uint8_t target_pos = 1;
		1265	// uint8_t val = 0;
		1266	//
		1267	// while(1)
		1268	// {
		1269	// lcd_cls ();
		1270	// lcd_printpns_at(0, 0, PSTR("conect2gps_menu "), 2);
		1271	//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
		1272	// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
		1273	// while(2)
		1274	// {
		1275	// ii = 0;
		1276	// if(Offset > 0)
		1277	// {
		1278	// lcd_printp_at(1,1, PSTR("\x12"), 0);
		1279	// }
		1280	// for(ii = 0;ii < 6 ; ii++)
		1281	// {
		1282	// if((ii+Offset) < size)
		1283	// {
		1284	// lcd_printp_at(3,ii+1,conect_gps_menuitems[ii+Offset], 0);
		1285	// }
		1286	// if((ii == 5)&&(ii+Offset < (size-1)))
		1287	// {
		1288	// lcd_printp_at(1,6, PSTR("\x13"), 0);
		1289	// }
		1290	// }
		1291	// if(dmode == 0)
		1292	// {
		1293	// if(Offset == 0)
		1294	// {
		1295	// if(size > 6)
		1296	// {
		1297	// val = menu_choose2 (1, 5, target_pos,0,1);
		1298	// }
		1299	// else
		1300	// {
		1301	// val = menu_choose2 (1, size, target_pos,0,0);
		1302	// }
		1303	// }
		1304	// else
		1305	// {
		1306	// val = menu_choose2 (2, 5, target_pos,1,1);
		1307	// }
		1308	// }
		1309	// if(dmode == 1)
		1310	// {
		1311	// if(Offset+7 > size)
		1312	// {
		1313	// val = menu_choose2 (2, 6, target_pos,1,0);
		1314	// }
		1315	// else
		1316	// {
		1317	// val = menu_choose2 (2, 5, target_pos,1,1);
		1318	// }
		1319	// }
		1320	// if(val == 254)
		1321	// {
		1322	// Offset++;
		1323	// dmode = 1;
		1324	// target_pos = 5;
		1325	// }
		1326	// else if(val == 253)
		1327	// {
		1328	// Offset--;
		1329	// dmode = 0;
		1330	// target_pos = 2;
		1331	// }
		1332	// else if(val == 255)
		1333	// {
		1334	// return;
		1335	// }
		1336	// else
		1337	// {
		1338	// break;
		1339	// }
		1340	// }
		1341	// target_pos = val;
		1342	//
		1343	// if((val+Offset) == 1 )
		1344	// conect2gps_ser();
		1345	// if((val+Offset) == 2 )
		1346	// conect2gps_bt();
		1347	// }
		1348	//}
		1349	//--------------------------------------------------------------
		1350	//void tracking_menu(void)
		1351	//{
		1352	// uint8_t ii = 0;
		1353	// uint8_t Offset = 0;
		1354	// uint8_t size = ITEMS_AT;
		1355	// uint8_t dmode = 0;
		1356	// uint8_t target_pos = 1;
		1357	// uint8_t val = 0;
		1358	//
		1359	// while(1)
		1360	// {
		1361	// lcd_cls ();
		1362	// lcd_printpns_at(1, 0, PSTR("Tracking Men\x06 V.01 "), 2);
		1363	//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
		1364	// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
		1365	// while(2)
		1366	// {
		1367	// ii = 0;
		1368	// if(Offset > 0)
		1369	// {
		1370	// lcd_printp_at(1,1, PSTR("\x12"), 0);
		1371	// }
		1372	// for(ii = 0;ii < 6 ; ii++)
		1373	// {
		1374	// if((ii+Offset) < size)
		1375	// {
		1376	// lcd_printp_at(3,ii+1,at_menuitems[ii+Offset], 0);
		1377	// }
		1378	// if((ii == 5)&&(ii+Offset < (size-1)))
		1379	// {
		1380	// lcd_printp_at(1,6, PSTR("\x13"), 0);
		1381	// }
		1382	// }
		1383	// if(dmode == 0)
		1384	// {
		1385	// if(Offset == 0)
		1386	// {
		1387	// if(size > 6)
		1388	// {
		1389	// val = menu_choose2 (1, 5, target_pos,0,1);
		1390	// }
		1391	// else
		1392	// {
		1393	// val = menu_choose2 (1, size, target_pos,0,0);
		1394	// }
		1395	// }
		1396	// else
		1397	// {
		1398	// val = menu_choose2 (2, 5, target_pos,1,1);
		1399	// }
		1400	// }
		1401	// if(dmode == 1)
		1402	// {
		1403	// if(Offset+7 > size)
		1404	// {
		1405	// val = menu_choose2 (2, 6, target_pos,1,0);
		1406	// }
		1407	// else
		1408	// {
		1409	// val = menu_choose2 (2, 5, target_pos,1,1);
		1410	// }
		1411	// }
		1412	// if(val == 254)
		1413	// {
		1414	// Offset++;
		1415	// dmode = 1;
		1416	// target_pos = 5;
		1417	// }
		1418	// else if(val == 253)
		1419	// {
		1420	// Offset--;
		1421	// dmode = 0;
		1422	// target_pos = 2;
		1423	// }
		1424	// else if(val == 255)
		1425	// {
		1426	// return;
		1427	// }
		1428	// else
		1429	// {
		1430	// break;
		1431	// }
		1432	// }
		1433	// target_pos = val;
		1434	//
		1435	// if((val+Offset) == 1 )
		1436	// test_servo_menu();
		1437	// if((val+Offset) == 2 )
		1438	// adjust_servo_menu();
		1439	// if((val+Offset) == 3 )
		1440	// show_angle();
		1441	// if((val+Offset) == 4 )
		1442	////TODO: start_tracking();
		1443	// if((val+Offset) == 5 )
		1444	// conect2at_unit();
		1445	// if((val+Offset) == 6 )
		1446	// conect2gps_menu();
		1447	// }
		1448	//}
		1449
		1450	//--------------------------------------------------------------
		1451	// kapeschi Ant.Treking Funktionen
		1452	//--------------------------------------------------------------
		1453
		1454	// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
		1455	// zur aktuellen Position(nach Motorstart)
		1456	//geo_t calc_geo(HomePos_t home, GPS_Pos_t pos)
		1457	//{ int32_t lat1, lon1, lat2, lon2;
		1458	// int32_t d1, dlat;
		1459	// geo_t geo;
		1460	//
		1461	// lon1 = MK_pos.Home_Lon;
		1462	// lat1 = MK_pos.Home_Lat;
		1463	// lon2 = pos->Longitude;
		1464	// lat2 = pos->Latitude;
		1465	//
		1466	// // Formel verwendet von http://www.kompf.de/gps/distcalc.html
		1467	// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator
		1468	// // es wird jedoch in dm Meter weiter gerechnet
		1469	// // (tlon1 - tlon2)/10) sonst uint32_t-�berlauf bei cos(0) gleich 1
		1470	// d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;
		1471	// dlat = 1113 * (lat1 - lat2) / 10000;
		1472	// geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit
		1473	// geo.distance = sqrt32(d1 * d1 + dlat * dlat);
		1474	// return(geo);
		1475	//}
		1476
		1477	//void do_tracking(void)
		1478	//{ //static uint8_t hysteresis = 0;
		1479	// // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
		1480	// // (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS
		1481	// currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
		1482	//
		1483	// geo = calc_geo(&MK_pos, &currentPos);
		1484	// angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance);
		1485	// //if (geo.distance < 4 \|\| (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt
		1486	// //geo.bearing = MK_pos.direction;
		1487	// //angleTilt = 0;
		1488	// //hysteresis = 1;
		1489	// //}
		1490	// //else {
		1491	// //hysteresis = 0;
		1492	// //}
		1493	////
		1494	// //// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen
		1495	// //// 360 grd negative Winkelwerte als positive
		1496	// anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90
		1497	//
		1498	// //if (angleTilt < 0) angleTilt = 0;
		1499	// //if (angleTilt > 180) angleTilt = 180;
		1500	////
		1501	// //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
		1502	// //anglePan = anglePan - 180;
		1503	// //angleTilt = 180 - angleTilt;
		1504	// //
		1505	// //}
		1506	////angleTilt = 180;
		1507	////angleTilt = 180;
		1508	//
		1509	//// servoSetAngle(0, anglePan);
		1510	//// servoSetAngle(1, angleTilt);
		1511	//}
		1512
		1513
		1514	/****************************************************************/
		1515	/* */
		1516	/* MK GPS Tracking */
		1517	/* */
		1518	/****************************************************************/
		1519
		1520	// MK OSD-Daten lesen und verifizieren
		1521	//uint8_t OSD_Data_valid(NaviData_t **navi_data)
		1522	//{ uint8_t ret = 0;
		1523	//char *tx_osd = {"#co?]==EH\r"};
		1524	//// char interval[2] = {10, '\0'};
		1525	//
		1526	//if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren
		1527	////navi_data = (NaviData_t)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen
		1528	//if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok.
		1529	//currentPos = (*navi_data)->CurrentPosition;
		1530	//if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK)
		1531	//ret = 1;
		1532	//// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
		1533	//// (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS
		1534	//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
		1535	//satsInUse = (*navi_data)->SatsInUse;
		1536	//}
		1537	//}
		1538	//// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
		1539	//// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval
		1540	//if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval
		1541	//return ret;
		1542	//}
		1543	//
		1544
		1545	// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel
		1546	// Hauptprogramm von GPS Antennen-Nachf�hrung
		1547	//void Tracking_GPS(void)
		1548	//{ //NaviData_t *navidata;
		1549	// static uint8_t track_running = 0;
		1550	//
		1551	// if (!track_running)
		1552	// {
		1553	// //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!!
		1554	// //if (OSD_Data_valid(&naviData)) {
		1555	// if (coldstart)
		1556	// {
		1557	// //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen
		1558	// //if (naviData->FCFlags & FC_FLAG_MOTOR_START)
		1559	// //{
		1560	// MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0;
		1561	// MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0;
		1562	// MK_pos.Home_Lon7 = naviData->HomePosition.Longitude;
		1563	// MK_pos.Home_Lat7 = naviData->HomePosition.Latitude;
		1564	// MK_pos.Home_Alt = naviData->HomePosition.Altitude;
		1565	// MK_pos.direction = naviData->CompassHeading;
		1566	// coldstart = 0;
		1567	// //}
		1568	// //}
		1569	// //else {
		1570	// //do_tracking();
		1571	// }
		1572	// //}
		1573	// track_running = 0;
		1574	// }
		1575	// do_tracking();
		1576	//}
		1577	#endif

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/tracking/servo_setup.c @ 2208 - Rev