Subversion Repositories Projects

/*

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

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

#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 "../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 "../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

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

// Flags

//uint8_t COSD_FLAGS2 = 0;

//

//GPS_Pos_t last5pos[7];

uint8_t FM_error = 0;

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

void FollowMe (void)

{

        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;

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

        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;

        do

        {

                if (rxd_buffer_locked)

                {

                        timer = TIMEOUT;

                        Decode64 ();

                        naviData = (NaviData_t *) pRxData;

                        if(FM_error == 1)

                                lcd_cls();

                        FM_error = 0;

                        GPS_Pos_t currpos;

                        currpos.Latitude = naviData->CurrentPosition.Latitude;

                        currpos.Longitude = naviData->CurrentPosition.Longitude;

                        flag = 1;

                        {

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

                                                                // OSD_Screen_Element() is depricated!

                                                                // USE NEW OSD_Element_xyz() FUNCTIONS!

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

                                // Altitude Control

                                //OSD_Screen_Element (0, 3, OSD_ALTITUDE_CONTROL,Config.OSD_ScreenMode);

                                OSD_Element_AltitudeControl( 0, 0 );

//

//                                // Altitude

//                                OSD_Screen_Element (11, 3, OSD_ALTITUDE);

                                // Battery level

                                //OSD_Screen_Element (0, 7, OSD_BATTERY_LEVEL,Config.OSD_ScreenMode);

                                                                OSD_Element_BatteryLevel( 0, 7, Config.OSD_ScreenMode );

                                // Capacity

                                //OSD_Screen_Element (13, 7, OSD_CAPACITY,Config.OSD_ScreenMode);

                                                                OSD_Element_Capacity( 13, 7 );

                                // Care Free

                                //OSD_Screen_Element (12, 2, OSD_CARE_FREE,Config.OSD_ScreenMode);

                                OSD_Element_CareFree( 12, 2 );

//                                // Compass Degree

//                                OSD_Screen_Element (13, 0, OSD_COMPASS_DEGREE);

//

//                                // Compass Direction

//                                OSD_Screen_Element (18, 0, OSD_COMPASS_DIRECTION);

//

//                                // Compass Rose

//                                OSD_Screen_Element (12, 1, OSD_COMPASS_ROSE);

                                // Current

                                //OSD_Screen_Element (7, 7, OSD_CURRENT,Config.OSD_ScreenMode);

                                OSD_Element_Current( 7, 7 );

                                // Flying time

                                lcd_printp_at (0, 1, PSTR("Flytime:"), 0);

                                //OSD_Screen_Element (8, 1, OSD_FLYING_TIME,Config.OSD_ScreenMode);

                                OSD_Element_FlyingTime( 8, 1 );

                                // Ground Speed

                                lcd_printp_at (0, 0, PSTR("Speed:"), 0);

                                //OSD_Screen_Element (6, 0, OSD_GROUND_SPEED,Config.OSD_ScreenMode);

                                OSD_Element_GroundSpeed( 6, 0 );

                                // Sats in use

                                //OSD_Screen_Element (16, 0, OSD_SATS_IN_USE,Config.OSD_ScreenMode);

                                                                OSD_Element_SatsInUse( 16, 0, Config.OSD_ScreenMode );

//                                // Home Circle

//                                OSD_Screen_Element (16, 4, OSD_HOME_CIRCLE);

//

//                                // Home Degree

//                                OSD_Screen_Element (12, 4, OSD_HOME_DEGREE);

                                // Home Distance

                                lcd_printp_at (11, 3, PSTR("Dist:"), 0);

                                //OSD_Screen_Element (17, 3, OSD_HOME_DISTANCE,Config.OSD_ScreenMode);

                                OSD_Element_HomeDistance( 17, 3, Config.OSD_ScreenMode );

//

//                                // LED1 Output

//                                OSD_Screen_Element (0, 6, OSD_LED1_OUTPUT);

//

//                                // LED2 Output

//                                OSD_Screen_Element (5, 6, OSD_LED2_OUTPUT);

                                // Manuell

                        //      OSD_Screen_Element (7, 0, OSD_MANUELL);

                                // Navi Mode

                                //OSD_Screen_Element (0, 2, OSD_NAVI_MODE,Config.OSD_ScreenMode);

                                OSD_Element_NaviMode( 0, 2 );

                                // RC Intensity

                        //      OSD_Screen_Element (11, 6, OSD_RC_INTENSITY);

                                // Status Flags

//                                OSD_Screen_Element (0, 2, OSD_STATUS_FLAGS);

//                                // Variometer

//                                OSD_Screen_Element (9, 0, OSD_VARIOMETER);

                                lcd_printp_at (0, 4, PSTR("MK:"), 0);

                                write_ndigit_number_u (3, 4, (uint16_t)(currpos.Latitude/10000000), 2, 0,0);

                                lcd_printp_at (5, 4, PSTR("."), 0);

                                write_ndigit_number_u (6, 4, (uint16_t)((currpos.Latitude/1000) % 10000), 4, 1,0);

                                write_ndigit_number_u (10, 4, (uint16_t)((currpos.Latitude/10) % 100), 2, 1,0);

                                write_ndigit_number_u (12, 4, (uint16_t)(currpos.Longitude/10000000), 2, 0,0);

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

                                write_ndigit_number_u (15, 4, (uint16_t)((currpos.Longitude/1000) % 10000), 4, 1,0);

                                write_ndigit_number_u (19, 4, (uint16_t)((currpos.Longitude/10) % 100), 2, 1,0);

                                lcd_printp_at (0, 5, PSTR("!!!not implemented!!!"), 2);

                                // Akku Warnung

                                if (naviData->UBat < Config.MK_LowBat)

                                { //Beeper ein

                                        set_beep ( 3000, 0x0020, BeepNormal);

                                }

                                if (naviData->UBat > Config.MK_LowBat)  //bei kurzzeitigen Schwankungen Beeper erst wieder aus wenn UBat 0,2 V höher als Warnschwelle

                                { //Beeper aus

                                    set_beep ( 0, 0, BeepOff);

                                }

//                               Akku Warnung Ende

                                old_FCFlags = naviData->FCStatusFlags;

                                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;

                        FM_error = 1;

                }

        }

        while (!get_key_press (1 << KEY_ESC));

        OSD_active = false;

}