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


typedef struct {
  double distance;
  double bearing;
} geo_t;

geo_t geo;
int AngelPan, AngelTilt;
float Altitude;
 
/**************************************************************/
/*                                                                                                                        */
/*                      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_GPS_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);
}

// 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);
}

// 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
    // GPS-Daten nicht zu alt und ok.
        if ((rx_timeout < RX_TIME_OLD) && (MK.navi_data.NCFlags & NC_FLAG_GPS_OK)) {
      ret = 1;
          MK_Motor_run = MK.navi_data.FCStatusFlags & FC_FLAG_MOTOR_RUN;
        }
  }
  // ca. 150ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
//  if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 10 * 10ms interval
  if ((track_tx) && (rx_timeout > RX_TIMEOUT)) USART_send_Str(tx_osd); // 10 * 10ms 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)
{  
  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);
          Altitude = (float)(MK.navi_data.CurrentPosition.Altitude - MK_pos.Home_Alt) / 1000;
          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 berechnet 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) && (bat_low != 0)) {
    if (gps_disp_clear) {
          gps_disp_clear = 0;
          lcdClear();
        }
        // makefile ohne! Minimalistic printf version
        switch(gps_display) {
          case GPS_DISP_CALC:
            Displ_GPS_Data(0, 0, "Dir:", my_itoa(MK_pos.direction, 0, 3, 0, 0), '\xdf');
        if (geo.distance < 10)  // Anzeige mit einer Kommastelle
              Displ_GPS_Data(8, 0, "Dis:", my_itoa((uint32_t)(geo.distance * 10), 0, 3, 1, 1), 'm');
                else                                    // Anzeige ohne Kommastelle bis 999m, Überlauf nicht abgefangen!
              Displ_GPS_Data(8, 0, "Dis:", my_itoa((uint32_t)geo.distance, 0, 3, 0, 0), 'm');
            Displ_GPS_Data(0, 1, "Bea:", my_itoa((uint32_t)(geo.bearing), 0, 3, 0, 0), '\xdf');
                Displ_GPS_Data(8 ,1, "Alt:", my_itoa((uint32_t)(Altitude), 1, 2, 0, 0), 'm');
                Displ_GPS_Data(0, 2, "Pan:", my_itoa(AngelPan, 0, 3, 0, 0), '\xdf');
            Displ_GPS_Data(8, 2, "Til:", my_itoa(AngelTilt, 0, 3, 0, 0), '\xdf');
                break;
          case GPS_DISP_CURRENT:
            Displ_GPS_Data(0, 0, "aLon:", my_itoa(MK.navi_data.CurrentPosition.Longitude, 0, 11, 7, 7), '\0');
            Displ_GPS_Data(0, 1, "aLat:", my_itoa(MK.navi_data.CurrentPosition.Latitude, 0, 11, 7, 7), '\0');
            Displ_GPS_Data(0, 2, "aAlt: ", my_itoa(MK.navi_data.CurrentPosition.Altitude, 1, 11, 3, 1), '\0');
                break;
          case GPS_DISP_HOME:
            Displ_GPS_Data(0, 0, "hLon:", my_itoa(MK_pos.Home_Lon_7, 0, 11, 7, 7), '\0');
            Displ_GPS_Data(0, 1, "hLat:", my_itoa(MK_pos.Home_Lat_7, 0, 11, 7, 7), '\0');
            Displ_GPS_Data(0, 2, "hAlt: ", my_itoa(MK_pos.Home_Alt, 1, 11, 3, 1), '\0');
    }
  }
  Displ_wiRX();
}

/************************************************************************/
/*                                                                                                                                              */
/*                      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));
        }
  }
}