Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 961 → Rev 962

/NGVideo5_8/tags/v1.00/tracking.c
0,0 → 1,264
 
/****************************************************************/
/* */
/* NG-Video 5,8GHz */
/* */
/* Copyright (C) 2011 - gebad */
/* */
/* This code is distributed under the GNU Public License */
/* which can be found at http://www.gnu.org/licenses/gpl.txt */
/* */
/****************************************************************/
 
#include "usart.h"
#include "math.h"
#include <stdio.h>
 
#include "servo.h"
 
#define DLEFT 0
#define DRIGHT 1
#define DEG_TO_RAD 0.0174533// degrees to radians = PI / 180
#define RAD_TO_DEG 57.2957795// radians to degrees = 180 / PI
 
 
typedef struct {
double distance;
double bearing;
} geo_t;
 
/**************************************************************/
/* */
/* RSSI Tracking */
/* */
/**************************************************************/
 
void Tracking_RSSI(void)
{ uint16_t u0, u1;
static uint8_t direction;
static uint16_t servo1pos;
u0 = (ADC_Read(RSSI0) * (uint32_t)udbm_korr_1)/UDBM_KORR_FA;
u1 = (ADC_Read(RSSI1) * (uint32_t)udbm_korr_2)/UDBM_KORR_FA;
 
if (direction == DLEFT) {
if (u0 < u1) {
if ( servo1pos > 0) --servo1pos; // Servo Endposition?
servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausführen
}
else
if (u0 > (u1 + track_hyst)) direction = DRIGHT; // Richtungwechsel, wenn Hysterese überschritten
}
else {
if (u0 > u1) {
if ( servo1pos < ServoSteps()) ++servo1pos; // Servo Endposition?
servoSetPosition(SERVO_PAN, servo1pos); // beibehaltene Richtung ==> sofort ausführen
}
else
if ((track_hyst + u0) < u1) direction = DLEFT; // Richtungwechsel, wenn Hysterese überschritten
}
}
 
/**************************************************************/
/* */
/* GPS Tracking */
/* */
/**************************************************************/
 
// Datenempfang vom MK ==> blinkt Antennensymbol
void Displ_wiRX(void)
{ static uint8_t timer = 0;
char Sats;
if ((bat_low != 0) && (pmenu[0] == '\0')) { // nicht im Menü und nicht bei Akku leer blinken
lcdGotoXY(1, 0);
if (wi232RX) { // Datensatz vom MK empfangen?
if (timer < BLINK_PERIOD/2)
lcdPutc(1);
else {
Sats = navi_data.SatsInUse; // aktuell empfangene Satellitenanzahl
if (navi_data.SatsInUse > 9)
Sats = 'X'; // Anzeige einstellig, da kein Platz auf Display
else
Sats = navi_data.SatsInUse + '0'; // Umrechnung Char
lcdPutc(Sats);
}
}
else
lcdPutc(' ');
wi232RX = 0;
if (++timer == BLINK_PERIOD) timer = 0;
}
}
 
// Berechnung von Distanz und Winkels aus GPS-Daten home(MK eingeschaltet)
// zur aktuellen Position(nach Motorstart)
geo_t calc_geo(HomePos_t home, GPS_Pos_t pos)
{ double lat1, lon1, lat2, lon2, d1, dlat;
geo_t geo;
lon1 = home.Home_Lon;
lat1 = home.Home_Lat;
lon2 = (double)pos.Longitude / 10000000;
lat2 = (double)pos.Latitude / 10000000;
 
// Formel verwendet von http://www.kompf.de/gps/distcalc.html
// 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator
// es wird jedoch in Meter weiter gerechnet
d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
dlat = 111300 * (double)(lat1 - lat2);
// returns a value in metres http://www.kompf.de/gps/distcalc.html
geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400
geo.distance = sqrt(d1 * d1 + dlat * dlat);
return(geo);
}
 
// MK OSD-Daten lesen und verifizieren
uint8_t OSD_Data_valid(void)
{ uint8_t ret = 0;
char *tx_osd = {"#co?]==EH\r"};
// char interval[2] = {10, '\0'};
if (rx_line_decode('O')) { // OSD-Datensatz prüfen/dekodieren
navi_data = *((NaviData_t*)data_decode); // dekodierte Daten mit Struktur OSD-Daten versehen
// GPS-Daten nicht zu alt und ok.
if ((rx_timeout < RX_TIME_OLD) && (navi_data.NCFlags & NC_FLAG_GPS_OK)) {
ret = 1;
}
}
// ca. 150ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 10 * 10ms interval
if ((track_tx) && (rx_timeout > RX_TIMEOUT)) USART_send_Str(tx_osd); // 10 * 10ms interval
return(ret);
}
 
// Umrechnung Winkel in Servoschritte
void servo_track(uint8_t servo_nr, int16_t Angel)
{
servoSetPosition(servo_nr, (uint16_t)((float)Angel * ServoSteps() / 180));
}
 
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel
// Hauptprogramm von GPS Antennen-Nachführung
void Tracking_GPS(void)
{ geo_t geo;
int AngelPan, AngelTilt;
char puffer[30];
if (OSD_Data_valid()) {
if (coldstart) {
// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen
if (navi_data.FCStatusFlags & FC_FLAG_MOTOR_START) {
MK_pos.Home_Lon = (double)navi_data.HomePosition.Longitude / 10000000;
MK_pos.Home_Lat = (double)navi_data.HomePosition.Latitude / 10000000;
MK_pos.Home_Alt = navi_data.HomePosition.Altitude;
MK_pos.direction = navi_data.CompassHeading;
coldstart = 0;
}
}
else {
geo = calc_geo(MK_pos, navi_data.CurrentPosition);
AngelTilt = RAD_TO_DEG * (double)atan2((double)(navi_data.CurrentPosition.Altitude - MK_pos.Home_Alt) / 1000, geo.distance);
if (geo.distance < 4) { // Abstand MK- zu Antennenposition < 4m ==> Pan-Servo in Mittelstellung
geo.bearing = MK_pos.direction;
AngelTilt = 0;
}
// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen
// 360 grd negative Winkelwerte als positive
AngelPan = fmod(geo.bearing - MK_pos.direction + 360 + 90, 360);
if (AngelPan > 360) AngelPan -= 360;
 
if (AngelTilt < 0) AngelTilt = 0;
if (AngelTilt > 180) AngelTilt = 180;
if (AngelPan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
AngelPan = AngelPan - 180;
AngelTilt = 180 - AngelTilt;
}
 
servo_track(0, AngelPan);
servo_track(1, AngelTilt);
if ((gps_display > GPS_DISP_NONE) && (bat_low != 0)) {
if (gps_disp_clear) {
gps_disp_clear = 0;
lcdClear();
}
// der Einfachheit halber doch noch mit Kommazahlen sprintf(...)
switch(gps_display) {
case GPS_DISP_CALC:
sprintf( puffer, "Dir:%3d", MK_pos.direction);
lcdGotoXY(0, 0);
lcdPuts(puffer);
sprintf( puffer, "Dis:%4.1f", geo.distance);
lcdGotoXY(8, 0);
lcdPuts(puffer);
sprintf( puffer, "Bea:%5.1f", geo.bearing);
lcdGotoXY(0, 1);
lcdPuts(puffer);
sprintf( puffer, "Pan:%3d", AngelPan);
lcdGotoXY(0, 2);
lcdPuts(puffer);
sprintf( puffer, "Til:%3d", AngelTilt);
lcdGotoXY(8, 2);
lcdPuts(puffer);
break;
case GPS_DISP_CURRENT:
sprintf( puffer, "aLon:%10.6f", (double)navi_data.CurrentPosition.Longitude / 10000000);
lcdGotoXY(0, 0);
lcdPuts(puffer);
sprintf( puffer, "aLat:%10.6f", (double)navi_data.CurrentPosition.Latitude / 10000000);
lcdGotoXY(0, 1);
lcdPuts(puffer);
sprintf( puffer, "aAlt:%10.1f", (double)navi_data.CurrentPosition.Altitude / 1000);
lcdGotoXY(0, 2);
lcdPuts(puffer);
break;
case GPS_DISP_HOME:
sprintf( puffer, "hLon:%10.6f", MK_pos.Home_Lon);
lcdGotoXY(0, 0);
lcdPuts(puffer);
sprintf( puffer, "hLat:%10.6f", MK_pos.Home_Lat);
lcdGotoXY(0, 1);
lcdPuts(puffer);
sprintf( puffer, "hAlt:%10.1f", (double)MK_pos.Home_Alt / 1000);
lcdGotoXY(0, 2);
lcdPuts(puffer);
break;
}
}
}
}
Displ_wiRX();
}
 
/************************************************************************/
/* */
/* MKCockPit Tracking */
/* */
/* http://www.pololu.com/file/download/ssc03a_guide.pdf?file_id=0J37 * */
/* */
/************************************************************************/
 
void Tracking_MKCockpit(void)
{ uint16_t ServoPos;
 
if (Get_Pololu_cmd(data_decode, POLOLU_CMD)) {
switch(data_decode[0]) {
// Command 0: Set Parameters(1 data byte)
case 0x00: ;
break;
// Command 1: Set Speed (1 data byte)
case 0x01: ;
break;
// Command 4: Set Position,Absollute(2 data bytes) - angepasst für default MKCockPit
case 0x04:ServoPos = data_decode[2];
ServoPos = ((ServoPos << 7) | data_decode[3]) / 20; // ServoPos * ServoSteps() sonst zu groß
if (ServoPos >= 60) ServoPos -= 60; else ServoPos = 0;
if (ServoPos > 180) ServoPos = 180;
servoSetPosition(data_decode[1], (uint16_t)((float)ServoPos * ServoSteps() / 180));
}
}
}