Blame |
Last modification |
View Log
| RSS feed
/****************************************************************/
/* */
/* NG-Video 5,8GHz */
/* */
/* Copyright (C) 2011 - gebad */
/* */
/* This code is distributed under the GNU Public License */
/* which can be found at http://www.gnu.org/licenses/gpl.txt */
/* */
/****************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "../cpu.h"
#include <util/delay.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include "../main.h"
#include "../timer/timer.h"
#include "servo_setup.h"
#include "tracking.h"
#include "../lcd/lcd.h"
#include "../timer/timer.h"
#include "../menu.h"
#include "../messages.h"
#include "../mk-data-structs.h"
#include "mymath.h"
#include "../uart/usart.h"
#include "../osd/osd.h"
#include "../eeprom/eeprom.h"
#include "../setup/setup.h"
#include "tools.h"
//#include "ng_usart.h"
//#include "ng_config.h"
//#include "servo.h"
//#include "tools.h"
//#include "mk.h"
//#include "keys.h"
//#include "mymath.h"
//
//GPS_Pos_t last5pos[7];
//uint8_t error1 = 0;
//NaviData_t *naviData;
//HomePos_t MK_pos; // Home position of station
//GPS_Pos_t currentPos; // Current position of flying object
//int8_t satsInUse; // Number of satelites currently in use
//uint8_t tracking = TRACKING_MIN;
//uint8_t track_hyst = TRACKING_HYSTERESE;
//uint8_t track_tx = 0;
//geo_t geo;
//int16_t anglePan, angleTilt; // Servo Winkel
//uint8_t coldstart = 1;
uint8_t servo_nr; // zwischen Servo 1 und 2 wird nur mit global servo_nr unterschieden
//uint8_t FCStatusFlags;
//--------------------------------------------------------------
#define ITEMS_SERVO 4
const prog_char servo_menuitems[ITEMS_SERVO][NUM_LANG][18]= // Zeilen,Zeichen+1
{
{"Servoschritte ","servo steps ","servo steps "},
{"Servo 1 \x1d","servo 1 \x1d","servo 1 \x1d"},
{"Servo 2 \x1d","servo 2 \x1d","servo 2 \x1d"},
{"Servotest \x1d","servotest \x1d","servotest \x1d"},
};
//--------------------------------------------------------------
#define ITEMS_SERVOTEST 4
const prog_char servotest_menuitems[ITEMS_SERVOTEST][NUM_LANG][18]= // Zeilen,Zeichen+1
{
{"Test Pulslänge ","test puls width","test puls width "},
{"Test fortlaufend\x1d","test cont. \x1d","test cont. \x1d"},
{"Servo ","servo ","servo "},
{"Periode ","frame ","frame "},
};
//--------------------------------------------------------------
#define ITEMS_SERVOTEST_CONT 5
const prog_char servotest_cont_menuitems[ITEMS_SERVOTEST_CONT][NUM_LANG][18]= // Zeilen,Zeichen+1
{ {"Start Test ","start test ","start test "},
{"Einzelschritt ","single step ","single step "},
{"Anzahl Test ","number of test ","number of test "},
{"Pause Endposition","pause end pos ","pasue end pos "},
{"Pause pro Inc. ","pause proc inc. ","pause proc inc. "},
};
//--------------------------------------------------------------
#define ITEMS_SERVOADJUST 4
const prog_char servo_adjust_menuitems[ITEMS_SERVOADJUST][NUM_LANG][18]= // Zeilen,Zeichen+1
{
{"Reverse ","reverse ","reverse "},
{"Links ","left ","left "},
{"Rechts ","right ","rigth "},
{"Mitte ","middle ","middle "},
};
///************************************************************************************/
///* */
///* Ändern der Werte mit Tasten +,- und Anzeige */
///* z.B. für U-Offset, Batterie leer Eingabe ... */
///* */
///* Parameter: */
///* uint16_t val :zu ändernter Wert */
///* uint16_t min_val, max_val :min, max Grenze Wert ändern darf */
///* uint8_t posX, posY :Darstellung Wert xPos, YPos auf LCD */
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */
///* uint8_t cycle :0 begrenzt Anzeige bei man_val, bzw. max_val */
///* :1 springt nach max_val auf min_val und umgedreht */
///* uint8_t vrepeat :beschleunigte Repeat-Funktion aus/ein */
///* uint16_t Change_Value_plmi(...) :Rückgabe geänderter Wert */
///* */
///************************************************************************************/
void Servo_tmp_Original(uint8_t track)
{
servoSetDefaultPos();
// tracking = track; // ursprünglicher Wert Tracking aus, RSSI oder GPS
// NoTracking_ServosOff(); // Servos sind nur zum Tracking oder bei Kalibrierung eingeschaltet
// Jump_Menu(pmenu);
}
uint8_t Servo_tmp_on(uint8_t servo_period)
{
// uint8_t tmp_tracking = tracking;
// tracking = 0; // Servopositionierung durch tracking abschalten
// if (tracking == TRACKING_MIN) servoInit(servo_period); // falls aus, Servos einschalten
servoInit(servo_period);
// lcdGotoXY(0, 0); // lcd Cursor vorpositionieren
// return(tmp_tracking);
return (0);
}
void Displ_Off_On(uint16_t val)
{
if (val == 0) lcd_puts_at(17, 2, strGet(OFF), 0); else lcd_puts_at(17, 2, strGet(ON), 0);
}
uint16_t Change_Value_plmi(uint16_t val, uint16_t min_val, uint16_t max_val, uint8_t posX, uint8_t posY,Displ_Fnct_t Displ_Fnct)
{
uint16_t tmp_val;
// >> Menueauswahl nach oben
tmp_val = val;
if (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3))
{
if (val < max_val) {
edit = 1;
val++;
}
else
{
val = min_val;
}
Displ_Fnct(val); // geänderten Wert darstellen, je nach Menüpunkt
}
// >> Menueauswahl nach unten
if (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3))
{
if (val > min_val) {
val--;
edit = 1;
}
else
{
val = max_val;
}
Displ_Fnct(val); // geänderten Wert darstellen, je nach Menüpunkt
}
return(val);
}
//
///************************************************************************************/
///* */
///* Ändern der Werte mit Tasten +,- repetierend; (long)Entertaste und Anzeige */
///* z.B. für U-Offset, Batterie leer Eingabe ... */
///* */
///* Parameter: */
///* uint16_t *val :zu ändernter Wert */
///* uint16_t min_val, max_val :min, max Grenze Wert ändern darf */
///* uint8_t fl_pos :Bit 7 beschleunigte Repeat-Funktion aus/ein */
///* Bit 6 zyklische Werteänderung aus/ein */
///* Bit 4-5 z.Z. ohne Funktion */
///* Bit 0-3 Wert xPos auf LCD */
///* Displ_Fnct_t Displ_Fnct :Index um variable Display Funktion aufzurufen */
///* uint8_t Change_Value(...) :Rückgabe geändert ergibt TRUE */
///* */
///************************************************************************************/
//// Bei Bedarf könnte einfach innerhalp fl_pos auch noch pos_y (Bit 4-5) übergeben werden
uint8_t Change_Value(uint16_t *val, uint16_t min_val, uint16_t max_val,Displ_Fnct_t Displ_Fnct)
{ uint16_t tmp_val;
tmp_val = *val;
Displ_Fnct(tmp_val); // initiale Wertdarstellung, je nach Menüpunkt
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC))
*val = Change_Value_plmi(*val, min_val, max_val, 16,2, Displ_Fnct);
if (*val == tmp_val) {
edit = 0;
// lcd_printp_at (0, 5, PSTR("Edit=0"), 0);
// _delay_ms(500);
//// return (*val);
}
//
else
{
edit = 1;
// lcd_printp_at (0, 5, PSTR("Edit=1"), 0);
// _delay_ms(500);
}
return (tmp_val != *val);
}
uint16_t calc_range(int16_t PosProzent, int16_t min, int16_t max, int16_t mid)
{ uint16_t range;
if (PosProzent < 0) {
range = mid - min;
// if (chrxs == CHRRS) { // falls Richtung geändert, anderen Zeichensatz laden
// chrxs = CHRLS;
//// lcdWriteCGRAM_Array(lcdSpecialChrLs, 5);// LCD-Char mit Rahmensymbole vom Graph
// }
}
else {
range = max - mid;
// if (chrxs == CHRLS) { // falls Richtung geändert, anderen Zeichensatz laden
//// lcdWriteCGRAM_Array(lcdSpecialChrRs, 5);// LCD-Char mit Rahmensymbole vom Graph
// chrxs = CHRRS;
// }
}
return(range);
}
/************************************************************************************/
/* zeigt einen max. 3-stelligen Integerwert auf Display an */
/* Parameter: */
/* uint16_t val :anzuzeigender Wert, */
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
/* */
/************************************************************************************/
void Displ_Format_Int(uint16_t val)
{
// lcdPuts(my_itoa(val, 3, 0, 0));
// lcdPuts(my_itoa(mid_val, 4, 0, 0));
lcd_puts_at(16,2,my_itoa(val, 3, 0, 0),0);
}
void Displ_PulseWidth(uint16_t val)
{ int16_t PosProzent, range;
uint16_t Pos_us;
char me[3] = {"ms"};
servoSetPositionRaw(servo_nr, val);
PosProzent = val - steps_pw[Config.sIdxSteps].mid;
range = calc_range(PosProzent, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max, steps_pw[Config.sIdxSteps].mid);
// draw_bar(PosProzent, range, 2); // auf 3. Display-Zeile
PosProzent = (int32_t)1000 * PosProzent / range;
// lcdGotoXY(1, 1);
Pos_us = pw_us(val); // Zeit in µs bei x Servoschritte
if (Pos_us < 1000) {
me[0] = 'u'; // soll 'µ' => programmierbarer Zeichensatz zu klein
// lcdPuts(" ");
Displ_Format_Int(Pos_us);
}
else {
// lcdPuts(my_itoa(Pos_us, 5, 3, 3));
lcd_puts_at(14,2,my_itoa(Pos_us, 5, 3, 3),0);
}
// lcdPuts(me);
// lcdGotoXY(8, 1);
// lcdPuts(my_itoa(PosProzent, 6, 1, 1));
lcd_puts_at(14,2,my_itoa(PosProzent, 6, 1, 1),0);
// lcdPutc('%');
}
/************************************************************************************/
/* zeigt Pausenlänge der Links-, Mittel- und Rechtsposition auf Display an */
/* Parameter: */
/* uint16_t val : Zeit in 1ms * 100 */
/* */
/************************************************************************************/
void Displ_Pause(uint16_t val)
{
if (val > 9) {
// lcdPuts(my_itoa(val, 3, 1, 1));
lcd_puts_at(16,2,my_itoa(val, 3, 1, 1),0);
// lcdPuts("s ");
}
else {
Displ_Format_Int(val * 100);
// lcdPuts("ms");
}
}
/************************************************************************************/
/* zeigt aus oder Integerwert auf Display an */
/* Parameter: */
/* uint16_t val : val = 0 ==> aus, sont Integerwert */
/* */
/************************************************************************************/
void Displ_Off_Format_Int(uint16_t val)
{
if (val == 0)
// lcdPutStrMid(Msg(MSG_OFF), ZLE_VAL);
lcd_puts_at(17, 2, strGet(OFF), 0);
else {
// write_ndigit_number_u (16, 2, val, 5, 0,0);
// lcdGotoXY(5,ZLE_VAL);
Displ_Format_Int(val);
// lcdPutc(' ');
}
}
/************************************************************************************/
/* zeigt aus oder Pausenzeit zwischen 2 Servoschritte auf Display an */
/* Parameter: */
/* uint16_t val : val = 0 ==> aus, sont Integerwert */
/* */
/************************************************************************************/
void Displ_Pause_Step(uint16_t val)
{
Displ_Off_Format_Int(val);
if (val > 0) {
// lcdGotoXY(8,ZLE_VAL);
// lcdPuts("ms");
}
}
/************************************************************************************/
/* zeigt zu testende Servonummer zur Auswahl auf Display an */
/* Parameter: */
/* uint16_t val :0 = Servo 1 oder 1 = Servo 2, */
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
/* */
/************************************************************************************/
void Displ_ServoNr(uint16_t val)
{
// if (val == 0) lcdPuts(Msg(MSG_SERVO1)); else lcdPuts(Msg(MSG_SERVO2));
lcd_printp_at (0, 2, PSTR("Servo:"), 0);
if (val == 0) lcd_printp_at (14, 2, PSTR("Servo 1"), 0); else lcd_printp_at (14, 2, PSTR("Servo 2"), 0);
}
/**************************/
/* */
/* Servos-Tests */
/* */
/**************************/
//void Menu_Servo_Test(void)
//{ uint8_t scr_sub_menu[SCROLL_MAX_6] = {SCROLL_MAX_6 - 2, MSG_RETURN, MSG_PULSE_WIDTH, MSG_CONTINOUS, MSG_SERVO, MSG_FRAME};
//
// Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global
// servo_nr = eeprom_read_byte(&ep_servo_nr);
// Jump_Menu(pmenu);
//}
void Menu_Test_Frame(void)
{ uint16_t tmp_val;
// Displ_Title(MSG_FRAME);
// lcdGotoXY(8, ZLE_VAL);
lcd_cls ();
lcd_puts_at(0, 0, strGet(SV_TEST3),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
// lcdPuts("ms");
tmp_val = Config.servo_frame;
if (Change_Value(&tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX,Displ_Format_Int)) { // pmenu global
Config.servo_frame = tmp_val;
// eeprom_write_byte(&ep_servo_frame, servo_frame);
// Double_Beep(DBEEPWR, DBEEPWRP);
}
// Jump_Menu(pmenu);
}
void Menu_Test_ServoNr(void)
{ uint16_t tmp_val;
// Displ_Title(MSG_SERVO);
lcd_cls ();
lcd_puts_at(0, 0, strGet(SV_TEST2),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_val = servo_nr;
if (Change_Value(&tmp_val, 0, 1,Displ_ServoNr)) { // pmenu global; es gibt nur 0=Servo1, 1=Servo2
servo_nr = tmp_val;
// eeprom_write_byte(&ep_servo_nr, servo_nr);
// Double_Beep(DBEEPWR, DBEEPWRP);
}
// Jump_Menu(pmenu);
}
// Dieser Test im raw-Modus ohne Anschlagkalibrierung (normiert) z.B.: für Modelleinstellungen ohne Empfänger
void Menu_Test_PulseWidth(void)
{ uint8_t tmp_tracking;
uint16_t tmp_val;
lcd_cls ();
lcd_puts_at(0, 0, strGet(SERVO_TEST1),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_tracking = Servo_tmp_on(Config.servo_frame);
// lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph
// chrxs = CHRLS; // verhindert wiederholtes Lesen bereits geladener LCD-Char
// Displ_Title(MSG_PULSE_WIDTH);
tmp_val = steps_pw[Config.sIdxSteps].mid;
Change_Value(&tmp_val, steps_pw[Config.sIdxSteps].min, steps_pw[Config.sIdxSteps].max,Displ_PulseWidth); // pmenu global
// lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char für Bargraph zurückschreiben
cli();
servoInit(SERVO_PERIODE);
sei();
Servo_tmp_Original(tmp_tracking);
}
//void Menu_Test_Continuous(void)
//{ uint8_t scr_sub_menu[SCROLL_MAX_7] = {SCROLL_MAX_7 - 2, MSG_RETURN, MSG_START, MSG_SINGLE_STEP, MSG_REPEAT, MSG_PAUSE, MSG_PAUSE_STEP};
//
// Scroll_Menu(scr_sub_menu, m_pkt); // pmenu global
// Jump_Menu(pmenu);
//}
void Menu_Test_SingleStep(void)
{uint16_t tmp_val;
//TODO:
// Displ_Title(MSG_SINGLE_STEP);
lcd_cls ();
tmp_val = Config.single_step;
lcd_puts_at(0, 0, strGet(SV_SINGLESTEP),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
if (Change_Value(&tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int)) { // pmenu global
Config.single_step = tmp_val;
// eeprom_write_byte(&ep_single_step, single_step);
// Double_Beep(DBEEPWR, DBEEPWRP);
}
// Jump_Menu(pmenu);
}
void Menu_Test_Repeat(void)
{uint16_t tmp_val;
//TODO:
// Displ_Title(MSG_REPEAT);
tmp_val = Config.repeat;
lcd_cls ();
lcd_puts_at(0, 0, strGet(SV_COUNTTEST),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
if (Change_Value(&tmp_val, REPEAT_MIN, REPEAT_MAX,Displ_Format_Int)) { // pmenu global
Config.repeat = tmp_val;
// eeprom_write_byte(&ep_repeat, repeat);
// Double_Beep(DBEEPWR, DBEEPWRP);
}
// Jump_Menu(pmenu);
}
void Menu_Test_Pause(void)
{uint16_t tmp_val;
//TODO:
// Displ_Title(MSG_PAUSE);
lcd_cls ();
lcd_puts_at(0, 0, strGet(SV_PAUSEEND),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_val = Config.pause;
if (Change_Value(&tmp_val, PAUSE_MIN, PAUSE_MAX,Displ_Pause)) { // pmenu global
Config.pause = tmp_val;
// eeprom_write_byte(&ep_pause, pause);
// Double_Beep(DBEEPWR, DBEEPWRP);
}
// Jump_Menu(pmenu);
}
void Menu_Test_Pause_Step(void)
{uint16_t tmp_val;
//TODO:
// Displ_Title(MSG_PAUSE_STEP);
lcd_cls ();
lcd_puts_at(0, 0, strGet(SV_PAUSEINC),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_val = Config.pause_step;
if (Change_Value(&tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX,Displ_Pause_Step)) { // pmenu global
Config.pause_step = tmp_val;
// eeprom_write_byte(&ep_pause_step, pause_step);
// Double_Beep(DBEEPWR, DBEEPWRP);
}
// Jump_Menu(pmenu);
}
int8_t calc_dir(uint8_t idx, int16_t *Position)
{ uint8_t nextIdx;
int8_t nextDir = 1;
nextIdx = idx;
if ((idx + 1) < POSIDX_MAX)
nextIdx++;
else
nextIdx = 0;
if (Position[PosIdx[idx]] > Position[PosIdx[nextIdx]]) nextDir = -1;
return(nextDir);
}
void Displ_LoopCounter(uint8_t val)
{
// lcdGotoXY(2,2);
// lcdPuts(Msg(MSG_COUNTER));
// lcdPuts(my_itoa(val, 4, 0, 0));
lcd_puts_at(16,2,my_itoa(val, 4, 0, 0),0);
}
// Test über Scalierung der Servos mit Anschlagkalibrierung
void Menu_Test_Start(void)
{ uint8_t tmp_tracking, idx, rep;
int8_t dir;
int16_t sPos;
int16_t Position[3];
int16_t range;
tmp_tracking = Servo_tmp_on(Config.servo_frame);
// lcdWriteCGRAM_Array(lcdSpecialChrLs, 8); // LCD-Char mit Rahmensymbole vom Graph
// chrxs = CHRLS; // Flag, welche Kästchensymbole geladen
// Displ_Title(MSG_CONTINOUS);
lcd_cls ();
lcd_puts_at(0, 0, strGet(SV_TESTCONT),2);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
Displ_LoopCounter(Config.repeat);
Position[0] = 0; // skalierte Servoposition aber unterschiedliche Schrittanzahl möglich
Position[1] = ServoSteps()/2;
Position[2] = ServoSteps();
// init Einzelschritt
idx = 0;
dir = calc_dir(idx, Position);
sPos = Position[PosIdx[idx]];
idx++;
rep = Config.repeat;
// Test bis Ende der Wiederholungen oder irgendein Enter
while(!get_key_press(1<<KEY_ENTER) && !get_key_press(1<<KEY_ESC)) {
range = calc_range(sPos - Position[1], Position[0], Position[2], Position[1]);
// draw_bar(sPos - Position[1], range, 1); // eingerahmter Balkengraph auf 2. Display-Zeile
servoSetPosition(servo_nr, sPos);
if ( sPos != Position[PosIdx[idx]]) { // Links-, Mittel- oder Rechtsposotion erreicht?
sPos += (Config.single_step * dir); // variable Schrittweite subtrahieren oder addieren
if (((dir < 0) && (sPos < Position[PosIdx[idx]])) || ((dir > 0) && (sPos > Position[PosIdx[idx]])) || !(Config.single_step))
sPos = Position[PosIdx[idx]]; // Überlauf bei variabler Schrittweite berücksichtigen oder Einzelschritt
Delay_ms(Config.servo_frame + 1 + Config.pause_step);// Bei Schrittweite um 1 würden welche übersprungen, zusätzlich pro Servoschritt verzögert
}
else {
dir = calc_dir(idx, Position); // Richtungsänderung
if (idx < (POSIDX_MAX - 1)) {
if (idx == 0) {
rep--; // bei jeden vollen Durchlauf Wiederholzähler verringern
Displ_LoopCounter(rep);
}
idx++; // Index für nächsten Positionswert ==> Array PosIdx[] bestimmt Anschlagreihenfolge
}
else
idx = 0;
delay_ms100x(Config.pause); // variable Pause bei Links-, Mittel- und Rechtsposotion Mindestzeit 400ms (Servolauf)
}
}
// lcdClear();
// if (pmenu[0] == '\0')
// Displ_Main_Disp();
// else
// return_m_pkt(strlen(pmenu)); // um bei Rücksprung auf ursprünglichen Menüpunkt zeigen oder Displ_Main_Disp()
// lcdWriteCGRAM_Array(lcdSpecialChr, 7); // LCD-Char für Bargraph zurückschreiben
cli();
servoInit(SERVO_PERIODE);
sei();
Servo_tmp_Original(tmp_tracking);
}
//--------------------------------------------------------------
void test_servo_menu(void)
{
// uint8_t ii = 0;
// uint8_t Offset = 0;
// uint8_t dmode = 0;
uint8_t target_pos = 1;
// uint8_t val = 0;
while(1)
{
size = ITEMS_SERVOTEST;
lcd_cls ();
lcd_printpns_at(0, 0, PSTR("test_servo_menu "), 2);
// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(2)
// {
// ii = 0;
// if(Offset > 0)
// {
// lcd_printp_at(1,1, PSTR("\x12"), 0);
// }
// for(ii = 0;ii < 6 ; ii++)
// {
// if((ii+Offset) < size)
// {
// lcd_printp_at(3,ii+1,servotest_menuitems[ii+Offset][Config.DisplayLanguage], 0);
// }
// if((ii == 5)&&(ii+Offset < (size-1)))
// {
// lcd_printp_at(1,6, PSTR("\x13"), 0);
// }
// }
// if(dmode == 0)
// {
// if(Offset == 0)
// {
// if(size > 6)
// {
// val = menu_choose2 (1, 5, target_pos,0,1);
// }
// else
// {
// val = menu_choose2 (1, size, target_pos,0,0);
// }
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(dmode == 1)
// {
// if(Offset+7 > size)
// {
// val = menu_choose2 (2, 6, target_pos,1,0);
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(val == 254)
// {
// Offset++;
// dmode = 1;
// target_pos = 5;
// }
// else if(val == 253)
// {
// Offset--;
// dmode = 0;
// target_pos = 2;
// }
// else if(val == 255)
// {
// return;
// }
// else
// {
// break;
// }
// }
val = menu_select(servotest_menuitems,size,target_pos);
if (val==255) break;
target_pos = val;
// {"Test Pulslänge ","test puls width","test puls width "},
// {"Test fortlaufend\x1d","test cont. \x1d","test cont. \x1d"},
// {"Servo ","servo ","servo "},
// {"Periode ","frame ","frame "},
if(val == 1 )
Menu_Test_PulseWidth();
if(val == 2 )
servotest_cont_menu();
if(val == 3 )
Menu_Test_ServoNr();
if(val == 4 )
Menu_Test_Frame();
}
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void servotest_cont_menu(void)
{
// uint8_t ii = 0;
// uint8_t Offset = 0;
// uint8_t dmode = 0;
uint8_t target_pos = 1;
// uint8_t val = 0;
while(1)
{
size = ITEMS_SERVOTEST_CONT;
lcd_cls ();
lcd_printpns_at(0, 0, PSTR("Servotest cont."), 2);
// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(2)
// {
// ii = 0;
// if(Offset > 0)
// {
// lcd_printp_at(1,1, PSTR("\x12"), 0);
// }
// for(ii = 0;ii < 6 ; ii++)
// {
// if((ii+Offset) < size)
// {
// lcd_printp_at(3,ii+1,servotest_cont_menuitems[ii+Offset][Config.DisplayLanguage], 0);
// }
// if((ii == 5)&&(ii+Offset < (size-1)))
// {
// lcd_printp_at(1,6, PSTR("\x13"), 0);
// }
// }
// if(dmode == 0)
// {
// if(Offset == 0)
// {
// if(size > 6)
// {
// val = menu_choose2 (1, 5, target_pos,0,1);
// }
// else
// {
// val = menu_choose2 (1, size, target_pos,0,0);
// }
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(dmode == 1)
// {
// if(Offset+7 > size)
// {
// val = menu_choose2 (2, 6, target_pos,1,0);
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(val == 254)
// {
// Offset++;
// dmode = 1;
// target_pos = 5;
// }
// else if(val == 253)
// {
// Offset--;
// dmode = 0;
// target_pos = 2;
// }
// else if(val == 255)
// {
// if (edit == 1)
// {
//// WriteParameter();
// edit = 0;
// return;
// }
// return;
// }
// else
// {
// break;
// }
// }
val = menu_select(servotest_cont_menuitems,size,target_pos);
if (val==255) break;
target_pos = val;
if(val == 1 )
Menu_Test_Start();
if(val == 2 )
Menu_Test_SingleStep();
if(val == 3)
Menu_Test_Repeat();
if(val == 4 )
Menu_Test_Pause();
if(val == 5)
Menu_Test_Pause_Step();
}
}
//--------------------------------------------------------------
void Servo_rev(void)
{ uint16_t tmp_val;
uint8_t tmp_tracking;
lcd_puts_at(0, 2, strGet(SERVO_REVERSE),0);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
tmp_val = Config.servo[servo_nr].rev;
if (Change_Value(&tmp_val , 0, 1, Displ_Off_On))
{ //reverse gibt es nur 0=off, 1=on
Config.servo[servo_nr].rev = tmp_val ;
servoSet_rev(servo_nr, tmp_val );
}
Servo_tmp_Original(tmp_tracking);
}
void Menu_Servotext(void)
{
lcd_cls ();
if (servo_nr == 0)
lcd_puts_at(0, 0, strGet(SERVO1_TEXT), 2);
else
lcd_puts_at(0, 0, strGet(SERVO2_TEXT), 2);
}
void Menu_Servo_rev(void)
{
Menu_Servotext();
Servo_rev();
}
/********************************************************************************/
/* zeigt Servo-Anschlagposition links auf Display an */
/* mit sofortiger Wirkung auf Servo */
/* Parameter: */
/* uint16_t val :anzuzeigender Wert, */
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
/* */
/********************************************************************************/
void Displ_Servo_Min(uint16_t val)
{ uint16_t steps = 0;
// Displ_Format_Int(val);
write_ndigit_number_s (16, 2, val, 5, 0,0);
servoSet_min(servo_nr, val); // Wert setzen damit nachfolgend die
if (Config.servo[servo_nr].rev) steps = ServoSteps();
servoSetPosition(servo_nr, steps); // Änderung direkt am Servo sichtbar ist
}
/************************************************************************************/
/* zeigt Servo-Anschlagposition rechts auf Display an */
/* mit sofortiger Wirkung auf Servo */
/* Parameter: */
/* uint16_t val :anzuzeigender Wert, */
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
/* */
/************************************************************************************/
void Displ_Servo_Max(uint16_t val)
{ uint16_t steps = ServoSteps();
// Displ_Format_Int(val);
write_ndigit_number_u (16, 2, val, 5, 0,0);
servoSet_max(servo_nr, val); // Wert setzen damit nachfolgend die
if (Config.servo[servo_nr].rev) steps = 0;
servoSetPosition(servo_nr, steps); // Änderung direkt am Servo sichtbar ist
}
/************************************************************************************/
/* zeigt Servo-Anschlagposition Mitte auf Display an */
/* mit sofortiger Wirkung auf Servo */
/* Parameter: */
/* uint16_t val :anzuzeigender Wert, */
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
/* */
/************************************************************************************/
void Displ_Servo_Mid(uint16_t val)
{ int16_t mid_val;
mid_val = val - ServoSteps()/2;
// lcdPuts(my_itoa(mid_val, 4, 0, 0));
lcd_puts_at(16,2,my_itoa(mid_val, 4, 0, 0),0);
servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die
servoSetPosition(servo_nr, ServoSteps()/2); // Änderung direkt am Servo sichtbar ist
}
void Servo_left(void)
{ uint16_t tmp_val;
uint8_t tmp_tracking;
lcd_puts_at(0, 2, strGet(SERVO_LEFT),0);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
servoSetPosition(servo_nr, ServoSteps()); // Linkssanschlag um Kalibrierung am Servo zu sehen
tmp_val = Config.servo[servo_nr].max;
// if (Change_Value(&tmp_val, servo_limit[sIdxSteps][LEFT].min, servo_limit[sIdxSteps][LEFT].max, 6|(1<<V_REPEAT), Displ_Servo_Max)) { // pmenu global
if (Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max))
{
Config.servo[servo_nr].max = tmp_val;
// eeprom_write_block(&servo[servo_nr],&ep_servo[servo_nr],sizeof(servo_t));
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlagsänderung verschieben
// Double_Beep(DBEEPWR, DBEEPWRP);
}
Servo_tmp_Original(tmp_tracking);
}
void Menu_Servo_left(void)
{
// Displ_Title(MSG_CALIB1_LEFT);
Menu_Servotext();
Servo_left();
}
void Servo_right(void)
{ uint16_t tmp_val;
uint8_t tmp_tracking;
lcd_puts_at(0, 2, strGet(SERVO_RIGTH),0);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
servoSetPosition(servo_nr, 0); // Rechtsanschlag um Kalibrierung am Servo zu sehen
tmp_val = Config.servo[servo_nr].min;
// if (Change_Value(&tmp_val, servo_limit[sIdxSteps][RIGHT].min, servo_limit[sIdxSteps][RIGHT].max, 6|(1<<V_REPEAT), Displ_Servo_Min)) { // pmenu global
if (Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min))
{Config.servo[servo_nr].min = tmp_val;
// eeprom_write_block(&servo[servo_nr],&ep_servo[servo_nr],sizeof(servo_t));
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlagsänderung verschieben
// Double_Beep(DBEEPWR, DBEEPWRP);
}
Servo_tmp_Original(tmp_tracking);
}
void Menu_Servo_rigth(void)
{
Menu_Servotext();
Servo_right();
}
void Servo_middle(void)
{ uint16_t tmp_val;
uint8_t tmp_tracking;
lcd_puts_at(0, 2, strGet(SERVO_MID),0);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_tracking = Servo_tmp_on(SERVO_PERIODE);
servoSetPosition(servo_nr, ServoSteps()/2); // Mittelposition um Kalibrierung am Servo zu sehen
tmp_val = Config.servo[servo_nr].mid;
// if (Change_Value(&tmp_val, servo_limit[sIdxSteps][MIDDLE].min, servo_limit[sIdxSteps][MIDDLE].max, 5|(1<<V_REPEAT), Displ_Servo_Mid)) { // pmenu global
if (Change_Value(&tmp_val , servo_limit[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid))
{ Config.servo[servo_nr].mid = tmp_val;
// eeprom_write_block(&servo[servo_nr], &ep_servo[servo_nr], sizeof(servo_t));
// Double_Beep(DBEEPWR, DBEEPWRP);
}
Servo_tmp_Original(tmp_tracking);
}
void Menu_Servo_mid(void)
{
Menu_Servotext();
Servo_middle();
}
void Servo_NewValues(uint8_t idx_presc)
{
for (uint8_t i = 0; i < SERVO_NUM_CHANNELS; i++) {
if (idx_presc == STEPS_255) { // Werte umrechnen für Prescaler = 256
Config.servo[i].min /= 4;
Config.servo[i].max /= 4;
Config.servo[i].mid /= 4;
}
else { // Werte umrechnen für Prescaler = 64
Config.servo[i].min *= 4;
Config.servo[i].max *= 4;
Config.servo[i].mid = (Config.servo[i].mid + 1) * 4 - 1;
}
servoSet_min(i, Config.servo[i].min);
servoSet_max(i, Config.servo[i].max);
servoSet_mid(i, Config.servo[i].mid);
// eeprom_write_block(&servo[i],&ep_servo[i],sizeof(servo_t));
}
// Vorberechnung von ServoChannels[channel].duty
servoSetDefaultPos(); // Ausgangsstellung beider Servos
}
/************************************************************************************/
/* zeigt Servoschritte zur Auswahl auf Display an */
/* Parameter: */
/* uint16_t val :0 = 255 oder 1 = 1023, */
/* uint16_t wegen Vereinheitlichung f. Funktionsaufrauf */
/* */
/************************************************************************************/
void Displ_Servo_Steps(uint16_t val)
{
if (val==0)
lcd_puts_at(16,2,INTERNAT_STEPS_255,0 );
else
lcd_puts_at(16,2,INTERNAT_STEPS_1023,0 );
}
void Menu_Servo_Steps(void)
{ uint16_t tmp_val;
lcd_cls ();
lcd_puts_at(0, 0, strGet(SERVOSTEPS), 2);
lcd_puts_at(0, 2, strGet(SERVOSTEPS),0);
lcd_puts_at(0, 7, strGet(KEYLINE2), 0);
tmp_val = Config.sIdxSteps;
if (Change_Value(&tmp_val, STEPS_255, STEPS_1023,Displ_Servo_Steps))
{
cli();
Config.sIdxSteps = tmp_val;
Servo_NewValues(Config.sIdxSteps); // hier ist der neue Index anzugeben!
servoInit(SERVO_PERIODE);
sei();
}
}
void adjust_servo_menu(uint8_t servo)
{
// uint8_t ii = 0;
// uint8_t Offset = 0;
// uint8_t dmode = 0;
uint8_t target_pos = 1;
// uint8_t val = 0;
char ServoNr;
servo_nr = servo;
if (servo_nr == 0) ServoNr = '1'; else ServoNr = '2';
while(1)
{
size = ITEMS_SERVOADJUST;
lcd_cls ();
lcd_printpns_at(0, 0, PSTR("adjust servo "), 2);
lcd_putc (18, 0, ServoNr, 2);
lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(2)
// {
// ii = 0;
// if(Offset > 0)
// {
// lcd_printp_at(1,1, PSTR("\x12"), 0);
// }
// for(ii = 0;ii < 6 ; ii++)
// {
// if((ii+Offset) < size)
// {
// lcd_printp_at(3,ii+1,servo_adjust_menuitems[ii+Offset][Config.DisplayLanguage], 0);
// }
// if((ii == 5)&&(ii+Offset < (size-1)))
// {
// lcd_printp_at(1,6, PSTR("\x13"), 0);
// }
// }
// if(dmode == 0)
// {
// if(Offset == 0)
// {
// if(size > 6)
// {
// val = menu_choose2 (1, 5, target_pos,0,1);
// }
// else
// {
// val = menu_choose2 (1, size, target_pos,0,0);
// }
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(dmode == 1)
// {
// if(Offset+7 > size)
// {
// val = menu_choose2 (2, 6, target_pos,1,0);
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(val == 254)
// {
// Offset++;
// dmode = 1;
// target_pos = 5;
// }
// else if(val == 253)
// {
// Offset--;
// dmode = 0;
// target_pos = 2;
// }
// else if(val == 255)
// {
// if (edit == 1)
// {
//
//// WriteParameter();
// edit = 0;
// return;
// }
// return;
// }
// else
// {
// break;
// }
// }
val = menu_select(servo_adjust_menuitems,size,target_pos);
if (val==255) break;
target_pos = val;
// {"Reverse ","reverse ","reverse "},
// {"Links ","left ","left "},
// {"Rechts ","right ","rigth "},
// {"Mitte ","middle ","middle "},
if(val == 1 )
{
Menu_Servo_rev();
}
if(val == 2 )
{
Menu_Servo_left();
}
if(val == 3 )
{
Menu_Servo_rigth();
}
if(val == 4 )
{
Menu_Servo_mid();
}
}
}
//--------------------------------------------------------------
void servo_menu(void)
{
// uint8_t ii = 0;
// uint8_t Offset = 0;
// uint8_t dmode = 0;
uint8_t target_pos = 1;
// uint8_t val = 0;
edit =0;
while(1)
{
size = ITEMS_SERVO;
lcd_cls ();
lcd_printpns_at(0, 0, PSTR("servo_menu"), 2);
// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(2)
// {
// ii = 0;
// if(Offset > 0)
// {
// lcd_printp_at(1,1, PSTR("\x12"), 0);
// }
// for(ii = 0;ii < 6 ; ii++)
// {
// if((ii+Offset) < size)
// {
// lcd_printp_at(3,ii+1,servo_menuitems[ii+Offset][Config.DisplayLanguage], 0);
// }
// if((ii == 5)&&(ii+Offset < (size-1)))
// {
// lcd_printp_at(1,6, PSTR("\x13"), 0);
// }
// }
// if(dmode == 0)
// {
// if(Offset == 0)
// {
// if(size > 6)
// {
// val = menu_choose2 (1, 5, target_pos,0,1);
// }
// else
// {
// val = menu_choose2 (1, size, target_pos,0,0);
// }
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(dmode == 1)
// {
// if(Offset+7 > size)
// {
// val = menu_choose2 (2, 6, target_pos,1,0);
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(val == 254)
// {
// Offset++;
// dmode = 1;
// target_pos = 5;
// }
// else if(val == 253)
// {
// Offset--;
// dmode = 0;
// target_pos = 2;
// }
// else if(val == 255)
// {
// if (edit == 1)
// {
//// WriteParameter();
// edit = 0;
// return;
// }
// return;
// }
// else
// {
// break;
// }
// }
val = menu_select(servo_menuitems,size,target_pos);
if (val==255) break;
target_pos = val;
// Edit_generic(uint8_t Value, uint8_t min, uint8_t max,uint8_t Text, uint8_t what
// if(val == 1 ) sIdxSteps=Edit_generic (sIdxSteps, STEPS_255, STEPS_1023,SERVOSTEPS,0);
if(val == 1 ) Menu_Servo_Steps();
if(val == 2 )
adjust_servo_menu(0);
if(val == 3 )
adjust_servo_menu(1);
if(val == 4 )
test_servo_menu();
}
}
//--------------------------------------------------------------
//--------------------------------------------------------------
//void start_tracking(void)
//{
// #define TIMEOUT 200 // 2 sec
//
// uint16_t old_anglePan = 0;
// uint16_t old_angleTilt = 0;
//
// //uint16_t old_hh = 0;
// uint8_t flag;
// uint8_t tmp_dat;
//
// lcd_cls ();
// //lcd_printpns_at(0, 0, PSTR("start_tracking "), 2);
//
// //lcd_printpns_at(0, 1, PSTR("ab jetzt Tracking"), 0);
//
// lcd_ecircle(22, 35, 16, 1);
// lcd_ecircle(88, 35, 16, 1);
// lcd_putc (10, 1, 0x1e, 0); // degree symbol
// lcd_putc (20, 1, 0x1e, 0); // degree symbol
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// 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);
//
// if (rxd_buffer_locked)
// {
// timer = TIMEOUT;
// Decode64 ();
// naviData = (NaviData_t *) pRxData;
//
// if(error1 == 1)
// lcd_cls();
// }
//
// GPS_Pos_t currpos;
// currpos.Latitude = naviData->CurrentPosition.Latitude;
// currpos.Longitude = naviData->CurrentPosition.Longitude;
//
// flag = 0;
// timer = TIMEOUT;
// abo_timer = ABO_TIMEOUT;
//
// coldstart = 1;
//
// do
// {
// if (rxd_buffer_locked)
// {
// timer = TIMEOUT;
// Decode64 ();
// naviData = (NaviData_t *) pRxData;
//
//
////CB uint8_t FCStatusFlag = naviData->FCFlags;
// uint8_t FCStatusFlag = naviData->FCStatusFlags;
// //write_ndigit_number_u (0, 0, FCStatusFlag);
//
// Tracking_GPS();
//
// //uint16_t heading_home = (naviData->HomePositionDeviation.Bearing + 360 - naviData->CompassHeading) % 360;
//
// // alte Linien löschen
// //lcd_ecirc_line (22, 35, 15, old_hh, 0);
// //old_hh = heading_home;
// lcd_ecirc_line (22, 35, 15, old_anglePan, 0);
// old_anglePan = anglePan;
// lcd_ecirc_line (88, 35, 15, old_angleTilt, 0);
// old_angleTilt = angleTilt;
//
// lcd_ecirc_line (22, 35, 15, anglePan, 1);
// write_ndigit_number_u (7, 1, anglePan, 3, 0,0);
// lcd_ecirc_line (88, 35, 15, angleTilt, 1);
// write_ndigit_number_u (17, 1, angleTilt, 3, 0,0);
//
// 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;
// }
// }
// while(!get_key_press (1 << KEY_ESC));
//
// //lcd_cls();
// //return;
//}
//--------------------------------------------------------------
//
//void conect2at_unit(void)
//{
// lcd_cls ();
// lcd_printpns_at(0, 0, PSTR("conect2at_unit "), 2);
//
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2);
//
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
//
// while(!get_key_press (1 << KEY_ESC));
//
// lcd_cls();
// return;
//}
//
////--------------------------------------------------------------
//
//void conect2gps_ser (void)
//{
// lcd_cls ();
// lcd_printpns_at(0, 0, PSTR("conect2gps_ser "), 2);
//
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2);
//
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(!get_key_press (1 << KEY_ESC));
//
// lcd_cls();
// return;
//}
//
////--------------------------------------------------------------
//
//void conect2gps_bt (void)
//{
// lcd_cls ();
// lcd_printpns_at(0, 0, PSTR("conect2gps_bt "), 2);
//
// lcd_printpns_at(0, 3, PSTR("work in progress ;)"), 2);
//
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(!get_key_press (1 << KEY_ESC));
//
// lcd_cls();
// return;
//}
//--------------------------------------------------------------
//void conect2gps_menu(void)
//{
// uint8_t ii = 0;
// uint8_t Offset = 0;
// uint8_t size = ITEMS_CONECT_GPS;
// uint8_t dmode = 0;
// uint8_t target_pos = 1;
// uint8_t val = 0;
//
// while(1)
// {
// lcd_cls ();
// lcd_printpns_at(0, 0, PSTR("conect2gps_menu "), 2);
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(2)
// {
// ii = 0;
// if(Offset > 0)
// {
// lcd_printp_at(1,1, PSTR("\x12"), 0);
// }
// for(ii = 0;ii < 6 ; ii++)
// {
// if((ii+Offset) < size)
// {
// lcd_printp_at(3,ii+1,conect_gps_menuitems[ii+Offset], 0);
// }
// if((ii == 5)&&(ii+Offset < (size-1)))
// {
// lcd_printp_at(1,6, PSTR("\x13"), 0);
// }
// }
// if(dmode == 0)
// {
// if(Offset == 0)
// {
// if(size > 6)
// {
// val = menu_choose2 (1, 5, target_pos,0,1);
// }
// else
// {
// val = menu_choose2 (1, size, target_pos,0,0);
// }
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(dmode == 1)
// {
// if(Offset+7 > size)
// {
// val = menu_choose2 (2, 6, target_pos,1,0);
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(val == 254)
// {
// Offset++;
// dmode = 1;
// target_pos = 5;
// }
// else if(val == 253)
// {
// Offset--;
// dmode = 0;
// target_pos = 2;
// }
// else if(val == 255)
// {
// return;
// }
// else
// {
// break;
// }
// }
// target_pos = val;
//
// if((val+Offset) == 1 )
// conect2gps_ser();
// if((val+Offset) == 2 )
// conect2gps_bt();
// }
//}
//--------------------------------------------------------------
//void tracking_menu(void)
//{
// uint8_t ii = 0;
// uint8_t Offset = 0;
// uint8_t size = ITEMS_AT;
// uint8_t dmode = 0;
// uint8_t target_pos = 1;
// uint8_t val = 0;
//
// while(1)
// {
// lcd_cls ();
// lcd_printpns_at(1, 0, PSTR("Tracking Men\x06 V.01 "), 2);
//// lcd_printp_at (0, 7, PSTR(KEY_LINE_1), 0);
// lcd_puts_at(0, 7, strGet(KEYLINE1), 0);
// while(2)
// {
// ii = 0;
// if(Offset > 0)
// {
// lcd_printp_at(1,1, PSTR("\x12"), 0);
// }
// for(ii = 0;ii < 6 ; ii++)
// {
// if((ii+Offset) < size)
// {
// lcd_printp_at(3,ii+1,at_menuitems[ii+Offset], 0);
// }
// if((ii == 5)&&(ii+Offset < (size-1)))
// {
// lcd_printp_at(1,6, PSTR("\x13"), 0);
// }
// }
// if(dmode == 0)
// {
// if(Offset == 0)
// {
// if(size > 6)
// {
// val = menu_choose2 (1, 5, target_pos,0,1);
// }
// else
// {
// val = menu_choose2 (1, size, target_pos,0,0);
// }
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(dmode == 1)
// {
// if(Offset+7 > size)
// {
// val = menu_choose2 (2, 6, target_pos,1,0);
// }
// else
// {
// val = menu_choose2 (2, 5, target_pos,1,1);
// }
// }
// if(val == 254)
// {
// Offset++;
// dmode = 1;
// target_pos = 5;
// }
// else if(val == 253)
// {
// Offset--;
// dmode = 0;
// target_pos = 2;
// }
// else if(val == 255)
// {
// return;
// }
// else
// {
// break;
// }
// }
// target_pos = val;
//
// if((val+Offset) == 1 )
// test_servo_menu();
// if((val+Offset) == 2 )
// adjust_servo_menu();
// if((val+Offset) == 3 )
// show_angle();
// if((val+Offset) == 4 )
////TODO: start_tracking();
// if((val+Offset) == 5 )
// conect2at_unit();
// if((val+Offset) == 6 )
// conect2gps_menu();
// }
//}
//--------------------------------------------------------------
// kapeschi Ant.Treking Funktionen
//--------------------------------------------------------------
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
// zur aktuellen Position(nach Motorstart)
//geo_t calc_geo(HomePos_t *home, GPS_Pos_t *pos)
//{ int32_t lat1, lon1, lat2, lon2;
// int32_t d1, dlat;
// geo_t geo;
//
// lon1 = MK_pos.Home_Lon;
// lat1 = MK_pos.Home_Lat;
// lon2 = pos->Longitude;
// lat2 = pos->Latitude;
//
// // Formel verwendet von http://www.kompf.de/gps/distcalc.html
// // 111.3 km = Abstand zweier Breitenkreise und/oder zweier Längenkreise am Äquator
// // es wird jedoch in dm Meter weiter gerechnet
// // (tlon1 - tlon2)/10) sonst uint32_t-Überlauf bei cos(0) gleich 1
// d1 = (1359 * (int32_t)(c_cos_8192((lat1 + lat2) / 20000000)) * ((lon1 - lon2)/10))/ 10000000;
// dlat = 1113 * (lat1 - lat2) / 10000;
// geo.bearing = (my_atan2(d1, dlat) + 540) % 360; // 360 +180 besserer Vergleich mit MkCockpit
// geo.distance = sqrt32(d1 * d1 + dlat * dlat);
// return(geo);
//}
//void do_tracking(void)
//{ //static uint8_t hysteresis = 0;
// // aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
// // (4 * (........))/5 ==> Wichtung Luftdruck-Höhe zu GPS
// currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
//
// geo = calc_geo(&MK_pos, ¤tPos);
// angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance);
// //if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschlägt
// //geo.bearing = MK_pos.direction;
// //angleTilt = 0;
// //hysteresis = 1;
// //}
// //else {
// //hysteresis = 0;
// //}
////
// //// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen
// //// 360 grd negative Winkelwerte als positive
// anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90
//
// //if (angleTilt < 0) angleTilt = 0;
// //if (angleTilt > 180) angleTilt = 180;
////
// //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo
// //anglePan = anglePan - 180;
// //angleTilt = 180 - angleTilt;
// //
// //}
////angleTilt = 180;
////angleTilt = 180;
//
//// servoSetAngle(0, anglePan);
//// servoSetAngle(1, angleTilt);
//}
/****************************************************************/
/* */
/* MK GPS Tracking */
/* */
/****************************************************************/
// MK OSD-Daten lesen und verifizieren
//uint8_t OSD_Data_valid(NaviData_t **navi_data)
//{ uint8_t ret = 0;
//char *tx_osd = {"#co?]==EH\r"};
//// char interval[2] = {10, '\0'};
//
//if (rx_line_decode('O')) { // OSD-Datensatz prüfen/dekodieren
////*navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen
//if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok.
//currentPos = (*navi_data)->CurrentPosition;
//if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK)
//ret = 1;
//// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136
//// (4 * (........))/5 ==> Wichtung Luftdruck-Höhe zu GPS
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
//satsInUse = (*navi_data)->SatsInUse;
//}
//}
//// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK
//// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval
//if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval
//return ret;
//}
//
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel
// Hauptprogramm von GPS Antennen-Nachführung
//void Tracking_GPS(void)
//{ //NaviData_t *navidata;
// static uint8_t track_running = 0;
//
// if (!track_running)
// {
// //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!!
// //if (OSD_Data_valid(&naviData)) {
// if (coldstart)
// {
// //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen
// //if (naviData->FCFlags & FC_FLAG_MOTOR_START)
// //{
// MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0;
// MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0;
// MK_pos.Home_Lon7 = naviData->HomePosition.Longitude;
// MK_pos.Home_Lat7 = naviData->HomePosition.Latitude;
// MK_pos.Home_Alt = naviData->HomePosition.Altitude;
// MK_pos.direction = naviData->CompassHeading;
// coldstart = 0;
// //}
// //}
// //else {
// //do_tracking();
// }
// //}
// track_running = 0;
// }
// do_tracking();
//}