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
#define AltFaktor 22.5
typedef struct {
double distance
;
double bearing
;
} geo_t
;
geo_t geo
;
int AngelPan
, AngelTilt
;
float Altitude
;
uint16_t dDCurrent
= 0;
uint16_t dUsedCapacity
= 0;
int32_t time_remaining
= 0;
uint16_t Ikorr
;
uint16_t Wkorr
;
/**************************************************************/
/* */
/* 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_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_Val_comma
(int32_t zahl
)
{
if (zahl
< 0)
lcdPuts
(my_itoa
(zahl
/ 10, 1, 2, 0, 0)); // Übergabe Integer, vorher *10 für eventuelle Kommastelle
else
if (zahl
< 100)
lcdPuts
(my_itoa
(zahl
, 0, 3, 1, 1));
else
lcdPuts
(my_itoa
(zahl
/ 10, 0, 3, 0, 0));
lcdPutc
('m');
}
// 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
);
}
void store_LipoData
(void)
{ uint32_t tmp_time_on
;
tmp_time_on
= eeprom_read_dword
(&ep_lipo
[akku_nr
].
time_on);
mk_timer
= 0;
if (tmp_time_on
!= lipo.
time_on) {
if (!MK_Motor_run
) lipo.
UsedCapacity = dUsedCapacity
;
eeprom_write_byte
(&ep_lipo
[akku_nr
].
Umk, lipo.
Umk);
eeprom_write_word
(&ep_lipo
[akku_nr
].
UsedCapacity, lipo.
UsedCapacity);
eeprom_write_dword
(&ep_lipo
[akku_nr
].
time_on, lipo.
time_on);
eeprom_write_block
(¤t
,&ep_current
,sizeof(current_t
));
Double_Beep
(DBEEPWR
, DBEEPWRP
);
}
}
#define I_MOTOR_OFF 6 // IR=280mA gemessen; IR vom MK Akku voll 500mA; I Motoren an 1,3A; Grenzwert für Erkennung Akkuwechsel zwischen 500 und 1200mA
void Misc
(void)
{
// Spannung, Strom, Leistung sofort (ohne Motorstart) lesen
if ((!MK_Motor_run
) && ((lipo.
Umk + 2) < MK.
navi_data.
UBat) && (MK.
navi_data.
Current <= I_MOTOR_OFF
)) { // Bei Ruhestrom kleiner 501mA Akku gewechselt? IR=280mA gemessen
lipo.
Umk = MK.
navi_data.
UBat;
mk_timer
= 0;
dUsedCapacity
= 0;
lipo.
UsedCapacity = 0;
lipo.
time_on = 0;
lcdClear
();
// Funktion wird innhalb Task ausgeführt, auch innerhalb eines anderen Menüpunkes oder Eingabeaufforderung
strcpy(tmp_pmenu
, pmenu
); // da bei Change_Value(...) ein Menüpunkt zurück
strcpy(pmenu
,"0");
Menu_MK_BatteryChangeNr
(); // eingeschobenes Menü, danach aber wieder zur ursprünglichen Anzeige!
strcpy(pmenu
, tmp_pmenu
); // Sonst müsste vorherige aktuelle LCD-Anzeige zwischengespeichert werden.
/* irgendeine Eingabe-While-Schleife (Menue, Change..) zum Verlassen zwingen und mit Jump_Menu(pmenu)
den gleichen Menüpunkt wieder aufrufen */
if (pmenu
[0] == '\0')
Displ_Main_Disp
();
else
exit_while
= 1; // wird nach den möglichen while, innerhalb der Fkt. wieder auf 0 gesetzt
store_LipoData
();
}
mk_timer
= 1; // MK-Timer auf on stellen
if ((!MK_Motor_run
) && (MK.
navi_data.
Current <= I_MOTOR_OFF
) && (MK.
navi_data.
UBat > 0) && (MK.
navi_data.
UBat < lipo.
Umk))
lipo.
Umk = MK.
navi_data.
UBat; // Bei Ruhestrom, da je nach Last UBat schwankend
/* if (current.Sum > 0xfffffff) { // wesentlich größerer Zeitraum für Mittelwertbildung
current.Sum /= 2;
current.Count /= 2;
} */
if (current.
Count > 18000) { // ungefär 30 Minuten Mittelwert
current.
Sum = (current.
Sum * 9)/10; // um 10% verkleinern
current.
Count = ((uint32_t)current.
Count * 9)/10;
}
Ikorr
= mk_i_offset
+ (uint32_t)(MK.
navi_data.
Current - MK_I_OFFSET_5
) * mk_i_faktor
/ 100;
current.
Sum += Ikorr
;
current.
Count++;
dDCurrent
= current.
Sum / current.
Count;
Wkorr
= (uint32_t)MK.
navi_data.
UsedCapacity * mk_w_faktor
/ 100;
dUsedCapacity
= lipo.
UsedCapacity + Wkorr
;
time_remaining
= ((int32_t)lipo.
Capacity - dUsedCapacity
) * 36 / dDCurrent
;
if (abs(time_remaining
) > M59S59
) {
if (time_remaining
< 0)
time_remaining
= -1 * M59S59
;
else
time_remaining
= M59S59
; // M59S59 sind 59 Minuten und 59 Sekunden
}
}
// 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
if (rx_timeout
< RX_TIME_OLD
) { // GPS-Daten nicht zu alt und ok.
if (MK.
navi_data.
NCFlags & NC_FLAG_GPS_OK
) {
ret
= 1;
MK_Motor_run
= MK.
navi_data.
FCStatusFlags & FC_FLAG_MOTOR_RUN
;
}
Misc
();
}
}
else
if (rx_timeout
== RX_TIME_END
) // ca. 4 Sekunden nach Signalverlust
store_LipoData
();
// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval
if ((track_tx
) && (rx_timeout
> RX_TIMEOUT
)) USART_send_Str
(tx_osd
); // 420 * 0.5ms 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)
{ 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
()) {
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);
// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
// (4 * (........))/5 ==> Wichtung Luftdruck-Höhe zu GPS
Altitude
= (4 * (MK.
navi_data.
Altimeter/AltFaktor
) + ((float)(MK.
navi_data.
CurrentPosition.
Altitude - MK_pos.
Home_Alt) / 1000)) /5;
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 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
) || (gps_display
== GPS_MISC
)) && (bat_low
!= 0)) {
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, 0, 3, 0, 0), '\xdf'); // '\xdf' entspricht '°'
lcdGotoXY
(8,0);
lcdPuts
("Dis:");
Displ_Val_comma
(geo.
distance * 10);
Displ_Format_Data
(0, 1, "Bea:", my_itoa
((uint32_t)(geo.
bearing), 0, 3, 0, 0), '\xdf');
lcdGotoXY
(8,1);
lcdPuts
("Alt:");
Displ_Val_comma
(Altitude
* 10);
Displ_Format_Data
(0, 2, "Pan:", my_itoa
(AngelPan
, 0, 3, 0, 0), '\xdf');
Displ_Format_Data
(8, 2, "Til:", my_itoa
(AngelTilt
, 0, 3, 0, 0), '\xdf');
break;
case GPS_DISP_CURRENT
:
Displ_Format_Data
(0, 0, "aLon:", my_itoa
(MK.
navi_data.
CurrentPosition.
Longitude, 0, 11, 7, 7), '\0');
Displ_Format_Data
(0, 1, "aLat:", my_itoa
(MK.
navi_data.
CurrentPosition.
Latitude, 0, 11, 7, 7), '\0');
Displ_Format_Data
(0, 2, "aAlt: ", my_itoa
(MK.
navi_data.
CurrentPosition.
Altitude, 1, 11, 3, 1), '\0');
break;
case GPS_DISP_HOME
:
Displ_Format_Data
(0, 0, "hLon:", my_itoa
(MK_pos.
Home_Lon_7, 0, 11, 7, 7), '\0');
Displ_Format_Data
(0, 1, "hLat:", my_itoa
(MK_pos.
Home_Lat_7, 0, 11, 7, 7), '\0');
Displ_Format_Data
(0, 2, "hAlt: ", my_itoa
(MK_pos.
Home_Alt, 1, 11, 3, 1), '\0');
break;
case GPS_MISC
:
Displ_Format_Data
(0, 0, "U:", my_itoa
(MK.
navi_data.
UBat, 0, 4, 1, 1), 'V');
Displ_Format_Data
(8, 0, "W:", my_itoa
(dUsedCapacity
, 0, 4, 0, 0), '\0');
Displ_Format_Data
(0, 1, "I:", my_itoa
(Ikorr
, 0, 4, 1, 1), 'A');
Displ_Format_Data
(8, 1, "\xee:", my_itoa
(dDCurrent
, 0, 4, 1, 1), 'A');
lcdGotoXY
(0,2);
lcdPuts
("\x1a");
Displ_TimeMS
(lipo.
time_on / T2SECDIV
);
lcdGotoXY
(8,2);
lcdPuts
("R");
Displ_TimeMS
(time_remaining
);
}
}
Displ_wiRX
();
track_running
= 0;
}
}
/************************************************************************/
/* */
/* 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));
servo_track
(MK.
data_decode[1], ServoPos
);
}
}
}