0,0 → 1,338 |
/* |
* 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; |
|
} |
|
|
|
|
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |