Subversion Repositories Projects

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

 *   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 "parameter.h"

#include "mk-data-structs.h"

#define COSD_WASFLYING 4

#define TIMEOUT 200     // 2 sec

// global definitions and global vars

NaviData_t *naviData;

mk_param_struct_t *mk_param_struct;

uint16_t old_hh = 0;

uint8_t Flags_ExtraConfig;

uint8_t Flags_GlobalConfig;

uint8_t Flags_GlobalConfig3;

unsigned char Element;

uint16_t heading_home;

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

volatile uint8_t OSD_active;

uint8_t Vario_Beep_Up = 0;

uint8_t Vario_Beep_Down = 0;

uint8_t Vario_Beep_Up_Interval = 9;

uint8_t Vario_Beep_Down_Interval = 6;

uint8_t Vario_Threshold = 0;

uint8_t Vario_Threshold_Value = 7;

uint8_t OldWP=0;

uint8_t NextWP = 0;

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

};

// Positionen der Anzeigeelemente im Bildschirm

#define OSD_ALTITUDE_CONTROL            1

#define OSD_ALTITUDE                    2

#define OSD_BATTERY_LEVEL               3

#define OSD_CAPACITY                    4

#define OSD_CARE_FREE                   5

#define OSD_COMPASS_DEGREE              6

#define OSD_COMPASS_DIRECTION           7

#define OSD_COMPASS_ROSE                8

#define OSD_CURRENT                     9

#define OSD_FLYING_TIME                 10

#define OSD_GROUND_SPEED                11

#define OSD_HOME_CIRCLE                 12

#define OSD_HOME_DEGREE                 13

#define OSD_HOME_DISTANCE               14

#define OSD_LED1_OUTPUT                 15

#define OSD_LED2_OUTPUT                 16

#define OSD_MANUELL                     17

#define OSD_NAVI_MODE                   18

#define OSD_RC_INTENSITY                19

#define OSD_SATS_IN_USE                 20

#define OSD_STATUS_FLAGS                21

#define OSD_VARIOMETER                  22

#define OSD_TARGET                      23

#define OSD_VARIOWERT                   24

#define OSD_WAYPOINT                    25

#define OSD_TARGET_DEGREE               26

#define MAX_CELL_VOLTAGE 43 // max cell volatage for LiPO

#define MIN_CELL_VOLTAGE 32 // min cell volatage for LiPO

// Flags

uint8_t COSD_FLAGS2 = 0;

GPS_Pos_t last5pos[7];

uint8_t error = 0;

uint8_t cells,BattLowVoltageWarning,CellIsChecked = 0;

uint8_t AkkuWarnThreshold = 0;

void CheckMKLipo(void)              // Quelle Mikrokopter FC-Software Holger + Ingo

{

            if(MK_LowBat < 50) // automatische Zellenerkennung

              {

               if (CellIsChecked <= 2)   //Nur beim Start 1x prüfen

                {

                        // up to 6s LiPo, less than 2s is technical impossible

                        for(cells = 2; cells < 7; cells++)

                        {

                                if(naviData->UBat < cells * MAX_CELL_VOLTAGE) break;

                        }

                        BattLowVoltageWarning = cells * MK_LowBat;

                        CellIsChecked++;

                }

              }

            else BattLowVoltageWarning = MK_LowBat;

            if (naviData->UBat < BattLowVoltageWarning)

              {

              if (AkkuWarnThreshold <= 4) AkkuWarnThreshold++;

              else

                { //Beeper ein

                        set_beep ( 1000, 0x0020, BeepSevere);

//                       BeepTime = 3000;

//                       BeepMuster = 0x0020;

                }

          }

}

//--------------------------------------------------------------

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

{

        x *= 6;

        y *= 8;

        y += 7;

        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;

                }

        }

}

//--------------------------------------------------------------

// Home symbol

// draw Homesymbol at <x>/<y>

//

void draw_homesymbol (uint8_t x, uint8_t y)

{

        x *= 6;

        y *= 8;

        y += 7;

        lcd_plot (x,y-4,1);

        lcd_line (x+1,y-1,x+1,y-5,1);

        lcd_plot (x+2,y-6,1);

        lcd_plot (x+3,y-7,1);

        lcd_plot (x+4,y-6,1);

        lcd_line (x+5,y-1,x+5,y-5,1);

        lcd_plot (x+6,y-4,1);

        lcd_plot (x+3,y-1,1);

        lcd_plot (x+3,y-2,1);

        lcd_line (x+1,y,x+5,y,1);

}

//--------------------------------------------------------------

// Target symbol

// draw Targetsymbol at <x>/<y>

//

void draw_targetsymbol (uint8_t x, uint8_t y)

{

        x *= 6;

        y *= 8;

        y += 7;

        lcd_circle (x+3, y-3, 4, 1);

        lcd_line (x,y-3,x+6,y-3,1);

        lcd_line (x+3,y,x+3,y-6,1);

        lcd_circle (x+3, y-3, 2, 1);

}

//--------------------------------------------------------------

void print_statistics (void)

{

        uint8_t line = 0;

        lcd_cls ();

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

        // max Altitude

        lcd_puts_at (0, line, strGet(STATS_ITEM_0), 0);

        write_ndigit_number_s (14, line, max_Altimeter / (30 / AltimeterAdjust), 4, 0,0);

        lcd_putc (18, line, 'm', 0);

//      max_GroundSpeed = 1;

        // max Speed

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_1), 0);

        write_ndigit_number_u (15, line, (uint16_t) (((uint32_t) max_GroundSpeed * (uint32_t) 9) / (uint32_t) 250), 3, 0,0);

        lcd_printp_at(18, line, PSTR("kmh"), 0);

        // max Distance

//      max_Distance = 64512;

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_2), 0);

        write_ndigit_number_u (15, line, max_Distance / 10, 4, 0,0);

        lcd_putc (19, line, 'm', 0);

//      max_FlyingTime = 3600;

        // max time

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_4), 0);

        write_time (14, line, max_FlyingTime);

        lcd_putc (19, line, 'm', 0);

        // min voltage

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_3), 0);

        write_ndigit_number_u_10th (14, line, min_UBat, 3, 0,0);

        lcd_putc (18, line, 'V', 0);

#if 1

//      max_Current = 1000;

        // max Current

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_5), 0);

        write_ndigit_number_u_10th (14, line, max_Current, 4, 0,0);

        lcd_putc (19, line, 'A', 0);

        // Used Capacity

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_6), 0);

        write_ndigit_number_u (14, line, max_Capacity, 4, 0,0);

        lcd_printp_at(18, line, PSTR("mAh"), 0);

#else

        // longitude

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_7), 0);

        write_gps_pos (8, line, naviData->CurrentPosition.Longitude);

        // latitude

        lcd_puts_at (0, ++line, strGet(STATS_ITEM_8), 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_puts_at(0, 0, strGet(START_LASTPOS1), 2);   // Breitengr  Längengr

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

                lcd_printp_at (3, ij+1, PSTR("."), 0);

                write_ndigit_number_u (4, ij+1, (uint16_t)((lon/1000) % 10000), 4, 1,0);

                write_ndigit_number_u (8, ij+1, (uint16_t)((lon/10) % 100), 2, 1,0);

                uint32_t lat = last5pos[ij].Longitude;

                write_ndigit_number_u (12, ij+1, (uint16_t)(lat/10000000), 2, 0,0);

                lcd_printp_at (14, ij+1, PSTR("."), 0);

                write_ndigit_number_u (15, ij+1, (uint16_t)((lat/1000) % 10000), 4, 1,0);

                write_ndigit_number_u (19, ij+1, (uint16_t)((lat/10) % 100), 2, 1,0);

        }

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

        lcd_printp_at (3, 6, PSTR("."), 0);

        write_ndigit_number_u (4, 6, (uint16_t)((lon/1000) % 10000), 4, 1,0);

        write_ndigit_number_u (8, 6, (uint16_t)((lon/10) % 100), 2, 1,0);

        uint32_t lat = last5pos[0].Longitude;

        write_ndigit_number_u (12, 6, (uint16_t)(lat/10000000), 2, 0,0);

        lcd_printp_at (14, 6, PSTR("."), 0);

        write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1,0);

        write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1,0);

}

//--------------------------------------------------------------

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 (flag)

        {

                // Falls Spannungswarnung an war Beeper aus//

                set_beep ( 0, 0, BeepOff);

                lcd_cls ();

                WriteLastPosition(last5pos[0].Longitude,last5pos[0].Latitude);  // im EEprom speichern

                lcd_puts_at(0, 0, strGet(OSD_ERROR), 2);                                                // ERROR: Datenverlust

                lcd_puts_at(0, 2, strGet(OSD_POS1), 0);                                                 // Letzte bekannte

                lcd_puts_at(0, 3, strGet(OSD_POS2), 0);                                                 // Position gespeichert.

                lcd_puts_at(0, 5, strGet(START_LASTPOS1), 0);                                   // Breitengr  Längengr

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

//              lcd_puts_at(19, 7, strGet(OK), 0);

//              if (OSD_RCErrorbeep==true)

//              {

                set_beep ( 250, 0x0040, BeepNormal);

//              }

                error = 1;

                uint32_t lon = last5pos[0].Latitude;

                write_ndigit_number_u (1, 6, (uint16_t)(lon/10000000), 2, 0,0);

                lcd_printp_at (3, 6, PSTR("."), 0);

                write_ndigit_number_u (4, 6, (uint16_t)((lon/1000) % 10000), 4, 1,0);

                write_ndigit_number_u (8, 6, (uint16_t)((lon/10) % 100), 2, 1,0);

                uint32_t lat = last5pos[0].Longitude;

                write_ndigit_number_u (12, 6, (uint16_t)(lat/10000000), 2, 0,0);

                lcd_printp_at (14, 6, PSTR("."), 0);

                write_ndigit_number_u (15, 6, (uint16_t)((lat/1000) % 10000), 4, 1,0);

                write_ndigit_number_u (19, 6, (uint16_t)((lat/10) % 100), 2, 1,0);

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

                if (OSD_RCErrorbeep==true) set_beep ( 200, 0x0080, BeepNormal);

//              _delay_ms(2000);

        }

        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 lcd_o_circle (int16_t x, int16_t y, int16_t breite, uint8_t mode)

{

        breite *= 6;

        int16_t radius = breite / 2;

        x += 2;

        x *= 6;

        x += 2;

        y += 1;

        y *= 8;

        y += 3;

        lcd_ellipse (x, y, radius - 3, radius - 5, mode);

}

//-----------------------------------------------------------

//

void lcd_o_circ_line (uint8_t x, uint8_t y, uint8_t breite, uint16_t deg, uint8_t mode)

{

        breite *= 6;

        int16_t radius = breite / 3;

        x += 2;

        x *= 6;

        x += 2;

        y += 1;

        y *= 8;

        y += 3;

        lcd_ellipse_line(x, y, radius, radius, deg, mode);

}

//--------------------------------------------------------------

void osd (uint8_t ShowMode)

{

        uint8_t flag;

        uint8_t tmp_dat;

        uint8_t OSD_Mode;

        uint8_t info_3D = 0;

        uint8_t status;

        // Clear statistics

        max_Altimeter = 0;

        max_GroundSpeed = 0;

        max_Distance = 0;

        min_UBat = 255;

        max_FlyingTime = 0;

        CellIsChecked = 0;

        cells = 0;

        AkkuWarnThreshold=0;

        OldWP=0;

        NextWP = false;

        // flags from last round to check for changes

        uint8_t old_FCFlags = 0;

        uint8_t old_AngleNick = 0;

        uint8_t old_AngleRoll = 0;

        lcd_cls();

        OSD_Mode = ShowMode;

        if (hardware == FC)

        {

                lcd_puts_at(0, 3, strGet(ONLY_NC), 0);  // Nur mit NC

                timer = 100;

                while (timer > 0);

                return;

        }

        SwitchToFC();

        status = load_setting(0xff);

        if(status == 255)

        {

                lcd_puts_at(0, 0, strGet(NO_SETTINGS), 0);      // Keine Setings

                _delay_ms(2000);

        }

        Flags_ExtraConfig = mk_param_struct->ExtraConfig;

        Flags_GlobalConfig = mk_param_struct->GlobalConfig;

        Flags_GlobalConfig3 = mk_param_struct->GlobalConfig3;

        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;

        OSD_active = true;              // benötigt für Navidata Ausgabe an SV2

        SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);

        flag = 0;

        timer = TIMEOUT;

        abo_timer = ABO_TIMEOUT;

//??    lcd_putc (10,5,0x18,1);

        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->FCStatusFlags & FC_STATUS_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 ();

//                                              draw_homesymbol(9,5);

//                                              draw_targetsymbol(9,6);

                                          }

                                        if (get_key_press (1 << KEY_PLUS))

                                          {

                                                print_position ();

//                                              draw_homesymbol(9,5);

//                                              draw_targetsymbol(9,6);

                                          }

                                }

                                // Altitude Control

                                switch (OSD_ScreenMode)

                                  {

                                    case    0 :  OSD_Screen_Element (0, 3, OSD_ALTITUDE_CONTROL); break;