WebSVN - Projects - Blame - Rev 1115 - /NGVideo5

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

Subversion Repositories Projects

(root)/NGVideo5_8/tags/v1.11/tracking.c @ 1538 - Rev 1115