Blame |
Last modification |
View Log
| RSS feed
/****************************************************************/
/* */
/* 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));
}
}
}