| 0,0 → 1,318 |
|---|
| /* |
| * 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.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 |
| |
| |
| // 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; |
| |
| |
| { |
| |
| |
| // Altitude Control |
| OSD_Screen_Element (0, 3, OSD_ALTITUDE_CONTROL); |
| // |
| // // Altitude |
| // OSD_Screen_Element (11, 3, OSD_ALTITUDE); |
| |
| // Battery level |
| OSD_Screen_Element (0, 7, OSD_BATTERY_LEVEL); |
| |
| // Capacity |
| OSD_Screen_Element (13, 7, OSD_CAPACITY); |
| |
| // Care Free |
| OSD_Screen_Element (12, 2, OSD_CARE_FREE); |
| |
| // // 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); |
| |
| // Flying time |
| lcd_printp_at (0, 1, PSTR("Flytime:"), 0); |
| OSD_Screen_Element (8, 1, OSD_FLYING_TIME); |
| |
| // Ground Speed |
| lcd_printp_at (0, 0, PSTR("Speed:"), 0); |
| OSD_Screen_Element (6, 0, OSD_GROUND_SPEED); |
| // Sats in use |
| OSD_Screen_Element (16, 0, OSD_SATS_IN_USE); |
| |
| // // 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); |
| // |
| // // 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); |
| |
| // 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 < MK_LowBat) |
| { //Beeper ein |
| set_beep ( 3000, 0x0020, BeepNormal); |
| } |
| if (naviData->UBat > 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; |
| |
| } |
| |
| |
| |
| |