• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

ProjectsBL-CtrlFlightCtrlMK3MagNaviCtrl

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/ @ 2135

  → /Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/ @ 2136

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 2135 → Rev 2136

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/ng_servo.c
0,0 → 1,236
/********************************************************************/
/* */
/* 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 */
/* */
/* using */
/*! \file servo.c \brief Interrupt-driven RC Servo function library.*/
/* */
/*File Name : 'servo.c' */
/*Title : Interrupt-driven RC Servo function library */
/*Author : Pascal Stang - Copyright (C) 2002 */
/*Created : 7/31/2002 */
/*Revised : 8/02/2002 */
/*Version : 1.0 */
/*Target MCU : Atmel AVR Series */
/*Editor Tabs : 2 */
/* */
/*This code is distributed under the GNU Public License */
/* which can be found at http://www.gnu.org/licenses/gpl.txt */
/* */
/********************************************************************/
//############################################################################
//# HISTORY ng_servo.c
//#
//# 25.08.2013 Cebra
//# - add: #ifdef USE_TRACKING
//#
//# 24.04.2013 OG
//# - chg: umbenannt servo_test(void) nach _servo_test(void)
//# das verursachte in meinem PKT-Projekt Errors weil die Funktion
//# servo_test() ebenfalls ein servo.c vorhanden ist
//############################################################################
#include <avr/interrupt.h>
#include <avr/io.h>
#include "../tracking/ng_servo.h"
#include "../tracking/tracking.h"
#include "../HAL_HW3_9.h"
#include "../eeprom/eeprom.h"
//#include "config.h"
#ifdef USE_TRACKING
// Global variables
uint16_t ServoPosTics;
uint16_t ServoPeriodTics;
// servo channel registers
ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
const ServoConst_t ServoConst[2] = {{SERVO_MAX, SERVO_MIN, SERVO_STEPS, SERVO_PRESCALER},
{SERVO_MAX * 4, SERVO_MIN * 4, (SERVO_STEPS + 1) * 4 - 1, SERVO_PRESCALER / 4}};
// Servo limits (depend on hardware)
const servo_limit_t servo_limit[2][3] = {{{SERVO_I0_RIGHT_MIN, SERVO_I0_RIGHT_MAX},
{SERVO_I0_LEFT_MIN, SERVO_I0_LEFT_MAX},
{SERVO_I0_MIDDLE_MIN, SERVO_I0_MIDDLE_MAX}},
{{SERVO_I1_RIGHT_MIN, SERVO_I1_RIGHT_MAX},
{SERVO_I1_LEFT_MIN, SERVO_I1_LEFT_MAX},
{SERVO_I1_MIDDLE_MIN, SERVO_I1_MIDDLE_MAX}}};
// Servopositionen normiert für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1
const steps_pw_t steps_pw[2] = {{(uint64_t)950*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*F_CPU*1e-6/SERVO_PRESCALER + 0.5,(uint64_t)1500*F_CPU*1e-6/SERVO_PRESCALER + 0.5},
{(uint64_t)950*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)2050*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5, (uint64_t)1500*4*F_CPU*1e-6/SERVO_PRESCALER + 0.5}};
// anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX
const uint8_t PosIdx[POSIDX_MAX] = {1, 0, 1, 2 };
// functions
void _servo_test(void)
{
//Dummy
}
//! initializes software PWM system 16-bit Timer
// normaler Weise wird ein Serv-PWM Signal aller 20ms wiederholt
// Werte: rev, min, max, mid vorher über servoSet...() initialisieren und einmal servoSetPosition(...) ausführen!!!
void servoInit(uint8_t servo_period)
{ uint16_t OCValue; // set initial interrupt time
// disble Timer/Counter1, Output Compare A Match Interrupt
TIMSK1 &= ~(1<<OCIE1A);
// set the prescaler for timer1
if (Config.sIdxSteps == STEPS_255) {
TCCR1B &= ~((1<<CS11) | (1<<CS10));
TCCR1B |= (1<<CS12); // prescaler 256, Servo-Schritte 185 bei 180 grd Winkel
}
else {
TCCR1B &= ~(1<<CS12);
TCCR1B |= (1<<CS11) | (1<<CS10); // prescaler 64, Servo-Schritte 740 bei 180 grd Winkel
}
// attach the software PWM service routine to timer1 output compare A
// timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
// enable channels
for(uint8_t channel=0; channel < SERVO_NUM_CHANNELS; channel++) {
// set default pins assignments SERVO2 Pin 4; SERVO1 Pin 5
ServoChannels[channel].pin = (1 << (SERVO2 + channel));
}
ServoPosTics = 0; // set PosTics
// set PeriodTics
ServoPeriodTics = F_CPU / ServoConst[Config.sIdxSteps].prescaler * servo_period * 1e-3;
// read in current value of output compare register OCR1A
OCValue = OCR1AL; // read low byte of OCR1A
OCValue += (OCR1AH << 8); // read high byte of OCR1A
OCValue += ServoPeriodTics; // increment OCR1A value by nextTics
// set future output compare time to this new value
OCR1AH = OCValue >> 8; // write high byte
OCR1AL = OCValue & 0x00FF; // write low byte
TIMSK1 |= (1<<OCIE1A); // enable the timer1 output compare A interrupt
coldstart = 1;
}
void servoSetDefaultPos(void)
{
servoSetPosition(SERVO_PAN, ServoSteps()); // Ausgangsstellung SERVO_PAN
servoSetPosition(SERVO_TILT,ServoSteps()); // Ausgangsstellung SERVO_TILT
}
uint16_t pw_us(uint16_t Steps)
{
return(Steps * ServoConst[Config.sIdxSteps].prescaler/(F_CPU * 1e-6) + 0.5); // Zeit pro Schritt (Wert * e-1) in µs
}
uint16_t ServoSteps(void)
{
return(ServoConst[Config.sIdxSteps].steps);
}
void servoSet_rev(uint8_t channel, uint8_t val)
{
ServoChannels[channel].rev = val & 0x01;
}
void servoSet_min(uint8_t channel, uint16_t min)
{
ServoChannels[channel].min = ServoConst[Config.sIdxSteps].min + min;
}
void servoSet_max(uint8_t channel, uint16_t max)
{
ServoChannels[channel].max = ServoConst[Config.sIdxSteps].max - max;
}
void servoSet_mid(uint8_t channel, uint16_t mid)
{
ServoChannels[channel].mid = mid;
// Faktor 16, bzw. 16/2 um mit einer Nachkommastelle zu Rechnen
ServoChannels[channel].mid_scaled = (8 * (ServoChannels[channel].max - ServoChannels[channel].min) + \
(16 * mid - 8 * ServoConst[Config.sIdxSteps].steps))/16 + ServoChannels[channel].min;
}
//! turns off software PWM system
void servoOff(void)
{
// disable the timer1 output compare A interrupt
TIMSK1 &= ~(1<<OCIE1A);
}
//! set servo position on channel (raw unscaled format)
void servoSetPositionRaw(uint8_t channel, uint16_t position)
{
// bind to limits
if (position < ServoChannels[channel].min) position = ServoChannels[channel].min;
if (position > ServoChannels[channel].max) position = ServoChannels[channel].max;
// set position
ServoChannels[channel].duty = position;
}
//! set servo position on channel
// input should be between 0 and ServoSteps() (entspricht Maximalausschlag)
void servoSetPosition(uint8_t channel, uint16_t position)
{ uint16_t pos_scaled;
// calculate scaled position
if (ServoChannels[channel].rev != 0) position = ServoConst[Config.sIdxSteps].steps - position;
if (position < ServoConst[Config.sIdxSteps].steps/2)
//bei Position < Servomittelstellung Position*(Mitte - Min)/(Servoschritte/2)
pos_scaled = ServoChannels[channel].min + ((int32_t)position*2*(ServoChannels[channel].mid_scaled-ServoChannels[channel].min))/ \
ServoConst[Config.sIdxSteps].steps;
else
//bei Position >= Servomittelstellung
pos_scaled = ServoChannels[channel].mid_scaled + (uint32_t)(position - ServoConst[Config.sIdxSteps].steps / 2) \
* 2 * (ServoChannels[channel].max-ServoChannels[channel].mid_scaled)/ServoConst[Config.sIdxSteps].steps;
// set position
servoSetPositionRaw(channel, pos_scaled);
}
// Umrechnung Winkel in Servoschritte
void servoSetAngle(uint8_t servo_nr, int16_t angle)
{
servoSetPosition(servo_nr, (uint16_t)((uint32_t)angle * ServoConst[Config.sIdxSteps].steps / 180));
}
//Interruptroutine
ISR(TIMER1_COMPA_vect)
{ static uint8_t ServoChannel;
uint16_t nextTics;
uint16_t OCValue; // schedule next interrupt
if(ServoChannel < SERVO_NUM_CHANNELS) {
PORTD &= ~ServoChannels[ServoChannel].pin; // turn off current channel
}
ServoChannel++; // next channel
if(ServoChannel != SERVO_NUM_CHANNELS) {
// loop to channel 0 if needed
if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0;
// turn on new channel
PORTD |= ServoChannels[ServoChannel].pin;
// schedule turn off time
nextTics = ServoChannels[ServoChannel].duty;
}
else {
// ***we could save time by precalculating this
// schedule end-of-period
nextTics = ServoPeriodTics-ServoPosTics;
}
// read in current value of output compare register OCR1A
OCValue = OCR1AL; // read low byte of OCR1A
OCValue += (OCR1AH <<8); // read high byte of OCR1A
OCValue += nextTics; // increment OCR1A value by nextTics
// set future output compare time to this new value
OCR1AH = OCValue >> 8; // write high byte
OCR1AL = OCValue & 0x00FF; // write low byte
ServoPosTics += nextTics; // set our new tic position
if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
}
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/ng_servo.h
0,0 → 1,190
/*********************************************************************/
/* */
/* 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 */
/* */
/* using */
/*! \file servo.c \brief Interrupt-driven RC Servo function library. */
/* */
/*File Name : 'servo.c' */
/*Title : Interrupt-driven RC Servo function library */
/*Author : Pascal Stang - Copyright (C) 2002 */
/*Created : 7/31/2002 */
/*Revised : 8/02/2002 */
/*Version : 1.0 */
/*Target MCU : Atmel AVR Series */
/*Editor Tabs : 4 */
/* */
/*ingroup driver_sw */
/*defgroup servo Interrupt-driven RC Servo Function Library (servo.c)*/
/*code #include "servo.h" \endcode */
/*par Overview */
/* */
/*This code is distributed under the GNU Public License */
/*which can be found at http://www.gnu.org/licenses/gpl.txt */
/* */
/*********************************************************************/
#ifndef SERVO_H
#define SERVO_H
/* Servo */
#define SERVO_PAN 0
#define SERVO_TILT 1
#define SERVO_NUM_CHANNELS 2 // Anzahl der angeschlossen Servos max. 2!!!
/* Servokalibrierungen derzeit zu SERVO_STEPS = 255 skaliert */
//prescaler 256
#define SERVO_I0_RIGHT 45 // default Wert, ca. 0,9ms
#define SERVO_I0_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position
#define SERVO_I0_RIGHT_MAX 100 // SERVO_MIN + SERVO_RIGHT
#define SERVO_I0_LEFT 45 // default Wert, ca. 2,1ms
#define SERVO_I0_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position
#define SERVO_I0_LEFT_MAX 100 // SERVO_MAX - SERVO_LEFT
#define SERVO_I0_MIDDLE SERVO_STEPS/2
#define SERVO_I0_MIDDLE_MIN SERVO_STEPS/2 - 25
#define SERVO_I0_MIDDLE_MAX SERVO_STEPS/2 + 25
//prescaler 64
#define SERVO_I1_RIGHT 180 // default Wert, ca. 0,9ms
#define SERVO_I1_RIGHT_MIN 0 // Servokalibrierung Grenze der linken Position
#define SERVO_I1_RIGHT_MAX 400 // SERVO_MIN + SERVO_RIGHT
#define SERVO_I1_LEFT 180 // default Wert, ca. 2,1ms
#define SERVO_I1_LEFT_MIN 0 // Servokalibrierung Grenze der rechten Position
#define SERVO_I1_LEFT_MAX 400 // SERVO_MAX - SERVO_LEFT
//#define SERVO_I1_MIDDLE ((SERVO_STEPS + 1) * 4 - 1)/2
#define SERVO_I1_MIDDLE_MIN ((SERVO_STEPS + 1) * 4 - 1)/2 - 100
#define SERVO_I1_MIDDLE_MAX ((SERVO_STEPS + 1) * 4 - 1)/2 + 100
#define SERVO_REV 0 // kein Reverse
/* Test Servo */
#define SERVO_PERIODE 20 // default Angabe in ms
#define SERVO_PERIODE_MIN 10 // 10ms
#define SERVO_PERIODE_MAX 30 // 30ms
#define SINGLE_STEP 0 // Einzelschritt aus
#define SINGLE_STEP_MIN 0
#define SINGLE_STEP_MAX 20 // bei prescaler 256, sonst * 4 (von links nach rechts in 9 Abschnitte)
// zwischen den Schritten muss Pause > der Servoperiode sein, sonst keine Aktualisierung
#define REPEAT 1
#define REPEAT_MIN 1
#define REPEAT_MAX 100
#define PAUSE 10
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms
#define PAUSE_STEP 0
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms
#define PAUSE_STEP_MAX 200
// The numbers below good for parallax servos at an F_CPU of 20.000MHz.
// Da einige Servo's auch eien Winkel von 180 grd zulassen, Wertebereich
// entgegen den sonst üblichen. Einschränkung mit default Kalibrierung
// auf 0,9ms (45) bis 2,1ms(45) gesetzt. Je nach Servo, entspricht einen
// Winkel von etwa 180grd
// Periode default 20ms
#define SERVO_MAX 211 // 2,7 ms bei prescaler 256, bei prescaler 64 SERVO_MAX * 4
#define SERVO_MIN 26 // 0,33ms bei prescaler 256, bei prescaler 64 SERVO_MIN * 4
#define SERVO_STEPS 255 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle
#define SERVO_PRESCALER 256 // bei prescaler 256, bei prescaler 64 SERVO_PRESCALER / 4
#define STEPS_255 0 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle
#define STEPS_1023 1 // Servo-Schritte von links nach rechts, Anschlagkalibrierung spielt keine Rolle
typedef struct //Servo-Konstante je nach Prescaler
{
uint16_t max;
uint16_t min;
uint16_t steps;
uint16_t prescaler;
}ServoConst_t;
typedef struct //struct_ServoChannel
{
uint8_t pin; // hardware I/O port and pin for this channel
uint16_t duty; // PWM duty setting which corresponds to servo position
uint8_t rev; // Parameter, wie on/off; reverse; range
uint16_t min; // SERVO_MIN + Parameter min
uint16_t max; // SERVO_MAX - Parameter max
uint16_t mid_scaled; // skalierte Servomitte
int16_t mid; // Servomitte = SERVO_STEPS/2 +/- x Schritte; bei Pescaler 256 wird nur uint8_t benötigt aber bei 64
}ServoChannelType;
//uint8_t sIdxSteps; // 0 für 255 und 1 für 1023 Schritte; Prescaler 256 oder 64
// Define servo limits (depend on hardware)
typedef struct {
uint16_t min;
uint16_t max;
}servo_limit_t;
extern const servo_limit_t servo_limit[2][3];
// Define servo positions (depend on hardware)
typedef struct {
uint16_t min;
uint16_t max;
uint16_t mid;
}steps_pw_t;
typedef struct {
uint8_t rev;
uint16_t min;
uint16_t max;
uint16_t mid;
} servo_t;
// Servopositionen für 950µs, 2,05ms und 1,5ms ==> Ergebnis Schritte. Da Zeit in µs ist F_CPU*e-1
extern const steps_pw_t steps_pw[2];
#define RIGHT 0 // Servopostionen, welche vom Einbau abhängig sind
#define LEFT 1
#define MIDDLE 2
#define POSIDX_MAX 4
extern const uint8_t PosIdx[POSIDX_MAX]; // anzufahrende Servopositionen 0=MIN, 1=MID, 2=MAX
// functions
void servoSetDefaultPos(void);
// initializes servo system
// You must run this to begin servo control
void servoInit(uint8_t servo_period);
// turns off servo system
// This stops controlling the servos and
// returns control of the SERVOPORT to your code
void servoOff(void);
// set servo position on a given channel
// servoSetPosition() commands the servo on <channel> to the position you
// desire. The position input must lie between 0 and POSITION_MAX and
// will be automatically scaled to raw positions between SERVO_MIN and
// SERVO_MAX
void servoSetPosition(uint8_t channel, uint16_t position);
// set raw servo position on a given channel
// Works like non-raw commands but position is not scaled. Position must
// be between SERVO_MIN and SERVO_MAX
void servoSetPositionRaw(uint8_t channel, uint16_t position);
// set servo to a specific angle
void servoSetAngle(uint8_t servo_nr, int16_t angle);
// vor servoInit(), oder vor sei() ServoWerte mit servoSet...() initialisieren, einschließlich servoSetPosition(...)!
void servoSet_rev(uint8_t channel, uint8_t val);
void servoSet_min(uint8_t channel, uint16_t min);
void servoSet_max(uint8_t channel, uint16_t max);
void servoSet_mid(uint8_t channel, uint16_t mid);
uint16_t pw_us(uint16_t Steps); // gibt Zeit in µs bei x Servoschritte
uint16_t ServoSteps(void); // gibt "Konstante" derzeitiger Servoschritte zürück
#endif /* SERVO_H */
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/servo_setup.c
0,0 → 1,1577
/****************************************************************/
/* */
/* 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 */
/* */
/****************************************************************/
//############################################################################
//# HISTORY servo_setup.c
//#
//# 27.06.2014 OG
//# - del: #include "tools.h"
//# - del: #include "mymath.h"
//#
//# 30.05.2014 OG
//# - chg: etliche Edit-Anzeigen/Funktionen auf neues Layout umgeschrieben;
//# einige Optimierungen an Aufruffunktionen (unnoetige Menue-
//# Interfacefunktionen entfernt)
//#
//# 13.05.2014 OG
//# - chg: Menu_Test_Start() - Variable 'range' auskommentiert
//# wegen Warning: variable set but not used
//# - chg: Displ_PulseWidth() - Variable 'me' auskommentiert
//# wegen Warning: variable set but not used
//# - chg: Change_Value_plmi() - Variable 'tmp_val' entfernt
//# wegen Warning: variable set but not used
//#
//# 11.05.2014 OG
//# - chg: einge Setup-Menues umgestellt auf MenuCtrl_SetTitleFromParentItem()
//# -> die Menues 'erben' damit ihren Titel vom aufrufenden Menuepunkt
//#
//# 30.03.2014 OG
//# - chg: ein paar englische Texte auf DE gemappt weil der Unterschied unerheblich war
//# - chg: Sprache Hollaendisch vollstaendig entfernt
//# - chg: MenuCtrl_PushML_P() umgestellt auf MenuCtrl_PushML2_P()
//#
//# 25.08.2013 Cebra
//# - add: #ifdef USE_TRACKING
//#
//# 30.05.2013 cebra
//# - chg: doppelte Texte auf #define umgestellt
//#
//# 24.05.2013 OG
//# - chg: Aufrufe von MenuCtrl_Push() umgestellt auf MenuCtrl_PushML_P()
//#
//# 22.05.2013 OG
//# - umgestellt auf menuctrl: Setup_ServoTracking(), Setup_ServoTest(),
//# Setup_ServoTestCont(), Setup_ServoAdjust()
//# - del: include utils/menuold.h
//#
//# 17.05.2013 OG
//# - chg: umgestellt auf utils/menuold.h
//#
//# 16.04.2013 Cebra
//# - chg: PROGMEM angepasst auf neue avr-libc und GCC, prog_char depreciated
//#
//# 04.04.2013 Cebra
//# - chg: Texttuning
//#
//# 03.04.2013 Cebra
//# - chg: Holländische Menütexte
//#
//# 27.03.2013 Cebra
//# - chg: Fehler bei der Men�steuerung behoben
//############################################################################
#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 "../uart/usart.h"
#include "../osd/osd.h"
#include "../eeprom/eeprom.h"
#include "../setup/setup.h"
#include "../utils/menuctrl.h"
#include "../pkt/pkt.h"
#ifdef USE_TRACKING
//#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;
//############################################################################
//-----------------------------
// Setup_ServoTracking()
//-----------------------------
#define MSERVO_STEPS 1
#define MSERVO_SERVO1 2
#define MSERVO_SERVO2 3
#define MSERVO_SERVOTEST 4
static const char MSERVO_STEPS_de[] PROGMEM = "Servoschritte";
static const char MSERVO_STEPS_en[] PROGMEM = "servo steps";
static const char MSERVO_SERVO1_de[] PROGMEM = "Servo 1";
#define MSERVO_SERVO1_en MSERVO_SERVO1_de
static const char MSERVO_SERVO2_de[] PROGMEM = "Servo 2";
#define MSERVO_SERVO2_en MSERVO_SERVO2_de
static const char MSERVO_SERVOTEST_de[] PROGMEM = "Servotest";
#define MSERVO_SERVOTEST_en MSERVO_SERVOTEST_de
//############################################################################
//-----------------------------
// Setup_ServoTest()
//-----------------------------
#define MSERVOT_PULS 1
#define MSERVOT_CONT 2
#define MSERVOT_SERVO 3
#define MSERVOT_FRAME 4
static const char MSERVOT_PULS_de[] PROGMEM = "Test Pulslänge";
static const char MSERVOT_PULS_en[] PROGMEM = "test puls width";
static const char MSERVOT_CONT_de[] PROGMEM = "Test Fortlfd.";
static const char MSERVOT_CONT_en[] PROGMEM = "test cont.";
static const char MSERVOT_SERVO_de[] PROGMEM = "Servo";
#define MSERVOT_SERVO_en MSERVOT_SERVO_de
static const char MSERVOT_FRAME_de[] PROGMEM = "Periode";
static const char MSERVOT_FRAME_en[] PROGMEM = "frame";
//############################################################################
//-----------------------------
// Setup_ServoTestCont()
//-----------------------------
#define MSERVOTC_START 1
#define MSERVOTC_SINGLE 2
#define MSERVOTC_COUNT 3
#define MSERVOTC_PAUSEEND 4
#define MSERVOTC_PAUSEINC 5
static const char MSERVOTC_START_de[] PROGMEM = "Start Test";
static const char MSERVOTC_START_en[] PROGMEM = "start test";
static const char MSERVOTC_SINGLE_de[] PROGMEM = "Einzelschritt";
static const char MSERVOTC_SINGLE_en[] PROGMEM = "single step";
static const char MSERVOTC_COUNT_de[] PROGMEM = "Anzahl Test";
static const char MSERVOTC_COUNT_en[] PROGMEM = "number of test";
static const char MSERVOTC_PAUSEEND_de[] PROGMEM = "Pause Endpos.";
static const char MSERVOTC_PAUSEEND_en[] PROGMEM = "pause end pos";
static const char MSERVOTC_PAUSEINC_de[] PROGMEM = "Pause pro Inc.";
static const char MSERVOTC_PAUSEINC_en[] PROGMEM = "pause proc inc.";
//############################################################################
//-----------------------------
// Setup_ServoAdjust()
//-----------------------------
#define MSERVOADJ_REV 1
#define MSERVOADJ_LEFT 2
#define MSERVOADJ_RIGHT 3
#define MSERVOADJ_MID 4
static const char MSERVOADJ_REV_de[] PROGMEM = "Reverse";
static const char MSERVOADJ_REV_en[] PROGMEM = "reverse";
static const char MSERVOADJ_LEFT_de[] PROGMEM = "Links";
static const char MSERVOADJ_LEFT_en[] PROGMEM = "left";
static const char MSERVOADJ_RIGHT_de[] PROGMEM = "Rechts";
static const char MSERVOADJ_RIGHT_en[] PROGMEM = "right";
static const char MSERVOADJ_MID_de[] PROGMEM = "Mitte";
static const char MSERVOADJ_MID_en[] PROGMEM = "middle";
//############################################################################
//--------------------------------------------------------------
//--------------------------------------------------------------
void draw_input_screen( void )
{
lcd_cls ();
PKT_TitleFromMenuItem( true );
lcdx_printf_at_P( 0, 2, MNORMAL, 0,0, PSTR("%S:"), MenuCtrl_GetItemText() ); // Menuetext muss im PGM sein! (aktuell keine Unterscheidung RAM/PGM)
lcd_printp_at(0, 7, strGet(KEYLINE2), MNORMAL);
}
///************************************************************************************/
///* */
///* �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_printp_at(17, 2, strGet(OFF), 0); else lcd_printp_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)
{
// >> Menueauswahl nach oben
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 )
{
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), val );
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Displ_PulseWidth( uint16_t val )
{
int16_t PosProzent, range;
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);
PosProzent = (int32_t)1000 * PosProzent / range;
lcdx_printf_at_P( 15, 2, MNORMAL, 0,0, PSTR("%4.1d"), PosProzent );
}
/************************************************************************************/
/* 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)
{
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%1.1d"), val );
//lcdPuts("s ");
}
/************************************************************************************/
/* 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 )
{
lcd_printp_at( 17, 2, strGet(OFF), MNORMAL);
}
else
{
Displ_Format_Int( val );
}
}
/************************************************************************************/
/* 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 );
}
/************************************************************************************/
/* 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 )
{
Displ_Format_Int( val+1 );
}
/**************************/
/* */
/* 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;
draw_input_screen();
tmp_val = Config.servo_frame;
if( Change_Value( &tmp_val, SERVO_PERIODE_MIN, SERVO_PERIODE_MAX, Displ_Format_Int) )
{
Config.servo_frame = tmp_val;
}
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Menu_Test_ServoNr( void )
{
uint16_t tmp_val;
draw_input_screen();
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;
}
}
//--------------------------------------------------------------
// 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;
draw_input_screen();
// 30.05.2014 OG: macht dieser Code Sinn?
// Change_Value() -> 'tmp_val' und 'tmp_val' ist local - was soll mir das sagen?
tmp_tracking = Servo_tmp_on(Config.servo_frame);
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
cli();
servoInit( SERVO_PERIODE );
sei();
Servo_tmp_Original(tmp_tracking);
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Menu_Test_SingleStep(void)
{
uint16_t tmp_val;
draw_input_screen();
tmp_val = Config.single_step;
if( Change_Value( &tmp_val, SINGLE_STEP_MIN, SINGLE_STEP_MAX, Displ_Off_Format_Int) )
{ // pmenu global
Config.single_step = tmp_val;
}
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Menu_Test_Repeat( void )
{
uint16_t tmp_val;
draw_input_screen();
tmp_val = Config.repeat;
if( Change_Value( &tmp_val, REPEAT_MIN, REPEAT_MAX, Displ_Format_Int) )
{
Config.repeat = tmp_val;
}
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Menu_Test_Pause( void )
{
uint16_t tmp_val;
draw_input_screen();
tmp_val = Config.pause;
if( Change_Value( &tmp_val, PAUSE_MIN, PAUSE_MAX, Displ_Pause) )
{
Config.pause = tmp_val;
}
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Menu_Test_Pause_Step( void )
{
uint16_t tmp_val;
draw_input_screen();
tmp_val = Config.pause_step;
if( Change_Value( &tmp_val, PAUSE_STEP_MIN, PAUSE_STEP_MAX, Displ_Pause_Step) )
{
Config.pause_step = tmp_val;
}
}
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 )
{
lcdx_printf_at_P( 16, 2, MNORMAL, 0,0, PSTR("%4d"), val );
//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 ();
PKT_TitleFromMenuItem( true );
lcd_printp_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)) {
calc_range(sPos - Position[1], Position[0], Position[2], Position[1]);
//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_ms( Config.pause*100 ); // 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);
}
/********************************************************************************/
/* 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;
write_ndigit_number_s( 15, 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();
write_ndigit_number_u( 15, 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;
lcdx_printf_at_P( 17, 2, MNORMAL, 0,0, PSTR("%3d"), mid_val );
servoSet_mid(servo_nr, val); // Wert setzen damit nachfolgend die
servoSetPosition(servo_nr, ServoSteps()/2); // �nderung direkt am Servo sichtbar ist
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Servo_rev( void )
{
uint16_t tmp_val;
uint8_t tmp_tracking;
draw_input_screen();
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 Servo_left( void )
{
uint16_t tmp_val;
uint8_t tmp_tracking;
draw_input_screen();
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[Config.sIdxSteps][LEFT].min, servo_limit[Config.sIdxSteps][LEFT].max, Displ_Servo_Max) )
{
Config.servo[servo_nr].max = tmp_val;
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben
}
Servo_tmp_Original(tmp_tracking);
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Servo_right( void )
{
uint16_t tmp_val;
uint8_t tmp_tracking;
draw_input_screen();
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[Config.sIdxSteps][RIGHT].min, servo_limit[Config.sIdxSteps][RIGHT].max, Displ_Servo_Min) )
{
Config.servo[servo_nr].min = tmp_val;
servoSet_mid(servo_nr, Config.servo[servo_nr].mid); // Mittelposition muss sich bei Ausschlags�nderung verschieben
}
Servo_tmp_Original(tmp_tracking);
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void Servo_middle( void )
{
uint16_t tmp_val;
uint8_t tmp_tracking;
draw_input_screen();
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[Config.sIdxSteps][MIDDLE].min, servo_limit[Config.sIdxSteps][MIDDLE].max, Displ_Servo_Mid) )
{
Config.servo[servo_nr].mid = tmp_val;
}
Servo_tmp_Original(tmp_tracking);
}
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;
draw_input_screen();
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();
}
}
//--------------------------------------------------------------
// Setup_ServoAdjust()
//--------------------------------------------------------------
void Setup_ServoAdjust( uint8_t servo )
{
servo_nr = servo;
//---------------
// Create
//---------------
MenuCtrl_Create();
MenuCtrl_SetTitleFromParentItem(); // "Servo 1" oder "Servo 1"
//MenuCtrl_SetTitle_P( PSTR("Servo Adjust") );
//---------------
// Menuitems
//---------------
MenuCtrl_PushML2_P( MSERVOADJ_REV , MENU_ITEM, &Servo_rev , MSERVOADJ_REV_de , MSERVOADJ_REV_en );
MenuCtrl_PushML2_P( MSERVOADJ_LEFT , MENU_ITEM, &Servo_left , MSERVOADJ_LEFT_de , MSERVOADJ_LEFT_en );
MenuCtrl_PushML2_P( MSERVOADJ_RIGHT , MENU_ITEM, &Servo_right , MSERVOADJ_RIGHT_de , MSERVOADJ_RIGHT_en );
MenuCtrl_PushML2_P( MSERVOADJ_MID , MENU_ITEM, &Servo_middle , MSERVOADJ_MID_de , MSERVOADJ_MID_en );
//---------------
// Control
//---------------
MenuCtrl_Control( MENUCTRL_EVENT );
//---------------
// Destroy
//---------------
MenuCtrl_Destroy();
}
//--------------------------------------------------------------
// Setup_ServoTestCont()
//--------------------------------------------------------------
void Setup_ServoTestCont( void )
{
//---------------
// Create
//---------------
MenuCtrl_Create();
MenuCtrl_SetTitleFromParentItem(); // "Test fortlfd."
//MenuCtrl_SetTitle_P( PSTR("Servotest Cont.") );
//---------------
// Menuitems
//---------------
MenuCtrl_PushML2_P( MSERVOTC_START , MENU_ITEM, &Menu_Test_Start , MSERVOTC_START_de , MSERVOTC_START_en );
MenuCtrl_PushML2_P( MSERVOTC_SINGLE , MENU_ITEM, &Menu_Test_SingleStep , MSERVOTC_SINGLE_de , MSERVOTC_SINGLE_en );
MenuCtrl_PushML2_P( MSERVOTC_COUNT , MENU_ITEM, &Menu_Test_Repeat , MSERVOTC_COUNT_de , MSERVOTC_COUNT_en );
MenuCtrl_PushML2_P( MSERVOTC_PAUSEEND , MENU_ITEM, &Menu_Test_Pause , MSERVOTC_PAUSEEND_de , MSERVOTC_PAUSEEND_en );
MenuCtrl_PushML2_P( MSERVOTC_PAUSEINC , MENU_ITEM, &Menu_Test_Pause_Step , MSERVOTC_PAUSEINC_de , MSERVOTC_PAUSEINC_en );
//---------------
// Control
//---------------
MenuCtrl_Control( MENUCTRL_EVENT );
//---------------
// Destroy
//---------------
MenuCtrl_Destroy();
}
//--------------------------------------------------------------
// Setup_ServoTest()
//--------------------------------------------------------------
void Setup_ServoTest( void )
{
//---------------
// Create
//---------------
MenuCtrl_Create();
MenuCtrl_SetTitleFromParentItem(); // "Servotest"
//---------------
// Menuitems
//---------------
MenuCtrl_PushML2_P( MSERVOT_PULS , MENU_ITEM, &Menu_Test_PulseWidth , MSERVOT_PULS_de , MSERVOT_PULS_en );
MenuCtrl_PushML2_P( MSERVOT_CONT , MENU_SUB , &Setup_ServoTestCont , MSERVOT_CONT_de , MSERVOT_CONT_en );
MenuCtrl_PushML2_P( MSERVOT_SERVO , MENU_ITEM, &Menu_Test_ServoNr , MSERVOT_SERVO_de , MSERVOT_SERVO_en );
MenuCtrl_PushML2_P( MSERVOT_FRAME , MENU_ITEM, &Menu_Test_Frame , MSERVOT_FRAME_de , MSERVOT_FRAME_en );
//---------------
// Control
//---------------
MenuCtrl_Control( MENUCTRL_EVENT );
//---------------
// Destroy
//---------------
MenuCtrl_Destroy();
}
//--------------------------------------------------------------
// Setup_ServoTracking()
//--------------------------------------------------------------
void Setup_ServoTracking( void )
{
uint8_t itemid;
//---------------
// Create
//---------------
MenuCtrl_Create();
MenuCtrl_SetTitleFromParentItem(); // "Tracking"
//---------------
// Menuitems
//---------------
MenuCtrl_PushML2_P( MSERVO_STEPS , MENU_ITEM, &Menu_Servo_Steps , MSERVO_STEPS_de , MSERVO_STEPS_en );
MenuCtrl_PushML2_P( MSERVO_SERVO1 , MENU_SUB , NOFUNC , MSERVO_SERVO1_de , MSERVO_SERVO1_en );
MenuCtrl_PushML2_P( MSERVO_SERVO2 , MENU_SUB , NOFUNC , MSERVO_SERVO2_de , MSERVO_SERVO2_en );
MenuCtrl_PushML2_P( MSERVO_SERVOTEST , MENU_SUB , &Setup_ServoTest , MSERVO_SERVOTEST_de , MSERVO_SERVOTEST_en );
//---------------
// Control
//---------------
while( true )
{
MenuCtrl_Control( MENUCTRL_EVENT );
if( MenuCtrl_GetKey() == KEY_ESC ) break; // ENDE
itemid = MenuCtrl_GetItemId(); // welcher Menu-Punkt (ID)
if( itemid == MSERVO_SERVO1 ) { Setup_ServoAdjust(0); }
if( itemid == MSERVO_SERVO2 ) { Setup_ServoAdjust(1); }
}
//---------------
// Destroy
//---------------
MenuCtrl_Destroy();
}
//--------------------------------------------------------------
//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, &currentPos);
// 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();
//}
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/servo_setup.h
0,0 → 1,45
#ifndef _TRACKING_H_
#define _TRACKING_H_
#define TRACKING_RSSI 1
#define TRACKING_GPS 2
#define TRACKING_MKCOCKPIT 3
#define TRACKING_NMEA 4
#define DLEFT 0
#define DRIGHT 1
#define AltFaktor 22.5
#define PAN_SERVO_CORRECT 3.125
#define TILT_SERVO_CORRECT 3.125
typedef void (*Displ_Fnct_t)( uint16_t );
//typedef struct {
// int32_t distance;
// int16_t bearing;
//}geo_t;
//typedef struct {
// double Home_Lon; // in degrees
// double Home_Lat; // in degrees
// int32_t Home_Lon7; // in 1E-7 degrees
// int32_t Home_Lat7; // in 1E-7 degrees
// int32_t Home_Alt; // in mm
// // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading
// int16_t direction;
//}__attribute__((packed)) HomePos_t;
#define INTERNAT_STEPS_255 "255 "
#define INTERNAT_STEPS_1023 "1023"
// kapeschi
void Setup_ServoTracking( void );
void Tracking_GPS(void);
void Tracking_NMEA(void);
void Tracking_RSSI(void);
void setNMEAdir(void);
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/tracking.c
0,0 → 1,648
/*
* tracking.c
*
* Created on: 13.02.2012
* Author: cebra
*/
//############################################################################
//# HISTORY tracking.c
//#
//# 27.06.2014 OG
//# - umgestellt auf gps/gps_nmea.c
//# - Codeformattierung
//# - chg: auf #include "../gps/mymath.h" angepasst
//# - del: #include "tools.h"
//#
//# 25.06.2014 OG
//# - chg: PKT_tracking() - auskommentierungen um Zugriffe auf Config.gps_UsedGPSMouse
//# zu unterbinden. Anmerkung dazu siehe dort.
//#
//# 22.06.2014 OG
//# - chg: Tracking_NMEA() - do_tracking() auskommentiert (spaeter fuer
//# Tracking wieder geradeziehen)
//# - Variable 'CheckGPS' hinzugefuegt weil aus anderen Sourcen (setup.c) entfernt
//#
//# 18.06.2014 OG
//# - chg: Code-Formattierung
//#
//# 16.06.2014 OG
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete)
//#
//# 01.06.2014 OG
//# - chg: PKT_trackingMK() - verschiedene Anzeigefunktionen auskommentiert und
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens
//#
//# 30.05.2014 OG
//# - chg: calc_geo()- my_itoa() ersetzt durch writex_gpspos() und lcdx_printf_at_P()
//# weil my_itoa() nicht mehr vorhanden
//#
//# 13.05.2014 OG
//# - chg: PKT_trackingBT() - Variable 'BT_WhasOn' auskommentiert
//# wegen Warning: variable set but not used
//# - chg: PKT_trackingBT() - Variable 'flag' auskommentiert
//# wegen Warning: variable set but not used
//#
//# 28.04.2014 OG
//# - chg: PKT_trackingBT(), PKT_trackingMK() - OSD_Timeout() entfernt (weil es
//# das nicht mehr gibt) und erstmal durch ein PKT_Message_P() ersetzt
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, tracking hat niedrige Prio)
//# - add: include "../pkt/pkt.h"
//#
//# 12.02.2014 OG
//# - del: mk_param_struct entfernt
//#
//# 29.08.2013 Cebra
//# - chg: falsche Parameterübergabe bei getLatitude/getLongitude
//#
//# 25.08.2013 Cebra
//# - add: #ifdef USE_TRACKING . NMEA Routinen werden nur noch für BT_NMEA genutzt
//#
//# 26.06.2013 Cebra
//# - chg: Modulumschaltung für WiFlypatch geändert
//#
//############################################################################
#include "../cpu.h"
#include <string.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <math.h>
#include "../main.h"
//++++++++++++++++++++++++++++++++++
#ifdef USE_TRACKING
//++++++++++++++++++++++++++++++++++
#include "../tracking/tracking.h"
#include "../tracking/ng_servo.h"
#include <avr/pgmspace.h>
#include "../bluetooth/fifo.h"
#include "../bluetooth/bluetooth.h"
#include "../lcd/lcd.h"
#include "../mk-data-structs.h"
#include "../messages.h"
#include "../lcd/lcd.h"
#include "../eeprom/eeprom.h"
#include "../timer/timer.h"
#include "../uart/uart1.h"
#include "../uart/usart.h"
#include "../osd/osd.h"
#include "../gps/mymath.h"
#include "../setup/setup.h"
#include "../pkt/pkt.h"
//#include "../gps/gps_nmea.h"
#include "../avr-nmea-gps-library/nmea.h"
uint8_t CheckGPS = true; // Patch 22.06.2014 OG: hier integriert weil ais anseren Sourcen entfernt (u.a. setup.c)
char data_decode[RXD_BUFFER_SIZE];
#define DLEFT 0
#define DRIGHT 1
#define DEG_TO_RAD 0.0174533 // degrees to radians = PI / 180
#define RAD_TO_DEG 57.2957795 // radians to degrees = 180 / PI
#define AltFaktor 22.5
#define TIMEOUT 200 // 2 sec
NaviData_t *naviData;
HomePos_t MK_pos; // Home position of station
GPS_Pos_t currentPos; // Current position of flying object
uint8_t tracking = TRACKING_MIN;
uint8_t track_hyst = TRACKING_HYSTERESE;
uint8_t track_tx =0;
uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix
geo_t geo;
int16_t anglePan, angleTilt;
//// 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)
//{ double lat1, lon1, lat2, lon2, d1, dlat;
// geo_t geo;
//
// lon1 = MK_pos.Home_Lon;
// lat1 = MK_pos.Home_Lat;
// lon2 = (double)pos->Longitude / 10000000.0;
// lat2 = (double)pos->Latitude / 10000000.0;
//
// // 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 Meter weiter gerechnet
// d1 = 111300 * (double)cos((double)(lat1 + lat2) / 2 * DEG_TO_RAD) * (lon1 - lon2);
// dlat = 111300 * (double)(lat1 - lat2);
// // returns a value in metres http://www.kompf.de/gps/distcalc.html
// geo.bearing = fmod((RAD_TO_DEG * (double)atan2(d1, dlat)) + 180, 360); // +180 besserer Vergleich mit MkCockpit
// if (geo.bearing > 360) geo.bearing -= 360; // bekam schon Werte über 400
// geo.distance = sqrt(d1 * d1 + dlat * dlat);
// return(geo);
//}
// Berechnung von Distanz und Winkel aus GPS-Daten home(MK eingeschaltet)
// zur aktuellen Position(nach Motorstart)
//--------------------------------------------------------------
// bei Gelegenheit den Code auf gps/gps.c/gps_Deviation() aendern
//--------------------------------------------------------------
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 = home->Home_Lon;
lat1 = home->Home_Lat;
lon2 = pos->Longitude;
lat2 = pos->Latitude;
if( !CheckGPS )
{
writex_gpspos( 0, 3, home->Home_Lat , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude)
writex_gpspos( 11, 3, home->Home_Lon , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude)
writex_gpspos( 0, 4, pos->Latitude , MNORMAL, 0,0); // Anzeige: Breitengrad (Latitude)
writex_gpspos( 11, 4, pos->Longitude , MNORMAL, 0,0); // Anzeige: Laengengrad (Longitude)
//lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr
//lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr
//lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr
//lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr
}
// Formel verwendet von http://www.kompf.de/gps/distcalc.html
// 111.3 km = Abstand zweier Breitenkreise und/oder zweier Langenkreise 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);
if( !CheckGPS )
{
lcd_printp_at (0, 5, PSTR("Bear:"), 0);
lcdx_printf_at_P( 5, 5, MNORMAL, 0,0, PSTR("%3d"), geo.bearing );
//lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr
lcd_printp_at (8, 5, PSTR("\x1e"), 0);
lcd_printp_at (9, 5, PSTR("Dist:"), 0);
lcdx_printf_at_P( 15, 5, MNORMAL, 0,0, PSTR("%3d"), geo.distance );
//lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0); // 30.05.2014 OG: my_itoa() gibt es nicht mehr
lcd_printp_at (20, 5, PSTR("m"), 0);
}
return(geo);
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void do_tracking( void )
{
static uint8_t hysteresis = 0;
geo = calc_geo(&MK_pos, &currentPos);
angleTilt = my_atan2((currentPos.Altitude - MK_pos.Home_Alt) / 100, geo.distance);
if (geo.distance < 40 || (geo.distance < 60 && hysteresis))
{ // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschl�gt
geo.bearing = MK_pos.direction;
if (currentPos.Altitude - MK_pos.Home_Alt < 4000) angleTilt = 0; // man fliegt nicht direkt �ber Kopf
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;
}
servoSetAngle(0, anglePan);
servoSetAngle(1, angleTilt);
if (!CheckGPS)
{
lcd_printp_at (0, 6, PSTR("Pan :"), 0);
write_ndigit_number_u (6, 6, anglePan, 3, 1,0);
lcd_printp_at (11, 6, PSTR("Tilt:"), 0);
write_ndigit_number_u (17, 6, angleTilt, 3, 1,0);
}
// write_ndigit_number_u (0, 5, (uint16_t)(currentPos.Altitude/10000000), 2, 0,0);
//// lcd_printp_at (4, 4, PSTR("."), 0);
// write_ndigit_number_u (2, 5, (uint16_t)((currentPos.Altitude/1000) % 10000), 4, 1,0);
// write_ndigit_number_u (6, 5, (uint16_t)((currentPos.Altitude/10) % 100), 2, 1,0);
//
// write_ndigit_number_u (10, 5, (uint16_t)(MK_pos.Home_Alt/10000000), 2, 0,0);
//// lcd_printp_at (4, 4, PSTR("."), 0);
// write_ndigit_number_u (12, 5, (uint16_t)((MK_pos.Home_Alt/1000) % 10000), 4, 1,0);
// write_ndigit_number_u (16, 5, (uint16_t)((MK_pos.Home_Alt/10) % 100), 2, 1,0);
}
//--------------------------------------------------------------
//--------------------------------------------------------------
uint8_t PKT_trackingBT( void ) // Tracking mit NMEA-Daten von BT-Maus
{
//uint8_t BT_WhasOn = 0;
uint8_t BT_status;
//uint8_t flag;
uint8_t tmp_dat;
coldstart =1;
//lcd_printp_at(0,1, PSTR("try NMEA data from:"), 0);
lcd_puts_at (0, 1,Config.gps_UsedDevName, 0);
//set_BTOn();
set_Modul_On(Bluetooth);
//BT_WhasOn = true;
if( Config.BTIsSlave==true )
{
bt_downlink_init();
}
lcd_printp_at (18, 1, PSTR(" ?? "), 0);
BT_status = bt_connect(Config.gps_UsedMac);
if( BT_status==true )
{
lcd_printp_at (18, 1, PSTR(" OK "), 0);
receiveNMEA = true;
}
else
{
lcd_printp_at (17, 1, PSTR("FAIL"), 2);
}
if( receiveNMEA==true )
{
lcd_cls_line( 0, 1, 20);
lcd_printp_at( 0, 2, PSTR(" Latitude Longitude"), 2);
lcd_printp_at( 0, 3, PSTR("H"), 0);
lcd_printp_at( 0, 4, PSTR("M"), 0);
bt_rx_ready = 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);
//OSD_active = true; // benoetigt für Navidata Ausgabe an SV2
//flag = 0;
timer = TIMEOUT;
abo_timer = ABO_TIMEOUT;
do
{
//bt_rx_ready = 0;
// if( !NMEA_receiveBT() )
// break;
if (!NMEA_isdataready())
break;
if( rxd_buffer_locked )
{
timer = TIMEOUT;
Decode64 ();
naviData = (NaviData_t *) pRxData;
currentPos = naviData->CurrentPosition;
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
//-----------------
// MK: Lat / Long
//-----------------
writex_gpspos( 2, 4, currentPos.Latitude , MNORMAL, 0,0 ); // MK: Latitude
writex_gpspos(12, 4, currentPos.Longitude, MNORMAL, 0,0 ); // MK: Longitude
// NMEA_GetNewData(); // neue NMEA GPGGA Daten von der BT GPA-Maus holen
//-----------------
// GPS-Maus: Lat / Long
//-----------------
writex_gpspos( 2, 3, NMEA.Latitude , MNORMAL, 0,0 ); // GPS-Maus: Latitude
writex_gpspos(12, 3, NMEA.Longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude
//do_tracking(); // das geht so noch nicht mit einer BT GPS-Maus! do_tracking(); ueberarbeiten!
if( !CheckGPS )
{
//lcd_printp_at( 0, 2, PSTR("GPS Time: "), 0);
lcd_puts_at( 13, 0, NMEA.Time , 2);
lcd_printp_at( 16, 1, PSTR("Sat:"), 0);
write_ndigit_number_u( 19, 1, NMEA.SatsInUse, 2, 1,0);
lcd_printp_at( 0, 1, PSTR("Fix:"), 0);
write_ndigit_number_u( 4, 1, NMEA.SatFix, 1, 1,0);
lcd_printp_at( 6, 1, PSTR("HDOP:"), 0);
write_ndigit_number_u_10th( 11, 1, NMEA.HDOP, 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;
}
} // end: if (rxd_buffer_locked)
if( !timer )
{
//OSD_Timeout(flag); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet)
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen )
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen
//flag = 0;
error = 1;
}
} while( !get_key_press (1 << KEY_ESC) || !receiveNMEA==true || error ==1);
lcd_cls_line(0,1,21);
lcd_cls_line(0,2,21);
lcd_cls_line(0,3,21);
lcd_cls_line(0,4,21);
lcd_cls_line(0,5,21);
lcd_cls_line(0,6,21);
if( !receiveNMEA )
lcd_printp_at (0, 2, PSTR("lost BT data"), 0);
lcd_printp_at (0, 3, PSTR("GPS trennen"), 0);
} // end: if( receiveNMEA==true )
else
{
lcd_printp_at (0, 4, PSTR("Error at connecting"), 0);
lcd_printp_at (0, 5, PSTR("switch on BT Mouse!!"), 0);
while( !get_key_press (1 << KEY_ENTER) );
}
receiveNMEA = false;
if( !bt_disconnect() )
lcd_printp_at (0, 3, PSTR("Fehler beim Trennen"), 0);
//set_BTOff();
set_Modul_On(USB);
return true;
}
//--------------------------------------------------------------
//--------------------------------------------------------------
uint8_t PKT_trackingMK( void ) // Tracking mit GPS-Daten vom Mikrokopter
{
//uint8_t BT_WhasOn = 0;
//uint8_t BT_status;
uint8_t GPSfix=0;
uint8_t tmp_dat;
uint8_t toggletimer=0;
coldstart = true;
lcd_printp_at (0, 2, PSTR("S Latitude Longitude"), 2);
lcd_cls_line (0,1,20);
//lcd_printp_at (0, 3, PSTR("H"), 0);
//lcd_printp_at (0, 4, PSTR("M"), 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);
timer = TIMEOUT;
abo_timer = ABO_TIMEOUT;
error = 0;
do
{
if( rxd_buffer_locked )
{
timer = TIMEOUT;
Decode64 ();
naviData = (NaviData_t *) pRxData;
//OSD_Screen_Element (18, 1, OSD_SATS_IN_USE,1);
//if (GPSfix == true) OSD_Screen_Element (0, 1, OSD_STATUS_FLAGS,1);
//---
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!)
//---
//OSD_Element_SatsInUse( 18, 1, 1);
//---
// 01.06.2014 OG: DEPRICATED (alte Funktionen in osd/osdold_screens.c - nicht mehr verwenden!)
//---
//if (GPSfix == true) OSD_Element_StatusFlags( 0, 1);
if (!(naviData->NCFlags & NC_FLAG_GPS_OK))
{
toggletimer++;
if (toggletimer == 50) toggletimer = 0;
if (toggletimer == 25) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 2);
if (toggletimer == 1) lcd_printp_at(0,1, PSTR("Whait for GPS Fix "), 0);
rxd_buffer_locked = false;
GPSfix = false;
}
else GPSfix = true;
if( GPSfix )
{
if( coldstart )
{
// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen
if (naviData->FCStatusFlags & FC_FLAG_MOTOR_START)
{
MK_pos.Home_Lon = naviData->HomePosition.Longitude;
MK_pos.Home_Lat = naviData->HomePosition.Latitude;
MK_pos.Home_Alt = naviData->HomePosition.Altitude;
MK_pos.direction = naviData->CompassHeading;
coldstart = false;
rxd_buffer_locked = false;
lcd_printp_at(0,1, PSTR(" "), 0);
}
else
{
lcd_printp_at(0,1, PSTR("GPS ok, start ok "), 0);
rxd_buffer_locked = false;
}
} // end: if( coldstart )
else
{
//run
currentPos = naviData->CurrentPosition;
currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
do_tracking();
//lcd_puts_at (13, 0, NMEAtime, 2);
//lcd_printp_at (16, 1, PSTR("Sat:"), 0);
//write_ndigit_number_u (19, 1, NMEAsatsInUse, 2, 1,0);
//lcd_printp_at (0, 1, PSTR("Fix:"), 0);
//write_ndigit_number_u (4, 1, posfix, 1, 1,0);
//lcd_printp_at (6, 1, PSTR("HDOP:"), 0);
//write_ndigit_number_u_10th (11, 1, HDOP, 3, 0,0);
rxd_buffer_locked = FALSE;
} // run
}
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;
}
} // end: if( rxd_buffer_locked )
if( !timer )
{
//OSD_Timeout(1); // <- 28.04.2014 OG: gibt es nicht mehr - ersetzt durch PKT_Message_P() (ungetestet)
//void PKT_Message_P( const char *text, uint8_t error, uint16_t timeout, uint8_t beep, uint8_t clearscreen )
PKT_Message_P( strGet(OSD_ERROR), true, 200, true, true ); // max. 2 Sekunden anzeigen
error = 1;
}
} while( (!get_key_press (1 << KEY_ENTER)) && (error ==0) );
lcd_cls_line(0,1,21);
lcd_cls_line(0,2,21);
lcd_cls_line(0,3,21);
lcd_cls_line(0,4,21);
lcd_cls_line(0,5,21);
lcd_cls_line(0,6,21);
if (error ==1)
{
lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0);
_delay_ms(2000);
}
return true;
}
//--------------------------------------------------------------
//--------------------------------------------------------------
void PKT_tracking(void)
{
clear_key_all();
lcd_cls ();
// 25.06.2014 OG: auskommentiert weil erstmal kein Config.gps_UsedGPSMouse mehr unterstuetzt
// wird. Falls tracking.c mal richtig implementiert wird sollte man sich auf eine BT-GPS-Maus
// konzentrieren bis Tracking einwandfrei laeuft - wenn dann noch immer Bedarf nach MK-GPS ist
// kann man das ja wieder einbauen
//
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) lcd_printp_at(0,0, PSTR("Tracking Bluetooth "), 2);
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) lcd_printp_at(0,0, PSTR(" Tracking Mikrokopter"), 2);
//if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT();
//if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK();
lcd_printp_at( 0,0, PSTR("Tracking Bluetooth "), MINVERS);
lcd_printp_at(12, 7, strGet(ENDE), MNORMAL); // Keyline
PKT_trackingBT();
clear_key_all();
}
//++++++++++++++++++++++++++++++++++
#endif // #ifdef USE_TRACKING
//++++++++++++++++++++++++++++++++++
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/tracking.h
0,0 → 1,77
/*
* tracking.h
*
* Created on: 13.02.2012
* Author: cebra
*/
//############################################################################
//# HISTORY tracking.h
//#
//# 27.06.2014 OG
//# - Entkernt: verschiedene Variablen und Funktionen geloescht
//#
//# 16.06.2014 OG
//# - add: NMEA_GPGGA_counter (zaehlt empfangene GPGGA-Pakete)
//# - add: Source-Historie
//############################################################################
#ifndef TRACKING_H_
#define TRACKING_H_
#define REPEAT 1
#define REPEAT_MIN 1
#define REPEAT_MAX 100
#define PAUSE 10
#define PAUSE_MIN 4 // mindestens 400ms, da mechanischer Servo-Lauf zur Position berücksichtigt werden muss
#define PAUSE_MAX 20 // Pause pro Links-, Mittel- und Rechtsposition 10*100ms
#define PAUSE_STEP 0
#define PAUSE_STEP_MIN 0 // Pause bei jeden Servoschritt in ms
#define PAUSE_STEP_MAX 200
// Antennen-Nachführung
#define TRACKING_MIN 0 // aus, TRACKING_RSSI, TRACKING_GPS, TRACKING_MKCOCKPIT, TRACKING_NMEA
#define TRACKING_MAX 4
// Antennen-Nachführung per RSSI
#define TRACKING_HYSTERESE 40 // Hysterese bevor Tracking bei Richtungswechsel anspricht
#define TRACKING_HYST_MIN 0
#define TRACKING_HYST_MAX 100
#define FC_FLAG_MOTOR_RUN 0x01
#define FC_FLAG_FLY 0x02
#define FC_FLAG_CALIBRATE 0x04
#define FC_FLAG_MOTOR_START 0x08
typedef struct {
int32_t distance;
int16_t bearing;
} geo_t;
typedef struct {
int32_t Home_Lon; // in 1E-7 degrees
int32_t Home_Lat; // in 1E-7 degrees
int32_t Home_Alt; // in mm
int16_t direction; // ermittelte Konstante aus Mittelposition Antenne geo.bearing - navi_data.CompassHeading
} __attribute__((packed)) HomePos_t;
extern uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix
#ifdef USE_TRACKING
void PKT_tracking(void);
#endif // #ifdef USE_TRACKING
#endif // TRACKING_H_
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/PKT/GPL_PKT_V3_85f/tracking/.
Property changes:
Added: svn:ignore
+_doc