0,0 → 1,259 |
|
/****************************************************************/ |
/* */ |
/* 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; |
|
geo_t geo; |
int AngelPan, AngelTilt; |
float Altitude; |
|
/**************************************************************/ |
/* */ |
/* 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 */ |
/* */ |
/**************************************************************/ |
|
// Umrechnung Winkel in Servoschritte |
void servo_track(uint8_t servo_nr, int16_t Angel) |
{ |
servoSetPosition(servo_nr, (uint16_t)((uint32_t)Angel * ServoSteps() / 180)); |
} |
|
// Anzeige eines GPS-Wertes auf LCD |
void Displ_GPS_Data(uint8_t x, uint8_t y, char *description, char *data, char measure) |
{ |
lcdGotoXY(x, y); |
lcdPuts(description); |
lcdPuts(data); |
if (measure != '\0') |
lcdPutc(measure); |
} |
|
// 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(5); |
else { |
Sats = MK.navi_data.SatsInUse; // aktuell empfangene Satellitenanzahl |
if (MK.navi_data.SatsInUse > 9) |
Sats = 'X'; // Anzeige einstellig, da kein Platz auf Display |
else |
Sats = MK.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 |
// GPS-Daten nicht zu alt und ok. |
if ((rx_timeout < RX_TIME_OLD) && (MK.navi_data.NCFlags & NC_FLAG_GPS_OK)) { |
ret = 1; |
MK_Motor_run = MK.navi_data.FCStatusFlags & FC_FLAG_MOTOR_RUN; |
} |
} |
// 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); |
} |
|
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
// Hauptprogramm von GPS Antennen-Nachführung |
void Tracking_GPS(void) |
{ |
if (OSD_Data_valid()) { |
if (coldstart) { |
// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
if (MK.navi_data.FCStatusFlags & FC_FLAG_MOTOR_START) { |
// doppelt um nur einmal nach Start zu rechnen und Anzeige ohne! Minimalistic printf version |
MK_pos.Home_Lon = (double)MK.navi_data.HomePosition.Longitude / 10000000; |
MK_pos.Home_Lat = (double)MK.navi_data.HomePosition.Latitude / 10000000; |
MK_pos.Home_Lon_7 = MK.navi_data.HomePosition.Longitude; |
MK_pos.Home_Lat_7 = MK.navi_data.HomePosition.Latitude; |
MK_pos.Home_Alt = MK.navi_data.HomePosition.Altitude; |
MK_pos.direction = MK.navi_data.CompassHeading; |
coldstart = 0; |
} |
} |
else { |
geo = calc_geo(MK_pos, MK.navi_data.CurrentPosition); |
Altitude = (float)(MK.navi_data.CurrentPosition.Altitude - MK_pos.Home_Alt) / 1000; |
AngelTilt = RAD_TO_DEG * atan2(Altitude, geo.distance); |
if (geo.distance < 4) { // Abstand MK- zu Antennenposition < 4m ==> Pan-Servo in Mittelstellung |
geo.bearing = MK_pos.direction; |
if (Altitude < 3) AngelTilt = 0; // wenn (Altitude <= 0) ist berechnet AngelTilt sowieso 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) && (!coldstart) && (bat_low != 0)) { |
if (gps_disp_clear) { |
gps_disp_clear = 0; |
lcdClear(); |
} |
// makefile ohne! Minimalistic printf version |
switch(gps_display) { |
case GPS_DISP_CALC: |
Displ_GPS_Data(0, 0, "Dir:", my_itoa(MK_pos.direction, 0, 3, 0, 0), '\xdf'); |
if (geo.distance < 10) // Anzeige mit einer Kommastelle |
Displ_GPS_Data(8, 0, "Dis:", my_itoa((uint32_t)(geo.distance * 10), 0, 3, 1, 1), 'm'); |
else // Anzeige ohne Kommastelle bis 999m, Überlauf nicht abgefangen! |
Displ_GPS_Data(8, 0, "Dis:", my_itoa((uint32_t)geo.distance, 0, 3, 0, 0), 'm'); |
Displ_GPS_Data(0, 1, "Bea:", my_itoa((uint32_t)(geo.bearing), 0, 3, 0, 0), '\xdf'); |
Displ_GPS_Data(8 ,1, "Alt:", my_itoa((uint32_t)(Altitude), 1, 2, 0, 0), 'm'); |
Displ_GPS_Data(0, 2, "Pan:", my_itoa(AngelPan, 0, 3, 0, 0), '\xdf'); |
Displ_GPS_Data(8, 2, "Til:", my_itoa(AngelTilt, 0, 3, 0, 0), '\xdf'); |
break; |
case GPS_DISP_CURRENT: |
Displ_GPS_Data(0, 0, "aLon:", my_itoa(MK.navi_data.CurrentPosition.Longitude, 0, 11, 7, 7), '\0'); |
Displ_GPS_Data(0, 1, "aLat:", my_itoa(MK.navi_data.CurrentPosition.Latitude, 0, 11, 7, 7), '\0'); |
Displ_GPS_Data(0, 2, "aAlt: ", my_itoa(MK.navi_data.CurrentPosition.Altitude, 1, 11, 3, 1), '\0'); |
break; |
case GPS_DISP_HOME: |
Displ_GPS_Data(0, 0, "hLon:", my_itoa(MK_pos.Home_Lon_7, 0, 11, 7, 7), '\0'); |
Displ_GPS_Data(0, 1, "hLat:", my_itoa(MK_pos.Home_Lat_7, 0, 11, 7, 7), '\0'); |
Displ_GPS_Data(0, 2, "hAlt: ", my_itoa(MK_pos.Home_Alt, 1, 11, 3, 1), '\0'); |
} |
} |
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(MK.data_decode, POLOLU_CMD)) { |
switch(MK.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 = MK.data_decode[2]; |
ServoPos = ((ServoPos << 7) | MK.data_decode[3]) / 20; // ServoPos * ServoSteps() sonst zu groß |
|
if (ServoPos >= 60) ServoPos -= 60; else ServoPos = 0; |
if (ServoPos > 180) ServoPos = 180; |
servoSetPosition(MK.data_decode[1], (uint16_t)((uint32_t)ServoPos * ServoSteps() / 180)); |
} |
} |
} |