Subversion Repositories Projects

Rev

Rev 1518 | Blame | Last modification | View Log | RSS feed

/*****************************************************************************
 *   Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de                  *
 *   Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net                  *
 *   Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
 *   Copyright (C) 2011 Harald Bongartz                                      *
 *                                                                           *
 *   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*
 *****************************************************************************/



#include "cpu.h"
#include <avr/io.h>
#include <inttypes.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
#include <util/delay.h>

#include "main.h"
#include "osd.h"
#include "lcd.h"
#include "timer.h"
#include "usart.h"
#include "eeprom.h"
#include "messages.h"

#include "mk-data-structs.h"

#define COSD_WASFLYING 4
#define TIMEOUT 200     // 2 sec

// global definitions and global vars
NaviData_t *naviData;

// löschen und alle mit "naviData->Variometer" ersetzen
//int8_t Variometer = 0;

// Hier Höhenanzeigefehler Korrigieren
#define AltimeterAdjust 1.5

// stats for after flight
int16_t max_Altimeter = 0;
uint16_t max_GroundSpeed = 0;
int16_t max_Distance = 0;
uint8_t min_UBat = 255;
uint16_t max_FlyingTime = 0;
uint16_t max_Current = 0;
uint16_t max_Capacity = 0;

// cache old vars for blinking attribute, checkup is faster than full
// attribute write each time
volatile uint8_t last_UBat = 255;
volatile uint8_t last_RC_Quality = 255;

volatile uint16_t ftimer = 0;

// store stats description in progmem to save space
const char stats_item_0[] PROGMEM = "max Altitude:";
const char stats_item_1[] PROGMEM = "max Speed   :";
const char stats_item_2[] PROGMEM = "max Distance:";
const char stats_item_3[] PROGMEM = "min Voltage :";
const char stats_item_4[] PROGMEM = "max Time    :";
#if 1
const char stats_item_5[] PROGMEM = "max Current :";
const char stats_item_6[] PROGMEM = "UsedCapacity:";
#else
const char stats_item_5[] PROGMEM = "Long. :";
const char stats_item_6[] PROGMEM = "Lat.  :";
#endif


const char *stats_item_pointers[] PROGMEM = {
        stats_item_0,
        stats_item_1,
        stats_item_2,
        stats_item_3,
        stats_item_4,
        stats_item_5,
        stats_item_6
};

//char* rose = "-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W-+-N-+-O-+-S-+-W";
const char rose[48] PROGMEM = {
        0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S',
        0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N', 0x0e, 0x0f, 0x0e, 'O',
        0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W', 0x0e, 0x0f, 0x0e, 'N',
        0x0e, 0x0f, 0x0e, 'O', 0x0e, 0x0f, 0x0e, 'S', 0x0e, 0x0f, 0x0e, 'W',
};

// the center is char 19 (north), we add the current heading in 8th
// which would be 22.5 degrees, but float would bloat up the code
// and *10 / 225 would take ages... so we take the uncorrect way

const char str_NE[] PROGMEM = "NE";
const char str_E[] PROGMEM  = "E ";
const char str_SE[] PROGMEM = "SE";
const char str_S[] PROGMEM  = "S ";
const char str_SW[] PROGMEM = "SW";
const char str_W[] PROGMEM  = "W ";
const char str_NW[] PROGMEM = "NW";
const char str_N[] PROGMEM  = "N ";
const char *directions_p[8] PROGMEM = {
        str_NE,
        str_E,
        str_SE,
        str_S,
        str_SW,
        str_W,
        str_NW,
        str_N
};

// Flags
uint8_t COSD_FLAGS2 = 0;

GPS_Pos_t last5pos[7];
uint8_t error = 0;


//--------------------------------------------------------------
// convert the <heading> gotton from NC into an index
uint8_t heading_conv (uint16_t heading)
{
        if (heading > 23 && heading < 68)
                return 0;               //direction = "NE";
        else if (heading > 67 && heading < 113)
                return 1;               //direction = "E ";
        else if (heading > 112 && heading < 158)
                return 2;               //direction = "SE";
        else if (heading > 157 && heading < 203)
                return 3;               //direction = "S ";
        else if (heading > 202 && heading < 248)
                return 4;               //direction = "SW";
        else if (heading > 247 && heading < 293)
                return 5;               //direction = "W ";
        else if (heading > 292 && heading < 338)
                return 6;               //direction = "NW";

        return 7;       //direction = "N ";
}

//--------------------------------------------------------------
// draw a compass rose at <x>/<y> for <heading>
void draw_compass (uint8_t x, uint8_t y, uint16_t heading)
{
        uint8_t front = 19 + (heading / 22);
        for (uint8_t i = 0; i < 9; i++)
                lcd_putc (x++, y, pgm_read_byte(&rose[front - 4 + i]), 0);
}


//--------------------------------------------------------------
// variometer
// draw variometer arrows at <x>/<y> according to <variometer>
//
void draw_variometer (uint8_t x, uint8_t y, uint8_t width, uint8_t hight, int16_t variometer)
{
        lcd_rect (x, y - ((hight) / 2), width, hight, 1);
        lcd_frect (x + 1, y - ((hight) / 2) + 1, width - 2, hight - 2, 0);
        lcd_line (x, y, x + width, y, 1);

        if (variometer > 0)  // steigend
        {
                switch (variometer / 5)
                {
                        case 0:
                                lcd_line  (x + 4, y - 1, x + 6, y - 1, 1);      //  1 >  4
                        break;

                        case 1:
                                lcd_line  (x + 4, y - 1, x + 6, y - 1, 1);      //  1 >  4
                                lcd_frect (x + 3, y - 3, 4, 1, 1);                      //  5 >  9
                        break;

                        case 2:
                                lcd_line  (x + 4, y - 1, x + 6, y - 1, 1);      //  1 >  4
                                lcd_frect (x + 3, y - 3, 4, 1, 1);                      //  5 >  9
                                lcd_frect (x + 2, y - 5, 6, 1, 1);                      // 10 > 14
                        break;

                        default:
                                lcd_line  (x + 4, y - 1, x + 6, y - 1, 1);  //  1 >  4
                                lcd_frect (x + 3, y - 3, 4, 1, 1);                      //  5 >  9
                                lcd_frect (x + 2, y - 5, 6, 1, 1);                      // 10 > 14
                                lcd_frect (x + 1, y - 6, 8, 1, 1);                      // 15 >
                        break;
                }
        }
        else if (variometer < 0)  // fallend
        {
                switch ((variometer) / -5)
                {
                        case 0:
                                lcd_line  (x + 4, y + 1, x + 6, y + 1, 1);      // - 1 > - 4
                        break;

                        case 1:
                                lcd_line  (x + 4, y + 1, x + 6, y + 1, 1);      // - 1 > - 4
                                lcd_frect (x + 3, y + 2, 4, 1, 1);                      // - 5 > - 9
                        break;

                        case 2:
                                lcd_line  (x + 4, y + 1, x + 6, y + 1, 1);      // - 1 > - 4
                                lcd_frect (x + 3, y + 2, 4, 1, 1);                      // - 5 > - 9
                                lcd_frect (x + 2, y + 4, 6, 1, 1);                      // -10 > -14
                        break;

                        default:
                                lcd_line  (x + 4, y + 1, x + 6, y + 1, 1);      // - 1 > - 4
                                lcd_frect (x + 3, y + 2, 4, 1, 1);                      // - 5 > - 9
                                lcd_frect (x + 2, y + 4, 6, 1, 1);                      // -10 > -14
                                lcd_frect (x + 1, y + 5, 8, 1, 1);                      // -15 >
                        break;
                }
        }
}


//--------------------------------------------------------------
void print_statistics (void)
{
        uint8_t line = 0;
        lcd_cls ();
//      lcd_printp_at (12, 7, PSTR("Ende"), 0);
        lcd_puts_at(12, 7, strGet(ENDE), 0);

        // max Altitude
        lcd_printp_at (0, line, stats_item_pointers[0], 0);
        write_ndigit_number_s (13, line, max_Altimeter / (30 / AltimeterAdjust), 4, 0);
        lcd_putc (17, line, 'm', 0);

        // max Speed
        lcd_printp_at (0, ++line, stats_item_pointers[1], 0);
        write_ndigit_number_u (14, line, (uint16_t) (((uint32_t) max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0);
        lcd_printp_at(17, line, PSTR("km/h"), 0);

        // max Distance
        lcd_printp_at (0, ++line, stats_item_pointers[2], 0);
        write_ndigit_number_u (14, line, max_Distance / 10, 3, 0);
        lcd_putc (17, line, 'm', 0);

        // max time
        lcd_printp_at (0, ++line, stats_item_pointers[4], 0);
        write_time (13, line, max_FlyingTime);

        // min voltage
        lcd_printp_at (0, ++line, stats_item_pointers[3], 0);
        write_ndigit_number_u_10th (13, line, min_UBat, 3, 0);
        lcd_putc (17, line, 'V', 0);

#if 1
        // max Current
        lcd_printp_at (0, ++line, stats_item_pointers[5], 0);
        write_ndigit_number_u_10th (13, line, max_Current, 3, 0);
        lcd_putc (17, line, 'A', 0);

        // Used Capacity
        lcd_printp_at (0, ++line, stats_item_pointers[6], 0);
        write_ndigit_number_u (13, line, max_Capacity, 4, 0);
        lcd_printp_at(17, line, PSTR("mAh"), 0);
#else
        // longitude
        lcd_printp_at (0, ++line, stats_item_pointers[5], 0);
        write_gps_pos (8, line, naviData->CurrentPosition.Longitude);

        // latitude
        lcd_printp_at (0, ++line, stats_item_pointers[6], 0);
        write_gps_pos (8, line, naviData->CurrentPosition.Latitude);
#endif

        while (!get_key_press (1 << KEY_ESC))
                timer = TIMEOUT;

        COSD_FLAGS2 &= ~COSD_WASFLYING;
        get_key_press(KEY_ALL);
        lcd_cls();
}

//--------------------------------------------------------------
void print_position (void)
{
        lcd_cls ();
//      lcd_printp_at (0, 0, PSTR(" Breitengr  Längengr "), 2);
        lcd_puts_at(0, 0, strGet(START_LASTPOS1), 2);
//      lcd_printp_at (12, 7, PSTR("Ende"), 0);
        lcd_puts_at(12, 7, strGet(ENDE), 0);
        uint8_t ij =0;

        for(ij=0;ij<6;ij++)
        {
                uint32_t lon = last5pos[ij].Latitude;
                write_ndigit_number_u (1, ij+1, (uint16_t)(lon/10000000), 2, 0);
                lcd_printp_at (3, ij+1, PSTR("."), 0);
                write_ndigit_number_u (4, ij+1, (uint16_t)((lon/1000) % 10000), 4, 1);
                write_ndigit_number_u (8, ij+1, (uint16_t)((lon/10) % 100), 2, 1);

                uint32_t lat = last5pos[ij].Longitude;
                write_ndigit_number_u (12, ij+1, (uint16_t)(lat/10000000), 2, 0);
                lcd_printp_at (14, ij+1, PSTR("."), 0);
                write_ndigit_number_u (15, ij+1, (uint16_t)((lat/1000) % 10000), 4, 1);
                write_ndigit_number_u (19, ij+1, (uint16_t)((lat/10) % 100), 2, 1);
        }

        while (!get_key_press (1 << KEY_ESC))
                timer = TIMEOUT;

        get_key_press(KEY_ALL);
        lcd_cls();
}
void Show_LastPosition(void)
{
  lcd_puts_at(0, 2, strGet(OSD_POS1), 0);
  lcd_puts_at(0, 3, strGet(OSD_POS2), 0);
  lcd_puts_at(0, 5, strGet(START_LASTPOS1), 0);
  uint32_t lon = last5pos[0].Latitude;
  write_ndigit_number_u (1, 6, (uint16_t)(lon/10000000), 2, 0);
  lcd_printp_at (3, 6, PSTR("."), 0);
  write_ndigit_number_u (4, 6, (uint16_t)((lon/1000) % 10000), 4, 1);
  write_ndigit_number_u (8, 6, (uint16_t)((lon/10) % 100), 2, 1);

  uint32_t lat = last5pos[0].Longitude;
  write_ndigit_number_u (12, 6, (uint16_t)(lat/10000000), 2, 0);
  lcd_printp_at (14, 6, PSTR("."), 0);
  write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1);
  write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1);

}

void OSD_Timeout(uint8_t flag)
{

//  uint8_t flag;
  uint8_t tmp_dat;
 // flag = 0;
  timer = TIMEOUT;
  // disable OSD Data from NC
  //      RS232_request_mk_data (1, 'o', 0);
//  tmp_dat = 0;
//  SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);

  mode = 0;
  rxd_buffer_locked = FALSE;


  // Bei Verbindungsverlusst werden hier die letzten bekannten Koordinaten ausgegeben!!!
//  if (!timer)
//  {       // timeout occured
          if (flag)
          {
                  // Falls Spannungswarnung an war Beeper aus//
                  BeepTime = 0;
                  BeepMuster = 0xFFFF;

                  lcd_cls ();
                  WriteLastPosition(last5pos[0].Longitude,last5pos[0].Latitude);  // im EEprom speichern
                  lcd_puts_at(0, 0, strGet(OSD_ERROR), 2);

//                      lcd_printp_at (0, 0, PSTR(" ERROR: Datenverlust "), 2);

//                      lcd_printp_at (0, 2, PSTR("Letzte bekannte"), 0);
//                      lcd_printp_at (0, 3, PSTR("Position gespeichert."), 0);
                  lcd_puts_at(0, 2, strGet(OSD_POS1), 0);
                  lcd_puts_at(0, 3, strGet(OSD_POS2), 0);
//                      lcd_printp_at (19, 7, PSTR("OK"), 0);
                  lcd_puts_at(19, 7, strGet(OK), 0);
//                      lcd_printp_at (0, 5, PSTR(" Breitengr  Längengr "), 0);

                  lcd_puts_at(0, 5, strGet(START_LASTPOS1), 0);

                  lcd_puts_at(12, 7, strGet(ENDE), 0);

                  BeepTime = 250;
                  BeepMuster = 0x0080;
                  error = 1;

                  uint32_t lon = last5pos[0].Latitude;
                  write_ndigit_number_u (1, 6, (uint16_t)(lon/10000000), 2, 0);
                  lcd_printp_at (3, 6, PSTR("."), 0);
                  write_ndigit_number_u (4, 6, (uint16_t)((lon/1000) % 10000), 4, 1);
                  write_ndigit_number_u (8, 6, (uint16_t)((lon/10) % 100), 2, 1);

                  uint32_t lat = last5pos[0].Longitude;
                  write_ndigit_number_u (12, 6, (uint16_t)(lat/10000000), 2, 0);
                  lcd_printp_at (14, 6, PSTR("."), 0);
                  write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1);
                  write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1);

//                  while (!get_key_press (1 << KEY_ENTER));
                  _delay_ms(1000);
                  timer = TIMEOUT;
                  lcd_cls();
//                  return;


          }
          else
            {
              lcd_puts_at(0, 0, strGet(OSD_ERROR), 2);
              Show_LastPosition();
              BeepTime = 250;
              BeepMuster = 0x0080;
            }
//  }
                  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);


}




//--------------------------------------------------------------
void osd (uint8_t ShowMode)
{
        uint8_t flag;
        uint8_t tmp_dat;
        uint8_t OSD_Mode;
        uint8_t info_3D = 0;

        // Clear statistics
        max_Altimeter = 0;
        max_GroundSpeed = 0;
        max_Distance = 0;
        min_UBat = 255;
        max_FlyingTime = 0;

        // flags from last round to check for changes
        uint8_t old_FCFlags = 0;

        uint16_t old_hh = 0;
        uint8_t old_AngleNick = 0;
        uint8_t old_AngleRoll = 0;
        lcd_cls();
        OSD_Mode = ShowMode;

//      if(error == 0)
//              lcd_cls();

        if (hardware == FC)
        {
                lcd_printp_at(0, 3, PSTR("Only with NC !"), 0);
                timer = 100;
                while (timer > 0);

                return;
        }

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

        flag = 0;
        timer = TIMEOUT;
        abo_timer = ABO_TIMEOUT;


        do
        {
                if (rxd_buffer_locked)
                {
                        timer = TIMEOUT;
                        Decode64 ();
                        naviData = (NaviData_t *) pRxData;

                        if(error == 1)
                                lcd_cls();

                        error = 0;
                        GPS_Pos_t currpos;
                        currpos.Latitude = naviData->CurrentPosition.Latitude;
                        currpos.Longitude = naviData->CurrentPosition.Longitude;

                        if((currpos.Latitude != last5pos[0].Latitude)&&(currpos.Longitude != last5pos[0].Longitude))
                        {
                                last5pos[6] = last5pos[5];
                                last5pos[5] = last5pos[4];
                                last5pos[4] = last5pos[3];
                                last5pos[3] = last5pos[2];
                                last5pos[2] = last5pos[1];
                                last5pos[1] = last5pos[0];
                                last5pos[0] = currpos;
                        }

                        flag = 1;

                        if (OSD_Mode == 1)
                        {
                                if (naviData->FCFlags & FCFLAG_MOTOR_RUN)
                                { // should be engines running
                                        // motors are on, assume we were/are flying
                                        COSD_FLAGS2 |= COSD_WASFLYING;
                                }
                                else
                                {       // stats
                                        if ((COSD_FLAGS2 & COSD_WASFLYING) | (get_key_press (1 << KEY_ENTER)))
                                                print_statistics ();

                                        if (get_key_press (1 << KEY_PLUS))
                                                print_position ();
                                }

                                lcd_ecircle(22, 35, 16, 1);

                                // Ground Speed
                                write_ndigit_number_u (1, 0, (uint16_t) (((uint32_t) naviData->GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0);
                                lcd_printp_at(4, 0, PSTR("km/h"), 0);

                                // Compass
                                write_ndigit_number_u (14, 0, naviData->CompassHeading, 3, 0);
                                lcd_putc (17, 0, 0x1E, 0);      // degree symbol
                                lcd_printp_at (18, 0, (const char *) (pgm_read_word ( &(directions_p[heading_conv(naviData->CompassHeading)]))), 0);

                                draw_compass (12, 1, naviData->CompassHeading);

                                // Altitude
                                //note:lephisto:according to several sources it's /30
                                if (naviData->Altimeter > (300 / AltimeterAdjust) || naviData->Altimeter < (-300 / AltimeterAdjust))    // above 10m only write full meters
                                        write_ndigit_number_s (0, 1, naviData->Altimeter / (30 / AltimeterAdjust), 4, 0);
                                else    // up to 10m write meters.dm
                                        write_ndigit_number_s_10th (0, 1, naviData->Altimeter / (3 / AltimeterAdjust), 3, 0);

                                lcd_putc (4, 1, 'm', 0);

                                draw_variometer (54, 7, 10, 14, naviData->Variometer);

                                // TODO: verify correctness
                                uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360;
                                lcd_ecirc_line (22, 35, 15, old_hh, 0);
                                old_hh = heading_home;
                                lcd_ecirc_line (22, 35, 15, heading_home, 1);

                                write_ndigit_number_u (7, 3, heading_home, 3, 0);
                                lcd_putc (10, 3, 0x1e, 0);      // degree symbol

                                write_ndigit_number_u (7, 2, naviData->HomePositionDeviation.Distance / 10, 3, 0);
                                lcd_putc (10, 2, 'm', 0);

                                // Sats in use
                                lcd_printp_at(11, 4, PSTR("Sats"), 0);
                                write_ndigit_number_u (8, 4, naviData->SatsInUse, 2, 0);

                                if (naviData->NCFlags & NC_FLAG_MANUAL_CONTROL)
                                        lcd_putc (19, 4, 'M', 0); // rc transmitter
                                else
                                        lcd_putc (19, 4, 'X', 0); // clear

                                if (naviData->NCFlags & NC_FLAG_CH)
                                        lcd_printp_at (8, 5, PSTR("Coming Home"), 0);
                                else if (naviData->NCFlags & NC_FLAG_PH)
                                        lcd_printp_at (8, 5, PSTR("Pos. Hold  "), 0);
                                else    // (naviData->NCFlags & NC_FLAG_FREE)
                                        lcd_printp_at (8, 5, PSTR("Free       "), 0);

                                // Flying time
                                write_time (7, 6, naviData->FlyingTime);
                                lcd_printp_at (12, 6, PSTR("h"), 0);

                                // RC
                                write_ndigit_number_u (15, 6, naviData->RC_Quality, 3, 0);
                                lcd_printp_at(18, 6, PSTR("\x1F"), 0);          // RC-transmitter
                                if (naviData->NCFlags & NC_FLAG_NOSERIALLINK)
                                {
                                        lcd_printpns_at(19, 6, PSTR("  "), 0);          // clear
                                }
                                else
                                {
                                        lcd_printpns_at(19, 6, PSTR("PC"), 0);
                                }

                                // Battery level
                                write_ndigit_number_u_10th (0, 7, naviData->UBat, 3, 0);
                                lcd_putc (4, 7, 'V', 0);

                                // Akku Warnung
                                if (naviData->UBat < MK_LowBat)
                                { //Beeper ein
                                        BeepTime = 3000;
                                        BeepMuster = 0x0020;
                                }

                                //if (naviData->UBat > MK_LowBat+2)  //bei kurzzeitigen Schwankungen Beeper erst wieder aus wenn UBat 0,2 V höher als Warnschwelle
                                //{ //Beeper aus
                                //      BeepTime = 0;
                                //      BeepMuster = 0xFFFF;
                                //}
                                // Akku Warnung Ende

                                // Current
                                write_ndigit_number_u_10th (7, 7, naviData->Current, 3, 0);
                                lcd_putc (11, 7, 'A', 0);

                                // Capacity
                                write_ndigit_number_u (14, 7, naviData->UsedCapacity, 4, 0);
                                lcd_printp_at(18, 7, PSTR("mAh"), 0);

                                // remember statistics (only when engines running)
                                if (naviData->FCFlags & FCFLAG_MOTOR_RUN)
                                {
                                        if (naviData->Altimeter > max_Altimeter) max_Altimeter = naviData->Altimeter;
                                        if (naviData->GroundSpeed > max_GroundSpeed) max_GroundSpeed = naviData->GroundSpeed;
                                        if (naviData->HomePositionDeviation.Distance > max_Distance) max_Distance = naviData->HomePositionDeviation.Distance;
                                        if (naviData->UBat < min_UBat) min_UBat = naviData->UBat;
                                        if (naviData->FlyingTime > max_FlyingTime) max_FlyingTime = naviData->FlyingTime;
                                        if (naviData->Current > max_Current) max_Current = naviData->Current;
                                        if (naviData->UsedCapacity > max_Capacity) max_Capacity = naviData->UsedCapacity;
                                }

                                // remember last values
                                last_RC_Quality = naviData->RC_Quality;
                                last_UBat = naviData->UBat;
                                old_FCFlags = naviData->FCFlags;

                                rxd_buffer_locked = FALSE;
                        }
        // 3D Lage anzeige beginnt hier -----------------------------------
                        else if (OSD_Mode == 3)
                        {
                                uint16_t head_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360;

                                lcd_cls ();

                                lcd_line(0,32,128,32,1);   // horizontal //
                                lcd_line(64,0,64,64,1);    // vertical   //
                                lcd_printp_at(12, 7, PSTR("End  Info"), 0);


                                // 45' Angel
                                lcd_line(62,11,66,11,1);   //    --    //
                                lcd_line(22,30,22,34,1);   //  |       //
                                lcd_line(106,30,106,34,1); //       |  //
                                lcd_line(62,53,66,53,1);   //    --    //

                                if (info_3D == 1)
                                {
                                        lcd_line(34,17,36,15,1);   //  /       //
                                        lcd_line(92,15,94,17,1);   //       \  //
                                        lcd_line(34,47,36,49,1);   //  \       //
                                        lcd_line(92,49,94,47,1);   //       /  //

//                                      lcd_printp_at(9, 0, PSTR("V"), 0);
//                                      lcd_printp_at(0, 3, PSTR("L"), 0);
//                                      lcd_printp_at(20, 3, PSTR("R"), 0);
//                                      lcd_printp_at(9, 7, PSTR("H"), 0);

                                        lcd_puts_at(9, 0, strGet(OSD_V), 0);
                                        lcd_puts_at(0, 3, strGet(OSD_L), 0);
                                        lcd_puts_at(20, 3, strGet(OSD_R), 0);
                                        lcd_puts_at(9, 7, strGet(OSD_H), 0);

                                        lcd_printp_at(0, 0, PSTR("N:"), 0);
                                        write_ndigit_number_s (2, 0, naviData->AngleNick, 3, 0);

                                        lcd_printp_at(0, 7, PSTR("R:"), 0);
                                        write_ndigit_number_s (2, 7, naviData->AngleRoll, 3, 0);

                                        lcd_printp_at(15, 0, PSTR("K:"), 0);
                                        write_ndigit_number_s (18, 0,head_home, 3, 0);
                                }

                                if (get_key_press (1 << KEY_ENTER))
                                {
                                        info_3D++;
                                        if (info_3D > 1)
                                                info_3D = 0;
                                }

                                uint8_t Nick = ((-naviData->AngleNick/2)+32);
                                uint8_t Roll = -naviData->AngleRoll+64;
                                lcd_ecircle(old_AngleRoll,old_AngleNick, 10, 0);
                                lcd_ecirc_line (old_AngleRoll, old_AngleNick, 9, old_hh, 0);

                                lcd_ecircle(Roll, Nick, 10, 1);
                                lcd_ecirc_line (Roll, Nick, 9, head_home, 1);

                                old_hh = head_home;
                                old_AngleNick = Nick;
                                old_AngleRoll = Roll;
                                // remember last values
                                last_RC_Quality = naviData->RC_Quality;
                                last_UBat = naviData->UBat;
                                old_FCFlags = naviData->FCFlags;
                                rxd_buffer_locked = FALSE;
                        }

                        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;
                        }
                }
            if (!timer)
              {
                OSD_Timeout(flag);
                flag = 0;
              }
        }

        while (!get_key_press (1 << KEY_ESC));
//        while (!get_key_press (1 << KEY_ESC) && timer);
//      get_key_press(KEY_ALL);
//
//
//      // disable OSD Data from NC
//      //      RS232_request_mk_data (1, 'o', 0);
//      tmp_dat = 0;
//      SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
//
//      mode = 0;
//      rxd_buffer_locked = FALSE;
//
//
//      // Bei Verbindungsverlusst werden hier die letzten bekannten Koordinaten ausgegeben!!!
//      if (!timer)
//      {       // timeout occured
//              if (flag)
//              {
//                      // Falls Spannungswarnung an war Beeper aus//
//                      BeepTime = 0;
//                      BeepMuster = 0xFFFF;
//
//                      lcd_cls ();
//                      WriteLastPosition(last5pos[0].Longitude,last5pos[0].Latitude);  // im EEprom speichern
//                      lcd_puts_at(0, 0, strGet(OSD_ERROR), 2);
//
////                    lcd_printp_at (0, 0, PSTR(" ERROR: Datenverlust "), 2);
//
////                    lcd_printp_at (0, 2, PSTR("Letzte bekannte"), 0);
////                    lcd_printp_at (0, 3, PSTR("Position gespeichert."), 0);
//                        lcd_puts_at(0, 2, strGet(OSD_POS1), 0);
//                        lcd_puts_at(0, 3, strGet(OSD_POS2), 0);
////                    lcd_printp_at (19, 7, PSTR("OK"), 0);
//                        lcd_puts_at(19, 7, strGet(OK), 0);
////                    lcd_printp_at (0, 5, PSTR(" Breitengr  Längengr "), 0);
//
//                      lcd_puts_at(0, 5, strGet(START_LASTPOS1), 0);
//
//                      lcd_puts_at(12, 7, strGet(ENDE), 0);
//
//                      BeepTime = 1500;
//                      BeepMuster = 0x0040;
//                      error = 1;
//
//                      uint32_t lon = last5pos[0].Longitude;
//                      write_ndigit_number_u (1, 6, (uint16_t)(lon/10000000), 2, 0);
//                      lcd_printp_at (3, 6, PSTR("."), 0);
//                      write_ndigit_number_u (4, 6, (uint16_t)((lon/1000) % 10000), 4, 1);
//                      write_ndigit_number_u (8, 6, (uint16_t)((lon/10) % 100), 2, 1);
//
//                      uint32_t lat = last5pos[0].Latitude;
//                      write_ndigit_number_u (12, 6, (uint16_t)(lat/10000000), 2, 0);
//                      lcd_printp_at (14, 6, PSTR("."), 0);
//                      write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1);
//                      write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1);
//
//                      while (!get_key_press (1 << KEY_ENTER));
//
//                      timer = TIMEOUT;
//                      lcd_cls();
//                      return;
//
//              }
//      }
}