WebSVN - Projects - Blame - Rev 1687 - /NGVideo5

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

Subversion Repositories Projects

(root)/NGVideo5_8/tags/v1.33/tracking.c - Rev 1687