| 0,0 → 1,398 |
|---|
| |
| /****************************************************************/ |
| /* */ |
| /* 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); |
| } |
| } |
| } |