WebSVN - Projects - Diff - Rev 2147 and 2192 - /Transportables


/*
 * FollowMe.c
 *
 *  Created on: 18.05.2012
 *      Author: cebra
 */
/*****************************************************************************
 *   Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
 *                                                                           *
 *   This program is free software; you can redistribute it and/or modify    *
 *   it under the terms of the GNU General Public License as published by    *
 *   the Free Software Foundation; either version 2 of the License.          *
 *                                                                           *
 *   This program is distributed in the hope that it will be useful,         *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
 *   GNU General Public License for more details.                            *
 *                                                                           *
 *   You should have received a copy of the GNU General Public License       *
 *   along with this program; if not, write to the                           *
 *   Free Software Foundation, Inc.,                                         *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
 *                                                                           *
 *                                                                           *
 *   Credits to:                                                             *
 *   Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN  *
 *                          http://www.mikrokopter.de                        *
 *   Gregor "killagreg" Stobrawa for his version of the MK code              *
 *   Thomas Kaiser "thkais" for the original project. See                    *
 *                          http://www.ft-fanpage.de/mikrokopter/            *
 *                          http://forum.mikrokopter.de/topic-4061-1.html    *
 *   Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
 *                          http://www.mylifesucks.de/oss/c-osd/             *
 *   Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
 *****************************************************************************/
//############################################################################
//# HISTORY  followme.c
//#
//# 03.08.2015 Cebra
//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe
//#
//# 20.07.2015 CB
//# - chg: FollowMe Datensatz an NC.211 angepasst
//#
//# 27.06.2014 OG
//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX
//#
//# 26.06.2014 OG
//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h)
//#
//# 24.06.2014 OG
//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData()
//#
//# 22.06.2014 OG
//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c)
//# - del: Variable CheckGPS
//#
//# 21.06.2014 OG
//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der
//#        Entfernung zwischen Kopter und Target
//# - chg: MK-Timeoutout Erkennung verbessert/angepasst
//#
//# 19.06.2014 OG
//# - erster Prototyp der Follow Me zum Kopter sendet
//# - etliche Aenderungen im Codeaufbau
//#
//# 01.06.2014 OG
//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und
//#        als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens
//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das
//#        Hauptmenue erledigt wird
//#
//# 13.05.2014 OG
//# - chg: FollowMe() - Variable 'flag' auskommentiert
//#        wegen Warning: variable set but not used
//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert
//#        wegen Warning: variable set but not used
//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb
//#        auskommentiert weil man das a) vorallem nicht benoetigt
//#        und b) die load_setting() so nicht mehr existiert
//#        (der Codebereich kann meines erachtens geloescht werden)
//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es
//#        das a) nicht mehr gibt und b) hier gar nicht benoetigt wird
//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt)
//#        und erstmal durch ein PKT_Message_P() ersetzt
//#        * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio)
//# - add: #include "../pkt/pkt.h"
//#
//# 05.05.2013 Cebra
//# - chg: #ifdef USE_FOLLOWME
//#
//############################################################################
 
#include "../cpu.h"
#include <avr/io.h>
#include <inttypes.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
 
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
 
#include "../main.h"
 
#ifdef USE_FOLLOWME
 
#include "../followme/followme.h"
#include "../osd/osd.h"
#include "../lcd/lcd.h"
#include "../timer/timer.h"
#include "../uart/usart.h"
#include "../eeprom/eeprom.h"
#include "../messages.h"
#include "../bluetooth/bluetooth.h"
#include "../setup/setup.h"
#include "../uart/uart1.h"
#include "../mk-data-structs.h"
#include "../pkt/pkt.h"
#include "../gps/gps.h"
//#include "../gps/gps_nmea.h"
#include "../avr-nmea-gps-library/nmea.h"
#include "../gps/gpsmouse.h"
 
 
//#######################################################################################################################
 
 
//--------------------
// Timings
//--------------------
//#define MK_TIMEOUT          300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec)
#define MK_TIMEOUT            400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec)
 
 
 
//--------------------
#define COSD_WASFLYING 4
 
// global definitions and global vars
NaviData_t *naviData;
unsigned char Element;
uint16_t heading_home;
 
// Hier Höhenanzeigefehler Korrigieren
#define AltimeterAdjust 1.5
 
 
 
// Flags
//uint8_t COSD_FLAGS2 = 0;
//
//GPS_Pos_t last5pos[7];
uint8_t FM_error = 0;
 
 
//---------------------
// Waypoint Types
// TODO OG: verschieben nach: mk-data-structs.h
//---------------------
#define POINT_TYPE_INVALID  255
#define POINT_TYPE_WP       0
#define POINT_TYPE_POI      1
 
 
//---------------------
// Status
// GPS_Pos_t
// aus MK source
//
// TODO OG: verschieben nach: mk-data-structs.h
//---------------------
#define INVALID         0x00
#define NEWDATA         0x01
#define PROCESSED       0x02
 
 
 
//--------------------------------------------------------------
//--------------------------------------------------------------
void FollowMe( void )
{
    //uint8_t status;
    GPS_Pos_t currpos;
    uint8_t   tmp_dat;
    uint8_t   redraw;
    uint8_t   drawmode;
    uint32_t  NMEA_GPGGA_counter_old;                                   // Merker: zaehlt empfangene GPGGA-Pakete
    uint32_t  send_followme_counter;
 
    int8_t    ok;
    int8_t    xoffs;
    int8_t    yoffs;
    Point_t   FollowMe;
    uint8_t   mktimeout = false;
 
 
 
    GPS_PosDev_t targetdev;
 
 
    //---------------------
    // 1. Daten GPS-Maus
    //---------------------
    ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 );            // 500 = 5 Sekunden Verzoegerung nach Satfix
    if( ok <= 0 )
    {
        return;                                                         // Fehler bzgl. BT GPS-Maus -> exit
    }
 
 
 
    //---------------------
    // 2. Follow Me
    //---------------------
    set_beep( 25, 0xffff, BeepNormal );                                 // kurzer Bestaetigungs-Beep
 
    lcd_cls ();
 
    SwitchToNC();
 
    mode = 'O';
 
    // disable debug...
    //      RS232_request_mk_data (0, 'd', 0);
    tmp_dat = 0;
    SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
 
    // request OSD Data from NC every 100ms
    //      RS232_request_mk_data (1, 'o', 100);
    tmp_dat = 10;
    SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
 
    //OSD_active = true;              // benötigt für Navidata Ausgabe an SV2
 
    //-------------------------
    // Init: Timer & Flags
    //-------------------------
    timer_mk_timeout      = MK_TIMEOUT;
 
    abo_timer = ABO_TIMEOUT;
 
 
    MKLiPoCells_Init();
 
    redraw = true;
 
    NMEA.Counter           = 0;
    NMEA_GPGGA_counter_old = 0;
    send_followme_counter  = 0;
 
 
    while( (receiveNMEA) )
    {
        //-----------------------------------------
        // Screen redraw
        //-----------------------------------------
        if( redraw )
        {
            lcd_cls();
 
            lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") );  // Titel: oben, mitte
 
            lcd_line(  (6*6-3),  0,  (6*6-3), 11, 1);                   // Linie Vertikal links
            lcd_line( (15*6+5),  0, (15*6+5), 11, 1);                   // Linie Vertikal rechts
            lcd_line(        0, 12,      127, 12, 1);                   // Linie Horizontal
 
          //lcd_line(        0, 39,      127, 39, 1);                   // Linie Horizontal Mitte
            lcd_line(        66, 39,      127, 39, 1);                  // Linie Horizontal Mitte
 
            lcd_rect_round( 0, 33, 66, 12, 1, R1);                      // Rahmen fuer "Di" (Distance)
 
            lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:")  );      // Label: "Al:"
 
            draw_icon_mk( 1, 18);
            draw_icon_target_round( 1, 50);
 
            redraw = false;
        }
 
 
 
        //-----------------------------------------
        // neue MK Daten vorhanden
        //-----------------------------------------
        if( rxd_buffer_locked )     // neue MK Daten
        {
            Decode64 ();
            naviData = (NaviData_t *) pRxData;
 
            if( mktimeout ) redraw = true;
            mktimeout = false;
 
            FM_error = 0;   // noch benoetigt?
 
            currpos.Latitude  = naviData->CurrentPosition.Latitude;
            currpos.Longitude = naviData->CurrentPosition.Longitude;
 
            //----------------------------------
            // speichere letzte GPS-Positionen
            //----------------------------------
            Config.LastLatitude  = naviData->CurrentPosition.Latitude;    // speichere letzte Position in Config
            Config.LastLongitude = naviData->CurrentPosition.Longitude;   // speichere letzte Position in Config
 
 
            //----------------------------------
            // LiPo Cell Check
            //----------------------------------
            MKLiPoCells_Check();                    // ggf. Zellenzahl ermitteln
 
 
            //#################
            //# MK
            //#################
 
            //-----------------
            // Oben: MK-Batt Level (Volt)
            //-----------------
            OSD_Element_BattLevel2( 0, 0, 0,0 );
 
            //-----------------
            // Oben: Batt Level Bar
            //-----------------
            OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H);
 
            //-----------------
            // Oben: MK-Flugzeit
            //-----------------
            writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0);
 
            //-----------------
            // Hoehe
            //-----------------
            xoffs = 3;
            yoffs = -1;
            writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs );
 
            //-----------------
            // MK: Sat Anzahl
            //-----------------
            yoffs = -27;
            if( naviData->SatsInUse > 5 )   drawmode = MNORMALX;
            else                            drawmode = MINVERSX;
            writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode,  3,2+yoffs);
            draw_icon_satmini( 116+3, 42+yoffs);
 
 
            //-----------------
            // MK: GPS
            //-----------------
            writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 );                 // Line 4 L: Latitude
            writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 );                 // Line 4 R: Longitude
//             lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude);
//             lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude);
 
 
 
            //#################
            //# DISTANCE TO TARGET
            //#################
 
            //-----------------
            // Target: Distance to Target
            //-----------------
            xoffs = 7;
            yoffs = 4;
            //GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 )
            targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition);
            lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) );
 
 
            //#################
            //# TARGET (GPS Maus)
            //#################
 
            yoffs = 8;
 
            //-----------------
            // Send Counter
            //-----------------
          //lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter );               // Anzahl gesendeter Target-Positionen
            lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter );        // Anzahl gesendeter Target-Positionen
 
 
            //-----------------
            // Target: Fix
            //-----------------
            if( NMEA.SatFix == 1 || NMEA.SatFix == 2 )
                drawmode = MNORMALX;
            else
                drawmode = MINVERSX;
 
            lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix );               // GPS-Maus: Satfix
 
 
            //-----------------
            // Target: Sat Anzahl
            //-----------------
            if( NMEA.SatsInUse > 5 )
                drawmode = MNORMALX;
            else
                drawmode = MINVERSX;
 
            writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode,  4,yoffs);
            draw_icon_satmini( 116+4, 40+yoffs);
 
 
            //-----------------
            // Target: Lat / Long
            //-----------------
            writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 );                 // GPS-Maus: Latitude
            writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 );                 // GPS-Maus: Longitude
 
 
            rxd_buffer_locked = FALSE;
 
            timer_mk_timeout = MK_TIMEOUT;
        }
 
        //-----------------------------------------
//        if( !NMEA_receiveBT() )
//        {
//            receiveNMEA = false;
//        }
//        else
//        {
//            NMEA_GetNewData();
//        }
 
 
 
// Sourcecode aus NaviCtrl/V2.10e/uart1.c
//  FOLLOW_ME
//        else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
//        {
//                if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
//                {
//                        TransmitAlsoToFC = 1;
//                        // update FollowMe content
//                        FollowMe.Position.Longitude     = GPSData.Position.Longitude;
//                        FollowMe.Position.Latitude      = GPSData.Position.Latitude;
//                        FollowMe.Position.Status = NEWDATA;
//                        FollowMe.Position.Altitude = 1;
//                        // 0  -> no Orientation
//                        // 1-360 -> CompassCourse Setpoint
//                        // -1 -> points to  WP1 -> itself
//                FollowMe.Heading = -1;
//                        FollowMe.ToleranceRadius = 1;
//                        FollowMe.HoldTime = 60;
//                        FollowMe.Event_Flag = 1;
//                        FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list
//                        FollowMe.Type = POINT_TYPE_WP;
//                        FollowMe.WP_EventChannelValue = 100;  // set servo value
//                        FollowMe.AltitudeRate = 0;                        // do not change height
//                        FollowMe.Speed = 0;                             // rate to change the Position (0 = max)
//                        FollowMe.CamAngle = 255;                // Camera servo angle in degree (255 -> POI-Automatic)
//                        FollowMe.Name[0] = 'F';             // Name of that point (ASCII)
//                        FollowMe.Name[1] = 'O';             // Name of that point (ASCII)
//                        FollowMe.Name[2] = 'L';             // Name of that point (ASCII)
//                        FollowMe.Name[3] = 'L';             // Name of that point (ASCII)
//                        FollowMe.reserve[0] = 0;                // reserve
//                        FollowMe.reserve[1] = 0;                // reserve
//                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
//                }
//                UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
//        }
//
 
 
 
 
 
        //-----------------------------------------
        // neue NMEA Daten?
        //-----------------------------------------
        if(NMEA_isdataready() && receiveNMEA)
        {
            if( NMEA.Counter > NMEA_GPGGA_counter_old )
            {
                if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) )
                {
                    //Config.FM_Refresh
 
                    FollowMe.Position.Status    = NEWDATA;
                    FollowMe.Position.Longitude = NMEA.Longitude;
                    FollowMe.Position.Latitude  = NMEA.Latitude;
                    FollowMe.Position.Altitude  = 1;                    // 20.7.2015 CB
    //                FollowMe.Position.Altitude  = NMEA.Altitude;        // ist das wirklich ok? NEIN C.B.
 
                    FollowMe.Heading = -1;                              // invalid heading
                    FollowMe.ToleranceRadius = Config.FM_Radius;        // 5 meter default
                    FollowMe.HoldTime = 60;                             // ????? go home after 60s without any update ??????
    //                FollowMe.Event_Flag = 0;                            // no event
                    FollowMe.Event_Flag = 1;                            //  20.7.2015 CB
                    FollowMe.Index = 1;                                 // 2st wp, 0 = Delete List, 1 place at first entry in the list
                    FollowMe.Type = POINT_TYPE_WP;                      // Typ des Wegpunktes
                    FollowMe.Name[0] = 'F';                             // Name des Wegpunktes (ASCII)
                    FollowMe.Name[1] = 'O';
                    FollowMe.Name[2] = 'L';
                    FollowMe.Name[3] = 'L';
    //                FollowMe.WP_EventChannelValue = 0;                  // Will be transferred to the FC and can be used as Poti value there
                    FollowMe.WP_EventChannelValue = 100;                 // set servo value 20.7.2015
                    FollowMe.AltitudeRate = 0;                          // rate to change the Aetpoint
                    FollowMe.Speed = Config.FM_Speed;                   // rate to change the Position
                    FollowMe.CamAngle = 255;                            // Camera servo angle in degree (255 -> POI-Automatic)
                    FollowMe.reserve[0] = 0;                            // reserve
                    FollowMe.reserve[1] = 0;                            // reserve
 
                    SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) );       //'s' = target Position  'w' = Waypoint
                    send_followme_counter++;
 
                    //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
                    // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
                    //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe));   //'s' = target Position  'w' = Waypoint
 
                }
 
                NMEA_GPGGA_counter_old = NMEA.Counter;
            }
        }
 
        //-----------------------------------------
        // TASTEN
        //-----------------------------------------
        if( get_key_press(1 << KEY_ESC) )
        {
            break;
        }
 
        if( get_key_press(1 << KEY_ENTER) )
        {
            break;
        }
 
 
        //-----------------------------------------
        //-----------------------------------------
        if( !abo_timer )
        {
            // renew abo every 3 sec
            // request OSD Data from NC every 100ms
            //      RS232_request_mk_data (1, 'o', 100);
            tmp_dat = 10;
            SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1);
 
            abo_timer = ABO_TIMEOUT;
        }
 
 
 
        //--------------------------
        // Daten Timeout vom MK?
        //--------------------------
        if( timer_mk_timeout == 0 )
        {
            if( !mktimeout ) OSD_MK_ShowTimeout();              // nur anzeigen wenn noch nicht im mktimeout-Modus
            set_beep ( 200, 0x0080, BeepNormal);                // Beep
            mktimeout        = true;
            timer_mk_timeout = MK_TIMEOUT;
            //OSDScreenRefresh = OSD_SCREEN_REDRAW;
            // OSD_MK_Connect( MK_CONNECT );
        }
 
    } // end: while( (receiveNMEA) );
 
 
 
    //---------------------
    // BEENDEN
    //---------------------
    OSD_active = false;
 
 
    //---------------------
    // GPS beenden
    //---------------------
    GPSMouse_Disconnect();
}
 
 
#ifdef DEBUG_GPS
 
 
 
void Debug_GPS (void)
 
{
 
uint8_t   redraw;
 
 
double l1;
GPS_Pos_t currpos;
 
set_beep( 25, 0xffff, BeepNormal );                                 // kurzer Bestaetigungs-Beep
redraw = true;
uint8_t mktimeout = false;
 
uint32_t  NMEA_GPGGA_counter_old;                                   // Merker: zaehlt empfangene GPGGA-Pakete
uint32_t  send_followme_counter;
Point_t   FollowMe;
 
 
while( true )
{
 
//######## Quell Koordinaten ##############################
                                                        //#
//        NMEApos.lat = 0x0FB51D1F;                         //#
//        NMEApos.lon = 0xE2CF4105;                          //#
        //#
//        NMEApos.lat = 0x1f1db5fb;                         //#
//        NMEApos.lon = 0x0541cfe2;                          //#
 
//        l1 = NMEApos.lat;
                                                        //#
//#########################################################
 
        int ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 );            // 500 = 5 Sekunden Verzoegerung nach Satfix
 
        if( ok <= 0 )
        {
                return;                                                         // Fehler bzgl. BT GPS-Maus -> exit
    }
 
        if(NMEA_isdataready() && receiveNMEA)
        {
                if( NMEA.Counter > NMEA_GPGGA_counter_old )
                {
                        if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) )
                        {
 
                                NMEApos.lat = NMEA.Latitude;
                                NMEApos.lon = NMEA.Longitude;
                                //Config.FM_Refresh
/*
                                FollowMe.Position.Status    = NEWDATA;
                                FollowMe.Position.Longitude = NMEA.Longitude;
                                FollowMe.Position.Latitude  = NMEA.Latitude;
                                FollowMe.Position.Altitude  = 1;                    // 20.7.2015 CB
//                FollowMe.Position.Altitude  = NMEA.Altitude;        // ist das wirklich ok? NEIN C.B.
 
                                FollowMe.Heading = -1;                              // invalid heading
                                FollowMe.ToleranceRadius = Config.FM_Radius;        // 5 meter default
                                FollowMe.HoldTime = 60;                             // ????? go home after 60s without any update ??????
//                FollowMe.Event_Flag = 0;                            // no event
                                FollowMe.Event_Flag = 1;                            //  20.7.2015 CB
                                FollowMe.Index = 1;                                 // 2st wp, 0 = Delete List, 1 place at first entry in the list
                                FollowMe.Type = POINT_TYPE_WP;                      // Typ des Wegpunktes
                                FollowMe.Name[0] = 'F';                             // Name des Wegpunktes (ASCII)
                                FollowMe.Name[1] = 'O';
                                FollowMe.Name[2] = 'L';
                                FollowMe.Name[3] = 'L';
//                FollowMe.WP_EventChannelValue = 0;                  // Will be transferred to the FC and can be used as Poti value there
                                FollowMe.WP_EventChannelValue = 100;                 // set servo value 20.7.2015
                                FollowMe.AltitudeRate = 0;                          // rate to change the Aetpoint
                                FollowMe.Speed = Config.FM_Speed;                   // rate to change the Position
                                FollowMe.CamAngle = 255;                            // Camera servo angle in degree (255 -> POI-Automatic)
                                FollowMe.reserve[0] = 0;                            // reserve
                                FollowMe.reserve[1] = 0;                            // reserve
 
                                SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) );       //'s' = target Position  'w' = Waypoint
                                send_followme_counter++;
 
                                //void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
                                // SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
                                //SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe));   //'s' = target Position  'w' = Waypoint
*/
                        }
 
                        NMEA_GPGGA_counter_old = NMEA.Counter;
                }
 
    //-----------------------------------------
    // Screen redraw
    //-----------------------------------------
    if( redraw )
    {
        lcd_cls();
 
        lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("GPS Test") );  // Titel: oben, mitte
        lcdx_printf_center_P( 4, MNORMAL, 1,0, PSTR(" Source Lat/Lon") );
        lcdx_printf_center_P( 6, MNORMAL, 1,0, PSTR(" Target Lat/Lon") );
 
 
//        sprintf(printbuff, "\r\nLat_Source:  %ld", NMEA.Latitude);
//        uart1_puts(printbuff);
//
//        sprintf(printbuff, "\r\nLon_Source:  %ld", NMEA.Longitude);
//        uart1_puts(printbuff);
//
//
//        writex_gpspos( 1, 6, NMEA.Latitude , MNORMAL, 0,6 );                                     // unten links: Latitude
//        writex_gpspos(12, 6, NMEA.Longitude, MNORMAL, 0,6 );
 
        redraw = false;
    }
 
        //#################
        //# DISTANCE TO TARGET
        //#################
 
        lcdx_printf_at_P( 0, 1, MNORMAL,0,0, PSTR("Distance: %3d Meter"), Config.FM_Distance );
 
        //#################
        //# TARGET Azimuth
        //#################
        lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR(" Azimuth: %3d Grad"), Config.FM_Azimuth);
 
 
 
 
        //-----------------
        // Original: Lat / Long
        //-----------------
        writex_gpspos( 1, 5, NMEApos.lat , MNORMAL,0,0 );                // GPS-Maus: Latitude
        writex_gpspos(10, 5, NMEApos.lon, MNORMAL, 0,0 );                // GPS-Maus: Longitude
 
 
//############################## Test GPS Offset
 
 
        //nmea_move_horz(&NMEApos,&NMEATarget, Config.FM_Azimuth, Config.FM_Distance/1000);                  // neues Ziel berechnen
 
        followme_calculate_offset(&NMEApos, &NMEATarget, 1, 1);
 
        writex_gpspos( 1, 7, (int32_t)NMEATarget.lat , MNORMAL,0,0 );   //  Ziel Latitude
        writex_gpspos(10, 7, (int32_t)NMEATarget.lon , MNORMAL, 0,0 );  //  Ziel Longitude
 
 
 
//############################## End Test GPS Offset
 
 
 
        //-----------------------------------------
        // TASTEN
        //-----------------------------------------
        if( get_key_press(1 << KEY_ESC) )
        {
            break;
        }
 
        if( get_key_press(1 << KEY_ENTER) )
        {
          redraw = true;
        //    break;
        }
        }
    }
}
 
 
 
 
#endif
 
#endif  // #ifdef USE_FOLLOWME
 
 
 

Rev 2147	Rev 2192
1	/*	1	/*
2	* FollowMe.c	2	* FollowMe.c
3	*	3	*
4	* Created on: 18.05.2012	4	* Created on: 18.05.2012
5	* Author: cebra	5	* Author: cebra
6	*/	6	*/
7	/*****************************************************************************	7	/*****************************************************************************
8	* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *	8	* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
9	* *	9	* *
10	* This program is free software; you can redistribute it and/or modify *	10	* This program is free software; you can redistribute it and/or modify *
11	* it under the terms of the GNU General Public License as published by *	11	* it under the terms of the GNU General Public License as published by *
12	* the Free Software Foundation; either version 2 of the License. *	12	* the Free Software Foundation; either version 2 of the License. *
13	* *	13	* *
14	* This program is distributed in the hope that it will be useful, *	14	* This program is distributed in the hope that it will be useful, *
15	* but WITHOUT ANY WARRANTY; without even the implied warranty of *	15	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
16	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *	16	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17	* GNU General Public License for more details. *	17	* GNU General Public License for more details. *
18	* *	18	* *
19	* You should have received a copy of the GNU General Public License *	19	* You should have received a copy of the GNU General Public License *
20	* along with this program; if not, write to the *	20	* along with this program; if not, write to the *
21	* Free Software Foundation, Inc., *	21	* Free Software Foundation, Inc., *
22	* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *	22	* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
23	* *	23	* *
24	* *	24	* *
25	* Credits to: *	25	* Credits to: *
26	* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *	26	* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
27	* http://www.mikrokopter.de *	27	* http://www.mikrokopter.de *
28	* Gregor "killagreg" Stobrawa for his version of the MK code *	28	* Gregor "killagreg" Stobrawa for his version of the MK code *
29	* Thomas Kaiser "thkais" for the original project. See *	29	* Thomas Kaiser "thkais" for the original project. See *
30	* http://www.ft-fanpage.de/mikrokopter/ *	30	* http://www.ft-fanpage.de/mikrokopter/ *
31	* http://forum.mikrokopter.de/topic-4061-1.html *	31	* http://forum.mikrokopter.de/topic-4061-1.html *
32	* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *	32	* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
33	* http://www.mylifesucks.de/oss/c-osd/ *	33	* http://www.mylifesucks.de/oss/c-osd/ *
34	* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*	34	* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
35	*****************************************************************************/	35	*****************************************************************************/
36	//############################################################################	36	//############################################################################
37	//# HISTORY followme.c	37	//# HISTORY followme.c
38	//#	38	//#
39	//# 03.08.2015 Cebra	39	//# 03.08.2015 Cebra
40	//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe	40	//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe
41	//#	41	//#
42	//# 20.07.2015 CB	42	//# 20.07.2015 CB
43	//# - chg: FollowMe Datensatz an NC.211 angepasst	43	//# - chg: FollowMe Datensatz an NC.211 angepasst
44	//#	44	//#
45	//# 27.06.2014 OG	45	//# 27.06.2014 OG
46	//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX	46	//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX
47	//#	47	//#
48	//# 26.06.2014 OG	48	//# 26.06.2014 OG
49	//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h)	49	//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h)
50	//#	50	//#
51	//# 24.06.2014 OG	51	//# 24.06.2014 OG
52	//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData()	52	//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData()
53	//#	53	//#
54	//# 22.06.2014 OG	54	//# 22.06.2014 OG
55	//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c)	55	//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c)
56	//# - del: Variable CheckGPS	56	//# - del: Variable CheckGPS
57	//#	57	//#
58	//# 21.06.2014 OG	58	//# 21.06.2014 OG
59	//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der	59	//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der
60	//# Entfernung zwischen Kopter und Target	60	//# Entfernung zwischen Kopter und Target
61	//# - chg: MK-Timeoutout Erkennung verbessert/angepasst	61	//# - chg: MK-Timeoutout Erkennung verbessert/angepasst
62	//#	62	//#
63	//# 19.06.2014 OG	63	//# 19.06.2014 OG
64	//# - erster Prototyp der Follow Me zum Kopter sendet	64	//# - erster Prototyp der Follow Me zum Kopter sendet
65	//# - etliche Aenderungen im Codeaufbau	65	//# - etliche Aenderungen im Codeaufbau
66	//#	66	//#
67	//# 01.06.2014 OG	67	//# 01.06.2014 OG
68	//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und	68	//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und
69	//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens	69	//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens
70	//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das	70	//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das
71	//# Hauptmenue erledigt wird	71	//# Hauptmenue erledigt wird
72	//#	72	//#
73	//# 13.05.2014 OG	73	//# 13.05.2014 OG
74	//# - chg: FollowMe() - Variable 'flag' auskommentiert	74	//# - chg: FollowMe() - Variable 'flag' auskommentiert
75	//# wegen Warning: variable set but not used	75	//# wegen Warning: variable set but not used
76	//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert	76	//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert
77	//# wegen Warning: variable set but not used	77	//# wegen Warning: variable set but not used
78	//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb	78	//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb
79	//# auskommentiert weil man das a) vorallem nicht benoetigt	79	//# auskommentiert weil man das a) vorallem nicht benoetigt
80	//# und b) die load_setting() so nicht mehr existiert	80	//# und b) die load_setting() so nicht mehr existiert
81	//# (der Codebereich kann meines erachtens geloescht werden)	81	//# (der Codebereich kann meines erachtens geloescht werden)
82	//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es	82	//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es
83	//# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird	83	//# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird
84	//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt)	84	//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt)
85	//# und erstmal durch ein PKT_Message_P() ersetzt	85	//# und erstmal durch ein PKT_Message_P() ersetzt
86	//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio)	86	//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio)
87	//# - add: #include "../pkt/pkt.h"	87	//# - add: #include "../pkt/pkt.h"
88	//#	88	//#
89	//# 05.05.2013 Cebra	89	//# 05.05.2013 Cebra
90	//# - chg: #ifdef USE_FOLLOWME	90	//# - chg: #ifdef USE_FOLLOWME
91	//#	91	//#
92	//############################################################################	92	//############################################################################
93		93
94	#include "../cpu.h"	94	#include "../cpu.h"
95	#include <avr/io.h>	95	#include <avr/io.h>
96	#include <inttypes.h>	96	#include <inttypes.h>
97	#include <stdlib.h>	97	#include <stdlib.h>
98	#include <avr/pgmspace.h>	98	#include <avr/pgmspace.h>
99	#include <util/delay.h>	99	#include <util/delay.h>
100		100
101	#include <string.h>	101	#include <string.h>
102	#include <stdarg.h>	102	#include <stdarg.h>
103	#include <stdio.h>	103	#include <stdio.h>
104		104
105	#include "../main.h"	105	#include "../main.h"
106		106
107	#ifdef USE_FOLLOWME	107	#ifdef USE_FOLLOWME
108		108
109	#include "../followme/followme.h"	109	#include "../followme/followme.h"
110	#include "../osd/osd.h"	110	#include "../osd/osd.h"
111	#include "../lcd/lcd.h"	111	#include "../lcd/lcd.h"
112	#include "../timer/timer.h"	112	#include "../timer/timer.h"
113	#include "../uart/usart.h"	113	#include "../uart/usart.h"
114	#include "../eeprom/eeprom.h"	114	#include "../eeprom/eeprom.h"
115	#include "../messages.h"	115	#include "../messages.h"
116	#include "../bluetooth/bluetooth.h"	116	#include "../bluetooth/bluetooth.h"
117	#include "../setup/setup.h"	117	#include "../setup/setup.h"
118	#include "../uart/uart1.h"	118	#include "../uart/uart1.h"
119	#include "../mk-data-structs.h"	119	#include "../mk-data-structs.h"
120	#include "../pkt/pkt.h"	120	#include "../pkt/pkt.h"
121	#include "../gps/gps.h"	121	#include "../gps/gps.h"
122	//#include "../gps/gps_nmea.h"	122	//#include "../gps/gps_nmea.h"
123	#include "../avr-nmea-gps-library/nmea.h"	123	#include "../avr-nmea-gps-library/nmea.h"
124	#include "../gps/gpsmouse.h"	124	#include "../gps/gpsmouse.h"
125		125
126		126
127	//#######################################################################################################################	127	//#######################################################################################################################
128		128
129		129
130	//--------------------	130	//--------------------
131	// Timings	131	// Timings
132	//--------------------	132	//--------------------
133	//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec)	133	//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec)
134	#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec)	134	#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec)
135		135
136		136
137		137
138	//--------------------	138	//--------------------
139	#define COSD_WASFLYING 4	139	#define COSD_WASFLYING 4
140		140
141	// global definitions and global vars	141	// global definitions and global vars
142	NaviData_t *naviData;	142	NaviData_t *naviData;
143	unsigned char Element;	143	unsigned char Element;
144	uint16_t heading_home;	144	uint16_t heading_home;
145		145
146	// Hier Höhenanzeigefehler Korrigieren	146	// Hier Höhenanzeigefehler Korrigieren
147	#define AltimeterAdjust 1.5	147	#define AltimeterAdjust 1.5
148		148
149		149
150		150
151	// Flags	151	// Flags
152	//uint8_t COSD_FLAGS2 = 0;	152	//uint8_t COSD_FLAGS2 = 0;
153	//	153	//
154	//GPS_Pos_t last5pos[7];	154	//GPS_Pos_t last5pos[7];
155	uint8_t FM_error = 0;	155	uint8_t FM_error = 0;
156		156
157		157
158	//---------------------	158	//---------------------
159	// Waypoint Types	159	// Waypoint Types
160	// TODO OG: verschieben nach: mk-data-structs.h	160	// TODO OG: verschieben nach: mk-data-structs.h
161	//---------------------	161	//---------------------
162	#define POINT_TYPE_INVALID 255	162	#define POINT_TYPE_INVALID 255
163	#define POINT_TYPE_WP 0	163	#define POINT_TYPE_WP 0
164	#define POINT_TYPE_POI 1	164	#define POINT_TYPE_POI 1
165		165
166		166
167	//---------------------	167	//---------------------
168	// Status	168	// Status
169	// GPS_Pos_t	169	// GPS_Pos_t
170	// aus MK source	170	// aus MK source
171	//	171	//
172	// TODO OG: verschieben nach: mk-data-structs.h	172	// TODO OG: verschieben nach: mk-data-structs.h
173	//---------------------	173	//---------------------
174	#define INVALID 0x00	174	#define INVALID 0x00
175	#define NEWDATA 0x01	175	#define NEWDATA 0x01
176	#define PROCESSED 0x02	176	#define PROCESSED 0x02
177		177
178		178
179		179
180	//--------------------------------------------------------------	180	//--------------------------------------------------------------
181	//--------------------------------------------------------------	181	//--------------------------------------------------------------
182	void FollowMe( void )	182	void FollowMe( void )
183	{	183	{
184	//uint8_t status;	184	//uint8_t status;
185	GPS_Pos_t currpos;	185	GPS_Pos_t currpos;
186	uint8_t tmp_dat;	186	uint8_t tmp_dat;
187	uint8_t redraw;	187	uint8_t redraw;
188	uint8_t drawmode;	188	uint8_t drawmode;
189	uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete	189	uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete
190	uint32_t send_followme_counter;	190	uint32_t send_followme_counter;
191		191
192	int8_t ok;	192	int8_t ok;
193	int8_t xoffs;	193	int8_t xoffs;
194	int8_t yoffs;	194	int8_t yoffs;
195	Point_t FollowMe;	195	Point_t FollowMe;
196	uint8_t mktimeout = false;	196	uint8_t mktimeout = false;
197		197
198		198
199		199
200	GPS_PosDev_t targetdev;	200	GPS_PosDev_t targetdev;
201		201
202		202
203	//---------------------	203	//---------------------
204	// 1. Daten GPS-Maus	204	// 1. Daten GPS-Maus
205	//---------------------	205	//---------------------
206	ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix	206	ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix
207	if( ok <= 0 )	207	if( ok <= 0 )
208	{	208	{
209	return; // Fehler bzgl. BT GPS-Maus -> exit	209	return; // Fehler bzgl. BT GPS-Maus -> exit
210	}	210	}
211		211
212		212
213		213
214	//---------------------	214	//---------------------
215	// 2. Follow Me	215	// 2. Follow Me
216	//---------------------	216	//---------------------
217	set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep	217	set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep
218		218
219	lcd_cls ();	219	lcd_cls ();
220		220
221	SwitchToNC();	221	SwitchToNC();
222		222
223	mode = 'O';	223	mode = 'O';
224		224
225	// disable debug...	225	// disable debug...
226	// RS232_request_mk_data (0, 'd', 0);	226	// RS232_request_mk_data (0, 'd', 0);
227	tmp_dat = 0;	227	tmp_dat = 0;
228	SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);	228	SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
229		229
230	// request OSD Data from NC every 100ms	230	// request OSD Data from NC every 100ms
231	// RS232_request_mk_data (1, 'o', 100);	231	// RS232_request_mk_data (1, 'o', 100);
232	tmp_dat = 10;	232	tmp_dat = 10;
233	SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);	233	SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
234		234
235	//OSD_active = true; // benötigt für Navidata Ausgabe an SV2	235	//OSD_active = true; // benötigt für Navidata Ausgabe an SV2
236		236
237	//-------------------------	237	//-------------------------
238	// Init: Timer & Flags	238	// Init: Timer & Flags
239	//-------------------------	239	//-------------------------
240	timer_mk_timeout = MK_TIMEOUT;	240	timer_mk_timeout = MK_TIMEOUT;
241		241
242	abo_timer = ABO_TIMEOUT;	242	abo_timer = ABO_TIMEOUT;
243		243
244		244
245	MKLiPoCells_Init();	245	MKLiPoCells_Init();
246		246
247	redraw = true;	247	redraw = true;
248		248
249	NMEA.Counter = 0;	249	NMEA.Counter = 0;
250	NMEA_GPGGA_counter_old = 0;	250	NMEA_GPGGA_counter_old = 0;
251	send_followme_counter = 0;	251	send_followme_counter = 0;
252		252
253		253
254	while( (receiveNMEA) )	254	while( (receiveNMEA) )
255	{	255	{
256	//-----------------------------------------	256	//-----------------------------------------
257	// Screen redraw	257	// Screen redraw
258	//-----------------------------------------	258	//-----------------------------------------
259	if( redraw )	259	if( redraw )
260	{	260	{
261	lcd_cls();	261	lcd_cls();
262		262
263	lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte	263	lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte
264		264
265	lcd_line( (66-3), 0, (66-3), 11, 1); // Linie Vertikal links	265	lcd_line( (66-3), 0, (66-3), 11, 1); // Linie Vertikal links
266	lcd_line( (156+5), 0, (156+5), 11, 1); // Linie Vertikal rechts	266	lcd_line( (156+5), 0, (156+5), 11, 1); // Linie Vertikal rechts
267	lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal	267	lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal
268		268
269	//lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte	269	//lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte
270	lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte	270	lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte
271		271
272	lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance)	272	lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance)
273		273
274	lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:"	274	lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:"
275		275
276	draw_icon_mk( 1, 18);	276	draw_icon_mk( 1, 18);
277	draw_icon_target_round( 1, 50);	277	draw_icon_target_round( 1, 50);
278		278
279	redraw = false;	279	redraw = false;
280	}	280	}
281		281
282		282
283		283
284	//-----------------------------------------	284	//-----------------------------------------
285	// neue MK Daten vorhanden	285	// neue MK Daten vorhanden
286	//-----------------------------------------	286	//-----------------------------------------
287	if( rxd_buffer_locked ) // neue MK Daten	287	if( rxd_buffer_locked ) // neue MK Daten
288	{	288	{
289	Decode64 ();	289	Decode64 ();
290	naviData = (NaviData_t *) pRxData;	290	naviData = (NaviData_t *) pRxData;
291		291
292	if( mktimeout ) redraw = true;	292	if( mktimeout ) redraw = true;
293	mktimeout = false;	293	mktimeout = false;
294		294
295	FM_error = 0; // noch benoetigt?	295	FM_error = 0; // noch benoetigt?
296		296
297	currpos.Latitude = naviData->CurrentPosition.Latitude;	297	currpos.Latitude = naviData->CurrentPosition.Latitude;
298	currpos.Longitude = naviData->CurrentPosition.Longitude;	298	currpos.Longitude = naviData->CurrentPosition.Longitude;
299		299
300	//----------------------------------	300	//----------------------------------
301	// speichere letzte GPS-Positionen	301	// speichere letzte GPS-Positionen
302	//----------------------------------	302	//----------------------------------
303	Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config	303	Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config
304	Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config	304	Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config
305		305
306		306
307	//----------------------------------	307	//----------------------------------
308	// LiPo Cell Check	308	// LiPo Cell Check
309	//----------------------------------	309	//----------------------------------
310	MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln	310	MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln
311		311
312		312
313	//#################	313	//#################
314	//# MK	314	//# MK
315	//#################	315	//#################
316		316
317	//-----------------	317	//-----------------
318	// Oben: MK-Batt Level (Volt)	318	// Oben: MK-Batt Level (Volt)
319	//-----------------	319	//-----------------
320	OSD_Element_BattLevel2( 0, 0, 0,0 );	320	OSD_Element_BattLevel2( 0, 0, 0,0 );
321		321
322	//-----------------	322	//-----------------
323	// Oben: Batt Level Bar	323	// Oben: Batt Level Bar
324	//-----------------	324	//-----------------
325	OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H);	325	OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H);
326		326
327	//-----------------	327	//-----------------
328	// Oben: MK-Flugzeit	328	// Oben: MK-Flugzeit
329	//-----------------	329	//-----------------
330	writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0);	330	writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0);
331		331
332	//-----------------	332	//-----------------
333	// Hoehe	333	// Hoehe
334	//-----------------	334	//-----------------
335	xoffs = 3;	335	xoffs = 3;
336	yoffs = -1;	336	yoffs = -1;
337	writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs );	337	writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs );
338		338
339	//-----------------	339	//-----------------
340	// MK: Sat Anzahl	340	// MK: Sat Anzahl
341	//-----------------	341	//-----------------
342	yoffs = -27;	342	yoffs = -27;
343	if( naviData->SatsInUse > 5 ) drawmode = MNORMALX;	343	if( naviData->SatsInUse > 5 ) drawmode = MNORMALX;
344	else drawmode = MINVERSX;	344	else drawmode = MINVERSX;
345	writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs);	345	writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs);
346	draw_icon_satmini( 116+3, 42+yoffs);	346	draw_icon_satmini( 116+3, 42+yoffs);
347		347
348		348
349	//-----------------	349	//-----------------
350	// MK: GPS	350	// MK: GPS
351	//-----------------	351	//-----------------
352	writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude	352	writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude
353	writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude	353	writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude
354	// lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude);	354	// lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude);
355	// lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude);	355	// lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude);
356		356
357		357
358		358
359	//#################	359	//#################
360	//# DISTANCE TO TARGET	360	//# DISTANCE TO TARGET
361	//#################	361	//#################
362		362
363	//-----------------	363	//-----------------
364	// Target: Distance to Target	364	// Target: Distance to Target
365	//-----------------	365	//-----------------
366	xoffs = 7;	366	xoffs = 7;
367	yoffs = 4;	367	yoffs = 4;
368	//GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 )	368	//GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 )
369	targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition);	369	targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition);
370	lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) );	370	lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) );
371		371
372		372
373	//#################	373	//#################
374	//# TARGET (GPS Maus)	374	//# TARGET (GPS Maus)
375	//#################	375	//#################
376		376
377	yoffs = 8;	377	yoffs = 8;
378		378
379	//-----------------	379	//-----------------
380	// Send Counter	380	// Send Counter
381	//-----------------	381	//-----------------
382	//lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen	382	//lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen
383	lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen	383	lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen
384		384
385		385
386	//-----------------	386	//-----------------
387	// Target: Fix	387	// Target: Fix
388	//-----------------	388	//-----------------
389	if( NMEA.SatFix == 1 \|\| NMEA.SatFix == 2 )	389	if( NMEA.SatFix == 1 \|\| NMEA.SatFix == 2 )
390	drawmode = MNORMALX;	390	drawmode = MNORMALX;
391	else	391	else
392	drawmode = MINVERSX;	392	drawmode = MINVERSX;
393		393
394	lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix	394	lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix
395		395
396		396
397	//-----------------	397	//-----------------
398	// Target: Sat Anzahl	398	// Target: Sat Anzahl
399	//-----------------	399	//-----------------
400	if( NMEA.SatsInUse > 5 )	400	if( NMEA.SatsInUse > 5 )
401	drawmode = MNORMALX;	401	drawmode = MNORMALX;
402	else	402	else
403	drawmode = MINVERSX;	403	drawmode = MINVERSX;
404		404
405	writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs);	405	writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs);
406	draw_icon_satmini( 116+4, 40+yoffs);	406	draw_icon_satmini( 116+4, 40+yoffs);
407		407
408		408
409	//-----------------	409	//-----------------
410	// Target: Lat / Long	410	// Target: Lat / Long
411	//-----------------	411	//-----------------
412	writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude	412	writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude
413	writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude	413	writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude
414		414
415		415
416	rxd_buffer_locked = FALSE;	416	rxd_buffer_locked = FALSE;
417		417
418	timer_mk_timeout = MK_TIMEOUT;	418	timer_mk_timeout = MK_TIMEOUT;
419	}	419	}
420		420
421	//-----------------------------------------	421	//-----------------------------------------
422	// if( !NMEA_receiveBT() )	422	// if( !NMEA_receiveBT() )
423	// {	423	// {
424	// receiveNMEA = false;	424	// receiveNMEA = false;
425	// }	425	// }
426	// else	426	// else
427	// {	427	// {
428	// NMEA_GetNewData();	428	// NMEA_GetNewData();
429	// }	429	// }
430		430
431		431
432		432
433	// Sourcecode aus NaviCtrl/V2.10e/uart1.c	433	// Sourcecode aus NaviCtrl/V2.10e/uart1.c
434	// FOLLOW_ME	434	// FOLLOW_ME
435	// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))	435	// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
436	// {	436	// {
437	// if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))	437	// if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
438	// {	438	// {
439	// TransmitAlsoToFC = 1;	439	// TransmitAlsoToFC = 1;
440	// // update FollowMe content	440	// // update FollowMe content
441	// FollowMe.Position.Longitude = GPSData.Position.Longitude;	441	// FollowMe.Position.Longitude = GPSData.Position.Longitude;
442	// FollowMe.Position.Latitude = GPSData.Position.Latitude;	442	// FollowMe.Position.Latitude = GPSData.Position.Latitude;
443	// FollowMe.Position.Status = NEWDATA;	443	// FollowMe.Position.Status = NEWDATA;
444	// FollowMe.Position.Altitude = 1;	444	// FollowMe.Position.Altitude = 1;
445	// // 0 -> no Orientation	445	// // 0 -> no Orientation
446	// // 1-360 -> CompassCourse Setpoint	446	// // 1-360 -> CompassCourse Setpoint
447	// // -1 -> points to WP1 -> itself	447	// // -1 -> points to WP1 -> itself
448	// FollowMe.Heading = -1;	448	// FollowMe.Heading = -1;
449	// FollowMe.ToleranceRadius = 1;	449	// FollowMe.ToleranceRadius = 1;
450	// FollowMe.HoldTime = 60;	450	// FollowMe.HoldTime = 60;
451	// FollowMe.Event_Flag = 1;	451	// FollowMe.Event_Flag = 1;
452	// FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list	452	// FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list
453	// FollowMe.Type = POINT_TYPE_WP;	453	// FollowMe.Type = POINT_TYPE_WP;
454	// FollowMe.WP_EventChannelValue = 100; // set servo value	454	// FollowMe.WP_EventChannelValue = 100; // set servo value
455	// FollowMe.AltitudeRate = 0; // do not change height	455	// FollowMe.AltitudeRate = 0; // do not change height
456	// FollowMe.Speed = 0; // rate to change the Position (0 = max)	456	// FollowMe.Speed = 0; // rate to change the Position (0 = max)
457	// FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic)	457	// FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic)
458	// FollowMe.Name[0] = 'F'; // Name of that point (ASCII)	458	// FollowMe.Name[0] = 'F'; // Name of that point (ASCII)
459	// FollowMe.Name[1] = 'O'; // Name of that point (ASCII)	459	// FollowMe.Name[1] = 'O'; // Name of that point (ASCII)
460	// FollowMe.Name[2] = 'L'; // Name of that point (ASCII)	460	// FollowMe.Name[2] = 'L'; // Name of that point (ASCII)
461	// FollowMe.Name[3] = 'L'; // Name of that point (ASCII)	461	// FollowMe.Name[3] = 'L'; // Name of that point (ASCII)
462	// FollowMe.reserve[0] = 0; // reserve	462	// FollowMe.reserve[0] = 0; // reserve
463	// FollowMe.reserve[1] = 0; // reserve	463	// FollowMe.reserve[1] = 0; // reserve
464	// MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));	464	// MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
465	// }	465	// }
466	// UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time	466	// UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
467	// }	467	// }
468	//	468	//
469		469
470		470
471		471
472		472
473		473
474	//-----------------------------------------	474	//-----------------------------------------
475	// neue NMEA Daten?	475	// neue NMEA Daten?
476	//-----------------------------------------	476	//-----------------------------------------
477	if(NMEA_isdataready() && receiveNMEA)	477	if(NMEA_isdataready() && receiveNMEA)
478	{	478	{
479	if( NMEA.Counter > NMEA_GPGGA_counter_old )	479	if( NMEA.Counter > NMEA_GPGGA_counter_old )
480	{	480	{
481	if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 \|\| NMEA.SatFix == 2) )	481	if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 \|\| NMEA.SatFix == 2) )
482	{	482	{
483	//Config.FM_Refresh	483	//Config.FM_Refresh
484		484
485	FollowMe.Position.Status = NEWDATA;	485	FollowMe.Position.Status = NEWDATA;
486	FollowMe.Position.Longitude = NMEA.Longitude;	486	FollowMe.Position.Longitude = NMEA.Longitude;
487	FollowMe.Position.Latitude = NMEA.Latitude;	487	FollowMe.Position.Latitude = NMEA.Latitude;
488	FollowMe.Position.Altitude = 1; // 20.7.2015 CB	488	FollowMe.Position.Altitude = 1; // 20.7.2015 CB
489	// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B.	489	// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B.
490		490
491	FollowMe.Heading = -1; // invalid heading	491	FollowMe.Heading = -1; // invalid heading
492	FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default	492	FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default
493	FollowMe.HoldTime = 60; // ????? go home after 60s without any update ??????	493	FollowMe.HoldTime = 60; // ????? go home after 60s without any update ??????
494	// FollowMe.Event_Flag = 0; // no event	494	// FollowMe.Event_Flag = 0; // no event
495	FollowMe.Event_Flag = 1; // 20.7.2015 CB	495	FollowMe.Event_Flag = 1; // 20.7.2015 CB
496	FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list	496	FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list
497	FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes	497	FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes
498	FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII)	498	FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII)
499	FollowMe.Name[1] = 'O';	499	FollowMe.Name[1] = 'O';
500	FollowMe.Name[2] = 'L';	500	FollowMe.Name[2] = 'L';

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/trunk/followme/followme.c - Rev 2147 → 2192