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