Blame |
Last modification |
View Log
| RSS feed
/*
* 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;
}