Subversion Repositories Projects

/****************************************************************/

/*                                                                                                                              */

/*                               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(&current,&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);

        }

  }

}