Subversion Repositories Projects

Rev

Rev 1465 | Blame | Compare with Previous | 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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <util/delay.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"

#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

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;
int                             anglePan, angleTilt;

/**************************************************************/
/*                                                                                                                                                                                                                                              */
/*                                                                                      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                                                                                                    */
/*                                                                                                                                                                                                                                              */
/**************************************************************/

// 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(':');
                wi232RX = 0;
                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)
{ double lat1, lon1, lat2, lon2, d1, dlat;
        geo_t geo;
       
        lon1 = home->Home_Lon;
        lat1 = home->Home_Lat;
        lon2 = (double)pos->Longitude   / 10000000.0;
        lat2 = (double)pos->Latitude    / 10000000.0;

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

// 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, 0, 0), '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_Lon7, 11, 7, 7), '\0');
                                Displ_Format_Data(0, 1, "hLat:", my_itoa(MK_pos.Home_Lat7, 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, &currentPos);
        angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance);
        if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) {             // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschlägt
                geo.bearing = MK_pos.direction;
                angleTilt = 0;
                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 = fmod(geo.bearing - MK_pos.direction + 360 + 90, 360);
        if (anglePan > 360) anglePan -= 360;

        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
                if (rx_timeout < RX_TIME_OLD) {                                                         // GPS-Daten nicht zu alt und ok.
                        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;                   
                        MK_Motor_run = (*navi_data)->FCStatusFlags & FC_FLAG_MOTOR_RUN;
                        MK_ProcessLipo(*navi_data);
                        satsInUse = (*navi_data)->SatsInUse;
                }
        }
        else
                if (rx_timeout == RX_TIME_END) store_LipoData();// ca. 4 Sekunden nach Signalverlust
        // 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)
{       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 = (double)navi_data->HomePosition.Longitude / 10000000.0;
                                        MK_pos.Home_Lat = (double)navi_data->HomePosition.Latitude       / 10000000.0;
                                        MK_pos.Home_Lon7 = navi_data->HomePosition.Longitude;
                                        MK_pos.Home_Lat7 = 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 = (double)currentPos.Longitude  / 10000000.0;
                                        MK_pos.Home_Lat = (double)currentPos.Latitude           / 10000000.0;
                                        MK_pos.Home_Lon7 = currentPos.Longitude;
                                        MK_pos.Home_Lat7 = 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);
                }
        }
}