WebSVN - Projects - Blame - Rev 962 - /NGVideo5

Rev	Author	Line No.	Line
962	-	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	#include "usart.h"
		14	#include "math.h"
		15	#include <stdio.h>
		16
		17	#include "servo.h"
		18
		19	#define DLEFT 0
		20	#define DRIGHT 1
		21	#define DEG_TO_RAD 0.0174533// degrees to radians = PI / 180
		22	#define RAD_TO_DEG 57.2957795// radians to degrees = 180 / PI
		23
		24
		25	typedef struct {
		26	double distance;
		27	double bearing;
		28	} geo_t;
		29
		30	/**************************************************************/
		31	/* */
		32	/* RSSI Tracking */
		33	/* */
		34	/**************************************************************/
		35
		36	void Tracking_RSSI(void)
		37	{ uint16_t u0, u1;
		38	static uint8_t direction;
		39	static uint16_t servo1pos;
		40
		41	u0 = (ADC_Read(RSSI0) * (uint32_t)udbm_korr_1)/UDBM_KORR_FA;
		42	u1 = (ADC_Read(RSSI1) * (uint32_t)udbm_korr_2)/UDBM_KORR_FA;
		43
		44	if (direction == DLEFT) {
		45	if (u0 < u1) {
		46	if ( servo1pos > 0) --servo1pos; // Servo Endposition?
		47	servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausf�hren
		48	}
		49	else
		50	if (u0 > (u1 + track_hyst)) direction = DRIGHT; // Richtungwechsel, wenn Hysterese �berschritten
		51	}
		52	else {
		53	if (u0 > u1) {
		54	if ( servo1pos < ServoSteps()) ++servo1pos; // Servo Endposition?
		55	servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausf�hren
		56	}
		57	else
		58	if ((track_hyst + u0) < u1) direction = DLEFT; // Richtungwechsel, wenn Hysterese �berschritten
		59	}
		60	}
		61
		62	/**************************************************************/
		63	/* */
		64	/* GPS Tracking */
		65	/* */
		66	/**************************************************************/
		67
		68	// Datenempfang vom MK ==> blinkt Antennensymbol
		69	void Displ_wiRX(void)
		70	{ static uint8_t timer = 0;
		71	char Sats;
		72
		73	if ((bat_low != 0) && (pmenu[0] == '\0')) { // nicht im Men� und nicht bei Akku leer blinken
		74	lcdGotoXY(1, 0);
		75	if (wi232RX) { // Datensatz vom MK empfangen?
		76	if (timer < BLINK_PERIOD/2)
		77	lcdPutc(1);
		78	else {
		79	Sats = navi_data.SatsInUse; // aktuell empfangene Satellitenanzahl
		80	if (navi_data.SatsInUse > 9)
		81	Sats = 'X'; // Anzeige einstellig, da kein Platz auf Display
		82	else
		83	Sats = navi_data.SatsInUse + '0'; // Umrechnung Char
		84	lcdPutc(Sats);
		85	}
		86	}
		87	else
		88	lcdPutc(' ');
		89	wi232RX = 0;
		90	if (++timer == BLINK_PERIOD) timer = 0;
		91	}
		92	}
		93
		94	// Berechnung von Distanz und Winkels aus GPS-Daten home(MK eingeschaltet)
		95	// zur aktuellen Position(nach Motorstart)
		96	geo_t calc_geo(HomePos_t home, GPS_Pos_t pos)
		97	{ double lat1, lon1, lat2, lon2, d1, dlat;
		98	geo_t geo;
		99
		100	lon1 = home.Home_Lon;
		101	lat1 = home.Home_Lat;
		102	lon2 = (double)pos.Longitude / 10000000;
		103	lat2 = (double)pos.Latitude / 10000000;
		104
		105	// Formel verwendet von http://www.kompf.de/gps/distcalc.html
		106	// 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator
		107	// es wird jedoch in Meter weiter gerechnet
		108	d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
		109	dlat = 111300 * (double)(lat1 - lat2);
		110	// returns a value in metres http://www.kompf.de/gps/distcalc.html
		111	geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
		112	if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte �ber 400
		113	geo.distance = sqrt(d1 * d1 + dlat * dlat);
		114	return(geo);
		115	}
		116
		117	// MK OSD-Daten lesen und verifizieren
		118	uint8_t OSD_Data_valid(void)
		119	{ uint8_t ret = 0;
		120	char *tx_osd = {"#co?]==EH\r"};
		121	// char interval[2] = {10, '\0'};
		122
		123	if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren
		124	navi_data = ((NaviData_t)data_decode); // dekodierte Daten mit Struktur OSD-Daten versehen
		125	// GPS-Daten nicht zu alt und ok.
		126	if ((rx_timeout < RX_TIME_OLD) && (navi_data.NCFlags & NC_FLAG_GPS_OK)) {
		127	ret = 1;
		128	}
		129	}
		130	// ca. 150ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
		131	// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 10 * 10ms interval
		132	if ((track_tx) && (rx_timeout > RX_TIMEOUT)) USART_send_Str(tx_osd); // 10 * 10ms interval
		133	return(ret);
		134	}
		135
		136	// Umrechnung Winkel in Servoschritte
		137	void servo_track(uint8_t servo_nr, int16_t Angel)
		138	{
		139	servoSetPosition(servo_nr, (uint16_t)((float)Angel * ServoSteps() / 180));
		140	}
		141
		142	// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel
		143	// Hauptprogramm von GPS Antennen-Nachf�hrung
		144	void Tracking_GPS(void)
		145	{ geo_t geo;
		146	int AngelPan, AngelTilt;
		147	char puffer[30];
		148
		149	if (OSD_Data_valid()) {
		150	if (coldstart) {
		151	// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen
		152	if (navi_data.FCStatusFlags & FC_FLAG_MOTOR_START) {
		153	MK_pos.Home_Lon = (double)navi_data.HomePosition.Longitude / 10000000;
		154	MK_pos.Home_Lat = (double)navi_data.HomePosition.Latitude / 10000000;
		155	MK_pos.Home_Alt = navi_data.HomePosition.Altitude;
		156	MK_pos.direction = navi_data.CompassHeading;
		157	coldstart = 0;
		158	}
		159	}
		160	else {
		161	geo = calc_geo(MK_pos, navi_data.CurrentPosition);
		162	AngelTilt = RAD_TO_DEG * (double)atan2((double)(navi_data.CurrentPosition.Altitude - MK_pos.Home_Alt) / 1000, geo.distance);
		163	if (geo.distance < 4) { // Abstand MK- zu Antennenposition < 4m ==> Pan-Servo in Mittelstellung
		164	geo.bearing = MK_pos.direction;
		165	AngelTilt = 0;
		166	}
		167	// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen
		168	// 360 grd negative Winkelwerte als positive
		169	AngelPan = fmod(geo.bearing - MK_pos.direction + 360 + 90, 360);
		170	if (AngelPan > 360) AngelPan -= 360;
		171
		172	if (AngelTilt < 0) AngelTilt = 0;
		173	if (AngelTilt > 180) AngelTilt = 180;
		174
		175	if (AngelPan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
		176	AngelPan = AngelPan - 180;
		177	AngelTilt = 180 - AngelTilt;
		178	}
		179
		180	servo_track(0, AngelPan);
		181	servo_track(1, AngelTilt);
		182
		183	if ((gps_display > GPS_DISP_NONE) && (bat_low != 0)) {
		184	if (gps_disp_clear) {
		185	gps_disp_clear = 0;
		186	lcdClear();
		187	}
		188	// der Einfachheit halber doch noch mit Kommazahlen sprintf(...)
		189	switch(gps_display) {
		190	case GPS_DISP_CALC:
		191	sprintf( puffer, "Dir:%3d", MK_pos.direction);
		192	lcdGotoXY(0, 0);
		193	lcdPuts(puffer);
		194	sprintf( puffer, "Dis:%4.1f", geo.distance);
		195	lcdGotoXY(8, 0);
		196	lcdPuts(puffer);
		197	sprintf( puffer, "Bea:%5.1f", geo.bearing);
		198	lcdGotoXY(0, 1);
		199	lcdPuts(puffer);
		200	sprintf( puffer, "Pan:%3d", AngelPan);
		201	lcdGotoXY(0, 2);
		202	lcdPuts(puffer);
		203	sprintf( puffer, "Til:%3d", AngelTilt);
		204	lcdGotoXY(8, 2);
		205	lcdPuts(puffer);
		206	break;
		207	case GPS_DISP_CURRENT:
		208	sprintf( puffer, "aLon:%10.6f", (double)navi_data.CurrentPosition.Longitude / 10000000);
		209	lcdGotoXY(0, 0);
		210	lcdPuts(puffer);
		211	sprintf( puffer, "aLat:%10.6f", (double)navi_data.CurrentPosition.Latitude / 10000000);
		212	lcdGotoXY(0, 1);
		213	lcdPuts(puffer);
		214	sprintf( puffer, "aAlt:%10.1f", (double)navi_data.CurrentPosition.Altitude / 1000);
		215	lcdGotoXY(0, 2);
		216	lcdPuts(puffer);
		217	break;
		218	case GPS_DISP_HOME:
		219	sprintf( puffer, "hLon:%10.6f", MK_pos.Home_Lon);
		220	lcdGotoXY(0, 0);
		221	lcdPuts(puffer);
		222	sprintf( puffer, "hLat:%10.6f", MK_pos.Home_Lat);
		223	lcdGotoXY(0, 1);
		224	lcdPuts(puffer);
		225	sprintf( puffer, "hAlt:%10.1f", (double)MK_pos.Home_Alt / 1000);
		226	lcdGotoXY(0, 2);
		227	lcdPuts(puffer);
		228	break;
		229	}
		230	}
		231	}
		232	}
		233	Displ_wiRX();
		234	}
		235
		236	/************************************************************************/
		237	/* */
		238	/* MKCockPit Tracking */
		239	/* */
		240	/* http://www.pololu.com/file/download/ssc03a_guide.pdf?file_id=0J37 * */
		241	/* */
		242	/************************************************************************/
		243
		244	void Tracking_MKCockpit(void)
		245	{ uint16_t ServoPos;
		246
		247	if (Get_Pololu_cmd(data_decode, POLOLU_CMD)) {
		248	switch(data_decode[0]) {
		249	// Command 0: Set Parameters(1 data byte)
		250	case 0x00: ;
		251	break;
		252	// Command 1: Set Speed (1 data byte)
		253	case 0x01: ;
		254	break;
		255	// Command 4: Set Position,Absollute(2 data bytes) - angepasst f�r default MKCockPit
		256	case 0x04:ServoPos = data_decode[2];
		257	ServoPos = ((ServoPos << 7) \| data_decode[3]) / 20; // ServoPos * ServoSteps() sonst zu gro�
		258
		259	if (ServoPos >= 60) ServoPos -= 60; else ServoPos = 0;
		260	if (ServoPos > 180) ServoPos = 180;
		261	servoSetPosition(data_decode[1], (uint16_t)((float)ServoPos * ServoSteps() / 180));
		262	}
		263	}
		264	}

Subversion Repositories Projects

(root)/NGVideo5_8/tags/v1.00/tracking.c - Rev 962