WebSVN - Projects - Blame - Rev 1465 - /NGVideo5

Rev	Author	Line No.	Line
1465	-	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 <stdio.h>
		14	#include <stdlib.h>
		15	#include <string.h>
		16	#include <math.h>
		17	#include <util/delay.h>
		18
		19	#include "tracking.h"
		20	#include "ngvideo.h"
		21	#include "servo.h"
		22	#include "tools.h"
		23	#include "usart.h"
		24	#include "config.h"
		25	#include "mk.h"
		26	#include "keys.h"
		27
		28	#define DLEFT 0
		29	#define DRIGHT 1
		30	#define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180
		31	#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI
		32	#define AltFaktor 22.5
		33
		34	HomePos_t MK_pos; // Home position of station
		35	GPS_Pos_t currentPos; // Current position of flying object
		36	int8_t satsInUse; // Number of satelites currently in use
		37
		38	uint8_t tracking = TRACKING_MIN;
		39	uint8_t track_hyst = TRACKING_HYSTERESE;
		40	uint8_t track_tx = 0;
		41
		42	geo_t geo;
		43	int anglePan, angleTilt;
		44
		45	/**************************************************************/
		46	/* */
		47	/* RSSI Tracking */
		48	/* */
		49	/**************************************************************/
		50
		51	void Tracking_RSSI(void)
		52	{ uint16_t u0, u1;
		53	static uint8_t direction;
		54	static uint16_t servo1pos;
		55
		56	u0 = (ADC_Read(RSSI0) * (uint32_t)udbm.korr_1)/UDBM_KORR_FA;
		57	u1 = (ADC_Read(RSSI1) * (uint32_t)udbm.korr_2)/UDBM_KORR_FA;
		58
		59	if (direction == DLEFT) {
		60	if (u0 < u1) {
		61	if ( servo1pos > 0) --servo1pos; // Servo Endposition?
		62	servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausf�hren
		63	}
		64	else
		65	if (u0 > (u1 + track_hyst)) direction = DRIGHT; // Richtungwechsel, wenn Hysterese �berschritten
		66	}
		67	else {
		68	if (u0 > u1) {
		69	if ( servo1pos < ServoSteps()) ++servo1pos; // Servo Endposition?
		70	servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausf�hren
		71	}
		72	else
		73	if ((track_hyst + u0) < u1) direction = DLEFT; // Richtungwechsel, wenn Hysterese �berschritten
		74	}
		75	}
		76
		77	/**************************************************************/
		78	/* */
		79	/* GPS Tracking */
		80	/* */
		81	/**************************************************************/
		82
		83	// Datenempfang vom MK ==> blinkt Antennensymbol
		84	void Displ_wiRX(void)
		85	{ static uint8_t timer = 0;
		86	static uint8_t picturedSats;
		87
		88	if (!bat_low && (pmenu[0] == '\0')) { // nicht im Men� und nicht bei Akku leer blinken
		89	lcdGotoXY(1, 0);
		90	if (wi232RX) { // Datensatz vom MK empfangen?
		91	if (timer < BLINK_PERIOD/2) {
		92	picturedSats = 0;
		93	lcdPutc(5); // Antennensymbol darstellen
		94	}
		95	else {
		96	if (!picturedSats) { // �nderung Satelittenanzahl innerhalb dieser Periode nicht anzeigen
		97	picturedSats = 1;
		98	// aktuell empfangene Satellitenanzahl, Anzeige einstellig, da kein Platz auf Display
		99	lcdPutc(satsInUse > 9? 'X': satsInUse + '0');
		100	}
		101	}
		102	}
		103	else
		104	lcdPutc(':');
		105	wi232RX = 0;
		106	if (++timer == BLINK_PERIOD) timer = 0;
		107	}
		108	}
		109
		110	// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
		111	// zur aktuellen Position(nach Motorstart)
		112	geo_t calc_geo(HomePos_t home, GPS_Pos_t pos)
		113	{ double lat1, lon1, lat2, lon2, d1, dlat;
		114	geo_t geo;
		115
		116	lon1 = home->Home_Lon;
		117	lat1 = home->Home_Lat;
		118	lon2 = (double)pos->Longitude / 10000000.0;
		119	lat2 = (double)pos->Latitude / 10000000.0;
		120
		121	// Formel verwendet von http://www.kompf.de/gps/distcalc.html
		122	// 111.3 km = Abstand zweier Breitenkreise und/oder zweier L�ngenkreise am �quator
		123	// es wird jedoch in Meter weiter gerechnet
		124	d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
		125	dlat = 111300 * (double)(lat1 - lat2);
		126	// returns a value in metres http://www.kompf.de/gps/distcalc.html
		127	geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
		128	if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte �ber 400
		129	geo.distance = sqrt(d1 * d1 + dlat * dlat);
		130	return(geo);
		131	}
		132
		133	// Anzeige eines GPS-Wertes auf LCD
		134	void Displ_Format_Data(uint8_t x, uint8_t y, char description, char data, char measure)
		135	{
		136	lcdGotoXY(x, y);
		137	lcdPuts(description);
		138	lcdPuts(data);
		139	if (measure != '\0')
		140	lcdPutc(measure);
		141	}
		142
		143
		144	//void Displ_GPS(int anglePan, int angleTilt, geo_t *geo)
		145	void Displ_GPS(void)
		146	{ uint16_t dDCurrent = 0;
		147	// int32_t time_remaining = M59S59;
		148	int32_t time_remaining = 0;
		149
		150	if (gps_display > GPS_DISP_NONE && (!coldstart \|\| gps_display == GPS_MISC) && !bat_low) {
		151	if (gps_disp_clear && !coldstart) {
		152	gps_disp_clear = 0;
		153	lcdClear();
		154	}
		155	// makefile ohne! Minimalistic printf version
		156	switch (gps_display) {
		157	case GPS_DISP_CALC:
		158	Displ_Format_Data(0, 0, "Dir:", my_itoa(MK_pos.direction, 3, 0, 0), '\xdf'); // '\xdf' entspricht '�'
		159	Displ_Format_Data(8, 0, "Dis:", my_itoa((uint32_t)geo.distance, 3, 0, 0), 'm');
		160	Displ_Format_Data(0, 1, "Bea:", my_itoa((uint32_t)geo.bearing, 3, 0, 0), '\xdf');
		161	Displ_Format_Data(8, 1, "Alt:", my_itoa(currentPos.Altitude - MK_pos.Home_Alt, 3, 3, 1), 'm');
		162	Displ_Format_Data(0, 2, "Pan:", my_itoa(anglePan, 3, 0, 0), '\xdf');
		163	Displ_Format_Data(8, 2, "Til:", my_itoa(angleTilt, 3, 0, 0), '\xdf');
		164	break;
		165	case GPS_DISP_CURRENT:
		166	Displ_Format_Data(0, 0, "aLon:", my_itoa(currentPos.Longitude, 11, 7, 7), '\0');
		167	Displ_Format_Data(0, 1, "aLat:", my_itoa(currentPos.Latitude, 11, 7, 7), '\0');
		168	Displ_Format_Data(0, 2, "aAlt:", my_itoa(currentPos.Altitude, 11, 3, 1), '\0');
		169	break;
		170	case GPS_DISP_HOME:
		171	Displ_Format_Data(0, 0, "hLon:", my_itoa(MK_pos.Home_Lon7, 11, 7, 7), '\0');
		172	Displ_Format_Data(0, 1, "hLat:", my_itoa(MK_pos.Home_Lat7, 11, 7, 7), '\0');
		173	Displ_Format_Data(0, 2, "hAlt:", my_itoa(MK_pos.Home_Alt, 11, 3, 1), '\0');
		174	// >> Menueauswahl nach oben
		175	if (Get_Key_Press( 1<<SW_PLUS ) \|\| Get_Key_Press( 1<<SW_MINUS )) coldstart = 1;
		176	break;
		177	case GPS_MISC:
		178	Displ_Format_Data(0, 0, "U:", my_itoa(mk_UBat, 4, 1, 1), 'V');
		179	Displ_Format_Data(8, 0, "W:", my_itoa(mk_dUsedCapacity, 4, 0, 0), '\0');
		180	Displ_Format_Data(0, 1, "I:", my_itoa(mk_Ikorr, 4, 1, 1), 'A');
		181	if (mk_current.Count > 0) {
		182	dDCurrent = mk_current.Sum / mk_current.Count;
		183	if (dDCurrent > 0) {
		184	time_remaining = ((int32_t)mk_lipo.Capacity - mk_dUsedCapacity) * 36 / dDCurrent;
		185	if (abs(time_remaining) > M59S59)
		186	time_remaining = time_remaining < 0? -M59S59: M59S59; // M59S59 sind 59 Minuten und 59 Sekunden
		187	}
		188	}
		189	Displ_Format_Data(8, 1, "\xee:", my_itoa(dDCurrent, 4, 1, 1), 'A');// Durchschnittszeichen
		190	lcdGotoXY(0,2);
		191	lcdPutc('\x1a'); // Summenzeichen
		192	Displ_TimeMS(mk_lipo.time_on / T2SECDIV);
		193	lcdGotoXY(8,2);
		194	lcdPuts("R");
		195	Displ_TimeMS(time_remaining);
		196	break;
		197	}
		198	}
		199	Displ_wiRX();
		200	}
		201
		202	void do_tracking(void)
		203	{ static uint8_t hysteresis = 0;
		204
		205	geo = calc_geo(&MK_pos, &currentPos);
		206	angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance);
		207	if (geo.distance < 4 \|\| (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt
		208	geo.bearing = MK_pos.direction;
		209	angleTilt = 0;
		210	hysteresis = 1;
		211	}
		212	else {
		213	hysteresis = 0;
		214	}
		215
		216	// egal wo der �bergangspunkt 359, 360, 1grd ist => Winkel�bergangspunkt auf 0 bzw. 180grd des Servos bringen
		217	// 360 grd negative Winkelwerte als positive
		218	anglePan = fmod(geo.bearing - MK_pos.direction + 360 + 90, 360);
		219	if (anglePan > 360) anglePan -= 360;
		220
		221	if (angleTilt < 0) angleTilt = 0;
		222	if (angleTilt > 180) angleTilt = 180;
		223
		224	if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
		225	anglePan = anglePan - 180;
		226	angleTilt = 180 - angleTilt;
		227	}
		228
		229	servoSetAngle(0, anglePan);
		230	servoSetAngle(1, angleTilt);
		231	}
		232
		233
		234	/**************************************************************/
		235	/* */
		236	/* MK GPS Tracking */
		237	/* */
		238	/**************************************************************/
		239
		240	// MK OSD-Daten lesen und verifizieren
		241	uint8_t OSD_Data_valid(NaviData_t **navi_data)
		242	{ uint8_t ret = 0;
		243	char *tx_osd = {"#co?]==EH\r"};
		244	// char interval[2] = {10, '\0'};
		245
		246	if (rx_line_decode('O')) { // OSD-Datensatz pr�fen/dekodieren
		247	navi_data = (NaviData_t)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen
		248	if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok.
		249	currentPos = (*navi_data)->CurrentPosition;
		250	if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK)
		251	ret = 1;
		252	// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
		253	// (4 * (........))/5 ==> Wichtung Luftdruck-H�he zu GPS
		254	currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
		255	MK_Motor_run = (*navi_data)->FCStatusFlags & FC_FLAG_MOTOR_RUN;
		256	MK_ProcessLipo(*navi_data);
		257	satsInUse = (*navi_data)->SatsInUse;
		258	}
		259	}
		260	else
		261	if (rx_timeout == RX_TIME_END) store_LipoData();// ca. 4 Sekunden nach Signalverlust
		262	// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
		263	// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval
		264	if ((track_tx) && (rx_timeout > RX_TIMEOUT)) USART_send_Str(tx_osd); // 420 * 0.5ms interval
		265	return ret;
		266	}
		267
		268
		269	// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel
		270	// Hauptprogramm von GPS Antennen-Nachf�hrung
		271	void Tracking_GPS(void)
		272	{ NaviData_t *navi_data;
		273	static uint8_t track_running = 0;
		274
		275	if (!track_running) {
		276	track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!!
		277	if (OSD_Data_valid(&navi_data)) {
		278	if (coldstart) {
		279	// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK �bernommen
		280	if (navi_data->FCStatusFlags & FC_FLAG_MOTOR_START) {
		281	MK_pos.Home_Lon = (double)navi_data->HomePosition.Longitude / 10000000.0;
		282	MK_pos.Home_Lat = (double)navi_data->HomePosition.Latitude / 10000000.0;
		283	MK_pos.Home_Lon7 = navi_data->HomePosition.Longitude;
		284	MK_pos.Home_Lat7 = navi_data->HomePosition.Latitude;
		285	MK_pos.Home_Alt = navi_data->HomePosition.Altitude;
		286	MK_pos.direction = navi_data->CompassHeading;
		287	coldstart = 0;
		288	}
		289	}
		290	else {
		291	do_tracking();
		292	}
		293	}
		294	Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteten MK sichtbar
		295	track_running = 0;
		296	}
		297	}
		298
		299	/**************************************************************/
		300	/* */
		301	/* NMEA GPS Tracking */
		302	/* */
		303	/**************************************************************/
		304
		305	void setNMEAdir(void)
		306	{
		307	if (!coldstart)
		308	MK_pos.direction = geo.bearing;
		309	}
		310
		311	// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps
		312	int32_t getLatitude(const char s, const char NS)
		313	{ int32_t deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees
		314	int32_t min = floatStrToInt(&s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps
		315
		316	deg = deg * 10000000 + min;
		317	if (*NS == 'S') deg = -deg;
		318	return deg;
		319	}
		320
		321
		322	// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps
		323	int32_t getLongitude(const char s, const char WE)
		324	{ int32_t deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees
		325	int32_t min = floatStrToInt(&s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps
		326
		327	deg = deg * 10000000 + min;
		328	if (*WE == 'W') deg = -deg;
		329	return deg;
		330	}
		331
		332
		333	void Tracking_NMEA(void)
		334	{ char *token;
		335	int32_t latitude, longitude;
		336
		337	if (decodeNMEA()) {
		338	token = strtok((char*)data_decode, ",");
		339	// http://de.wikipedia.org/wiki/NMEA_0183
		340	if (!strcmp(token, "GPGGA")) {
		341	// $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,,
		342	// Skip time
		343	strtok(0, ",");
		344	// Latitude
		345	latitude = getLatitude(strtok(0, ","), strtok(0, ","));
		346	// Longitude
		347	longitude = getLongitude(strtok(0, ","), strtok(0, ","));
		348	// Signal valid? (Position Fix Indicator)
		349	if (*strtok(0, ",") != '0') {
		350	// Satellites in use
		351	satsInUse = atoi(strtok(0, ","));
		352	// Skip dilution
		353	strtok(0, ",");
		354	// Altitude
		355	currentPos.Altitude = floatStrToInt(strtok(0, ","), 3);
		356	currentPos.Latitude = latitude;
		357	currentPos.Longitude = longitude;
		358
		359	if ((coldstart) && (satsInUse > 5)) {
		360	// First position after reboot (or change of mode) will be the home position (facing north)
		361	MK_pos.Home_Lon = (double)currentPos.Longitude / 10000000.0;
		362	MK_pos.Home_Lat = (double)currentPos.Latitude / 10000000.0;
		363	MK_pos.Home_Lon7 = currentPos.Longitude;
		364	MK_pos.Home_Lat7 = currentPos.Latitude;
		365	MK_pos.Home_Alt = currentPos.Altitude;
		366	MK_pos.direction = 0;
		367	coldstart = 0;
		368	Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP);
		369	}
		370	do_tracking();
		371	}
		372	}
		373	}
		374	Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar
		375	}
		376
		377
		378	/************************************************************************/
		379	/* */
		380	/* MKCockPit Tracking */
		381	/* */
		382	/* http://www.pololu.com/file/download/ssc03a_guide.pdf?file_id=0J37 */
		383	/* */
		384	/************************************************************************/
		385
		386	void Tracking_MKCockpit(void)
		387	{ uint16_t ServoPos;
		388
		389	if (Get_Pololu_cmd(data_decode, POLOLU_CMD)) {
		390	switch (data_decode[0]) {
		391	// Command 0: Set Parameters(1 data byte)
		392	case 0x00: ;
		393	break;
		394	// Command 1: Set Speed (1 data byte)
		395	case 0x01: ;
		396	break;
		397	// Command 4: Set Position,Absollute(2 data bytes) - angepasst f�r default MKCockPit
		398	case 0x04: ServoPos = data_decode[2];
		399	ServoPos = ((ServoPos << 7) \| data_decode[3]) / 20; // ServoPos * ServoSteps() sonst zu gro�
		400
		401	if (ServoPos >= 60) ServoPos -= 60; else ServoPos = 0;
		402	if (ServoPos > 180) ServoPos = 180;
		403	servoSetAngle(data_decode[1], ServoPos);
		404	}
		405	}
		406	}

Subversion Repositories Projects

(root)/NGVideo5_8/tags/V1.30/tracking.c - Rev 1465