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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "tracking.h"
#include "ngvideo.h"
#include "servo.h"
#include "tools.h"
#include "usart.h"
#include "config.h"
#include "mk.h"
#include "keys.h"
#include "mymath.h"
HomePos_t MK_pos
; // Home position of station
GPS_Pos_t currentPos
; // Current position of flying object
int8_t satsInUse
; // Number of satelites currently in use
uint8_t tracking
= TRACKING_MIN
;
uint8_t track_hyst
= TRACKING_HYSTERESE
;
uint8_t track_tx
= 0;
geo_t geo
;
int16_t anglePan
, angleTilt
;
/**************************************************************/
/* */
/* RSSI Tracking */
/* */
/**************************************************************/
void Tracking_RSSI
(void)
{ uint16_t u0
, u1
;
static uint8_t direction
;
static uint16_t servo1pos
;
u0
= (ADC_Read_inv
(RSSI0
) * (uint32_t)udbm.
korr_1)/UDBM_KORR_FA
;
u1
= (ADC_Read_inv
(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;
static uint8_t picturedSats
;
if (!bat_low
&& (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) {
picturedSats
= 0;
lcdPutc
(5); // Antennensymbol darstellen
}
else {
if (!picturedSats
) { // Änderung Satelittenanzahl innerhalb dieser Periode nicht anzeigen
picturedSats
= 1;
// aktuell empfangene Satellitenanzahl, Anzeige einstellig, da kein Platz auf Display
lcdPutc
(satsInUse
> 9? 'X': satsInUse
+ '0');
}
}
}
else
lcdPutc
(':');
if (timer
% 20 == 0) wi232RX
= 0; // mehr Zeit für nächsten Datensatz lassen
if (++timer
== BLINK_PERIOD
) timer
= 0;
}
}
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
// zur aktuellen Position(nach Motorstart)
geo_t calc_geo
(HomePos_t
*home
, GPS_Pos_t
*pos
)
{ int32_t lat1
, lon1
, lat2
, lon2
;
int32_t d1
, dlat
;
geo_t geo
;
lon1
= MK_pos.
Home_Lon;
lat1
= MK_pos.
Home_Lat;
lon2
= pos
->Longitude
;
lat2
= pos
->Latitude
;
// 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 dm Meter weiter gerechnet
// (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1
d1
= (1359 * (int32_t)(c_cos_8192
((lat1
+ lat2
) / 20000000)) * ((lon1
- lon2
)/10))/ 10000000;
dlat
= 1113 * (lat1
- lat2
) / 10000;
geo.
bearing = (my_atan2
(d1
, dlat
) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit
geo.
distance = sqrt32
(d1
* d1
+ dlat
* dlat
);
return(geo
);
}
// Anzeige eines GPS-Wertes auf LCD
void Displ_Format_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
);
}
//void Displ_GPS(int anglePan, int angleTilt, geo_t *geo)
void Displ_GPS
(void)
{ uint16_t dDCurrent
= 0;
// int32_t time_remaining = M59S59;
int32_t time_remaining
= 0;
if (gps_display
> GPS_DISP_NONE
&& (!coldstart
|| gps_display
== GPS_MISC
) && !bat_low
) {
if (gps_disp_clear
&& !coldstart
) {
gps_disp_clear
= 0;
lcdClear
();
}
// makefile ohne! Minimalistic printf version
switch (gps_display
) {
case GPS_DISP_CALC
:
Displ_Format_Data
(0, 0, "Dir:", my_itoa
(MK_pos.
direction, 3, 0, 0), '\xdf'); // '\xdf' entspricht '°'
Displ_Format_Data
(8, 0, "Dis:", my_itoa
((uint32_t)geo.
distance, 3, 1, 1), 'm');
Displ_Format_Data
(0, 1, "Bea:", my_itoa
((uint32_t)geo.
bearing, 3, 0, 0), '\xdf');
Displ_Format_Data
(8, 1, "Alt:", my_itoa
(currentPos.
Altitude - MK_pos.
Home_Alt, 3, 3, 1), 'm');
Displ_Format_Data
(0, 2, "Pan:", my_itoa
(anglePan
, 3, 0, 0), '\xdf');
Displ_Format_Data
(8, 2, "Til:", my_itoa
(angleTilt
, 3, 0, 0), '\xdf');
break;
case GPS_DISP_CURRENT
:
Displ_Format_Data
(0, 0, "aLon:", my_itoa
(currentPos.
Longitude, 11, 7, 7), '\0');
Displ_Format_Data
(0, 1, "aLat:", my_itoa
(currentPos.
Latitude, 11, 7, 7), '\0');
Displ_Format_Data
(0, 2, "aAlt:", my_itoa
(currentPos.
Altitude, 11, 3, 1), '\0');
break;
case GPS_DISP_HOME
:
Displ_Format_Data
(0, 0, "hLon:", my_itoa
(MK_pos.
Home_Lon, 11, 7, 7), '\0');
Displ_Format_Data
(0, 1, "hLat:", my_itoa
(MK_pos.
Home_Lat, 11, 7, 7), '\0');
Displ_Format_Data
(0, 2, "hAlt:", my_itoa
(MK_pos.
Home_Alt, 11, 3, 1), '\0');
// >> Menueauswahl nach oben
if (Get_Key_Press
( 1<<SW_PLUS
) || Get_Key_Press
( 1<<SW_MINUS
)) coldstart
= 1;
break;
case GPS_MISC
:
Displ_Format_Data
(0, 0, "U:", my_itoa
(mk_UBat
, 4, 1, 1), 'V');
Displ_Format_Data
(8, 0, "W:", my_itoa
(mk_dUsedCapacity
, 4, 0, 0), '\0');
Displ_Format_Data
(0, 1, "I:", my_itoa
(mk_Ikorr
, 4, 1, 1), 'A');
if (mk_current.
Count > 0) {
dDCurrent
= mk_current.
Sum / mk_current.
Count;
if (dDCurrent
> 0) {
time_remaining
= ((int32_t)mk_lipo.
Capacity - mk_dUsedCapacity
) * 36 / dDCurrent
;
if (abs(time_remaining
) > M59S59
)
time_remaining
= time_remaining
< 0? -M59S59
: M59S59
; // M59S59 sind 59 Minuten und 59 Sekunden
}
}
Displ_Format_Data
(8, 1, "\xee:", my_itoa
(dDCurrent
, 4, 1, 1), 'A');// Durchschnittszeichen
lcdGotoXY
(0,2);
lcdPutc
('\x1a'); // Summenzeichen
Displ_TimeMS
(mk_lipo.
time_on / T2SECDIV
);
lcdGotoXY
(8,2);
lcdPuts
("R");
Displ_TimeMS
(time_remaining
);
break;
}
}
Displ_wiRX
();
}
void do_tracking
(void)
{ static uint8_t hysteresis
= 0;
geo
= calc_geo
(&MK_pos
, ¤tPos
);
angleTilt
= my_atan2
((currentPos.
Altitude - MK_pos.
Home_Alt) / 100, geo.
distance);
if (geo.
distance < 40 || (geo.
distance < 60 && hysteresis
)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschlägt
geo.
bearing = MK_pos.
direction;
if (currentPos.
Altitude - MK_pos.
Home_Alt < 4000) angleTilt
= 0; // man fliegt nicht direkt über Kopf
hysteresis
= 1;
}
else {
hysteresis
= 0;
}
// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen
// 360 grd negative Winkelwerte als positive
anglePan
= (geo.
bearing + 450 - MK_pos.
direction) % 360; // 450 = 360 + 90
if (angleTilt
< 0) angleTilt
= 0;
if (angleTilt
> 180) angleTilt
= 180;
if (anglePan
>= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
anglePan
= anglePan
- 180;
angleTilt
= 180 - angleTilt
;
}
servoSetAngle
(0, anglePan
);
servoSetAngle
(1, angleTilt
);
}
/**************************************************************/
/* */
/* MK GPS Tracking */
/* */
/**************************************************************/
// MK OSD-Daten lesen und verifizieren
uint8_t OSD_Data_valid
(NaviData_t
**navi_data
)
{ 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
currentPos
= (*navi_data
)->CurrentPosition
;
if ((*navi_data
)->NCFlags
& NC_FLAG_GPS_OK
)
ret
= 1;
// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
// (4 * (........))/5 ==> Wichtung Luftdruck-Höhe zu GPS
currentPos.
Altitude = MK_pos.
Home_Alt + (4000 * (int32_t)((*navi_data
)->Altimeter
) / AltFaktor
+ currentPos.
Altitude - MK_pos.
Home_Alt) / 5;
// currentPos.Altitude = MK_pos.Home_Alt + 1000 * (int32_t)(*navi_data)->Altimeter / AltFaktor;
MK_Motor_run
= (*navi_data
)->FCStatusFlags
& FC_FLAG_MOTOR_RUN
;
MK_ProcessLipo
(*navi_data
);
satsInUse
= (*navi_data
)->SatsInUse
;
}
else
// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
if ((track_tx
) && (rx_timeout
> RX_TIMEOUT
)) {
USART_send_Str
(tx_osd
); // 420 * 0.5ms interval
// tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval
if (rx_timeout
== RX_TIME_END
) store_LipoData
();// ca. 4 Sekunden nach Signalverlust
}
return ret
;
}
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel
// Hauptprogramm von GPS Antennen-Nachführung
void Tracking_GPS
(void)
{ NaviData_t
*navi_data
;
static uint8_t track_running
= 0;
if (!track_running
) {
track_running
= 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!!
if (OSD_Data_valid
(&navi_data
)) {
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 = navi_data
->HomePosition.
Longitude;
MK_pos.
Home_Lat = navi_data
->HomePosition.
Latitude;
MK_pos.
Home_Alt = navi_data
->HomePosition.
Altitude;
MK_pos.
direction = navi_data
->CompassHeading
;
coldstart
= 0;
}
}
else {
do_tracking
();
}
}
Displ_GPS
(); // letzte empfangene Daten auch bei ausgeschalteten MK sichtbar
track_running
= 0;
}
}
/**************************************************************/
/* */
/* NMEA GPS Tracking */
/* */
/**************************************************************/
void setNMEAdir
(void)
{
if (!coldstart
)
MK_pos.
direction = geo.
bearing;
}
// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps
int32_t getLatitude
(const char *s
, const char *NS
)
{ int32_t deg
= (s
[0] - '0') * 10 + s
[1] - '0'; // First 2 chars are full degrees
int32_t min
= floatStrToInt
(&s
[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps
deg
= deg
* 10000000 + min
;
if (*NS
== 'S') deg
= -deg
;
return deg
;
}
// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps
int32_t getLongitude
(const char *s
, const char *WE
)
{ int32_t deg
= ((s
[0] - '0') * 10 + s
[1] - '0') * 10 + s
[2] - '0'; // First 3 chars are full degrees
int32_t min
= floatStrToInt
(&s
[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps
deg
= deg
* 10000000 + min
;
if (*WE
== 'W') deg
= -deg
;
return deg
;
}
void Tracking_NMEA
(void)
{ char *token
;
int32_t latitude
, longitude
;
if (decodeNMEA
()) {
token
= strtok((char*)data_decode
, ",");
// http://de.wikipedia.org/wiki/NMEA_0183
if (!strcmp(token
, "GPGGA")) {
// $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,,
// Skip time
strtok(0, ",");
// Latitude
latitude
= getLatitude
(strtok(0, ","), strtok(0, ","));
// Longitude
longitude
= getLongitude
(strtok(0, ","), strtok(0, ","));
// Signal valid? (Position Fix Indicator)
if (*strtok(0, ",") != '0') {
// Satellites in use
satsInUse
= atoi(strtok(0, ","));
// Skip dilution
strtok(0, ",");
// Altitude
currentPos.
Altitude = floatStrToInt
(strtok(0, ","), 3);
currentPos.
Latitude = latitude
;
currentPos.
Longitude = longitude
;
if ((coldstart
) && (satsInUse
> 5)) {
// First position after reboot (or change of mode) will be the home position (facing north)
MK_pos.
Home_Lon = currentPos.
Longitude;
MK_pos.
Home_Lat = currentPos.
Latitude;
MK_pos.
Home_Alt = currentPos.
Altitude;
MK_pos.
direction = 0;
coldstart
= 0;
Double_Beep
(DBEEPNMEAFIX
, DBEEPMEAFIXP
);
}
do_tracking
();
}
}
}
Displ_GPS
(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar
}
/************************************************************************/
/* */
/* 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;
servoSetAngle
(data_decode
[1], ServoPos
);
}
}
}