• 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

  → / @ 1914

  → / @ 1915

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 1914 → Rev 1915

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/ng_servo.c
0,0 → 1,223
/********************************************************************/
/* */
/* 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 */
/* */
/********************************************************************/
#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"
// 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;
}
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/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/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/servo.c
0,0 → 1,153
/*****************************************************************************
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* Copyright (C) 2011 Harald Bongartz *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
#include "../cpu.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include "../lcd/lcd.h"
#include "../timer/timer.h"
#include "../tracking/servo.h"
#include "../messages.h"
#define SERVO_CORRECT 3.125
//--------------------------------------------------------------
//
void servo_test (void)
{
#ifdef HWVERSION3_9
uint8_t chg = 0;
uint8_t Pos = 150; // 1,5mS
OCR1A = 150 * SERVO_CORRECT; // Servomitte
lcd_cls ();
lcd_printp (PSTR(" Servo Tester "), 2);
lcd_printp_at (7, 5, PSTR("%"), 0);
lcd_printp_at (16, 5, PSTR("mS"), 0);
// lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0);
lcd_puts_at(0, 7, strGet(KEYLINE3), 0);
lcd_printp_at (18, 7, PSTR("\x19O\x18"), 0);
lcd_rect(3, 23, 120, 8, 1); // +-150% Rahmen
lcd_line(23,23,23,31,1); // -100%
lcd_line(43,23,43,31,1); // -50%
lcd_frect(61, 23, 3, 8, 1); // 0%
lcd_line(83,23,83,31,1); // +50%
lcd_line(103,23,103,31,1); // +100%
write_ndigit_number_u (4, 5, 0, 3, 0,0); // Pulse width in %
write_ndigit_number_u_100th(12, 5, 150, 3, 0); // Pulse width in ms
do
{
if ((get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (Pos < 225))
{
if (Pos < 150)
lcd_frect ((63 - ((150 - Pos) * 0.8)), 24, 1, 6, 0);
Pos++;
if (Pos == 75 || Pos == 100 || Pos == 125 || Pos == 150 || Pos == 175 || Pos == 200 || Pos == 225)
{
set_beep ( 200, 0x0080, BeepNormal);
}
if (Pos >= 225)
Pos = 225;
chg++;
}
else if ((get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (Pos > 75))
{
if (Pos > 150)
lcd_frect ((((Pos - 150) * 0.8) + 63), 24, ((Pos - 150) * 0.8), 6, 0);
Pos--;
if (Pos == 75 || Pos == 100 || Pos == 125 || Pos == 150 || Pos == 175 || Pos == 200 || Pos == 225)
{
set_beep ( 200, 0x0080, BeepNormal);
}
if (Pos <= 75)
Pos = 75;
chg++;
}
else if (get_key_press (1 << KEY_ENTER))
{
lcd_frect (4, 24, 118, 6, 0); // Balken löschen
lcd_frect(61, 23, 3, 8, 1); // 0%
Pos = 150;
set_beep ( 200, 0x0080, BeepNormal);
chg++;
}
if (chg)
{
chg = 0;
if (Pos >= 150)
{
lcd_frect (63, 24, ((Pos - 150) * 0.8), 6, 1);
write_ndigit_number_u (4, 5, ((Pos - 150) * 2), 3, 0,0); // Pulse width in %
lcd_frect(62, 23, 2, 8, 1); // 0%
}
else
{
lcd_frect (63 - ((150 - Pos) * 0.8), 24, ((150 - Pos) * 0.8), 6, 1);
write_ndigit_number_u (4, 5, ((150 - Pos) * 2), 3, 0,0); // Pulse width in %
lcd_frect(61, 23, 2, 8, 1); // 0%
}
write_ndigit_number_u_100th(12, 5, Pos, 3, 0); // Pulse width in ms
lcd_line(3, 23,3, 31,1); // -150%
lcd_line(23, 23,23, 31,1); // -100%
lcd_line(43, 23,43, 31,1); // -50%
lcd_line(83, 23,83, 31,1); // +50%
lcd_line(103,23,103,31,1); // +100%
lcd_line(123,23,123,31,1); // +150%
OCR1A = Pos * SERVO_CORRECT; // Servostellung
}
}
while (!get_key_press (1 << KEY_ESC));
get_key_press(KEY_ALL);
#endif
}
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/servo.h
0,0 → 1,40
/*****************************************************************************
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* Copyright (C) 2011 Harald Bongartz *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
#ifndef _SERVO_H
#define _SERVO_H
void servo_test (void);
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/servo_setup.c
0,0 → 1,1780
/****************************************************************/
/* */
/* 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, &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();
//}
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/servo_setup.h
0,0 → 1,46
#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 servo_menu(void);
void servotest_cont_menu(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/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/tools.c
0,0 → 1,180
/****************************************************************/
/* */
/* 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 <stdlib.h>
#include <util/delay.h>
#include <avr/pgmspace.h>
//#include "config.h"
//#include "dogm.h"
#include "tools.h"
//#include "messages.h"
#define MAX_POWER 10
#define getPower(x) (int32_t)pgm_read_dword(&powers[x])
const int32_t PROGMEM powers[MAX_POWER] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};
/* Funktion zur Umwandlung einer vorzeichenbehafteten Integer
32-Bit "Festkomma-Zahl"(gedachtes Komma in Integer) in einen String
vereinfacht Variablenübergabe funktion change_value(uint16_t x),
kein printf, double oder float
siehe http://www.mikrocontroller.net/articles/Festkommaarithmetik
len: max 13, Gesamtlänge des Resultats inklusive Vorzeichen und Komma, Rest wird mit ' ' aufgefüllt
fixedPoint: Position des Kommas im Integer-Wert. Bei Wert in mm und Anzeige in m ist das z.B. 3
afterPoint: Ziffern nach dem Komma = wieviele der fixedPoint Ziffern angezeigt werden sollen
Ist nicht genug Platz für die Zahl vorhanden werden nur '*' Zeichen ausgegeben!
makefile derzeit somit auch ohne! Minimalistic printf version*/
char *my_itoa(int32_t value, uint8_t len, uint8_t fixedPoint, uint8_t afterPoint)
{ int8_t i;
int8_t digits, digitsNeeded;
uint8_t neg = 0;
static char str[13];
// Terminate string
str[len] = '\0';
// Reduce precision of value if we're not supposed to show all of the mantissa
if (fixedPoint > afterPoint) {
value /= getPower(fixedPoint - afterPoint);
fixedPoint = afterPoint;
}
// Handle negative values
if (value < 0) {
value = -value;
neg = 1;
}
// Check how many digits we've got in total and if it fits in our space
for (digits = 1; digits < MAX_POWER && value >= getPower(digits); digits++);
if (neg) digits++; // We also need space for the sign
if (fixedPoint) digits++; // Plus space for decimal point
digitsNeeded = digits - len;
if (digitsNeeded > 0) {
// Not enough space, do something
if (digitsNeeded == fixedPoint || digitsNeeded == fixedPoint + 1) { // +1 = space for decimal point that we can get rid of
// If space is just big enough for integer part then simply don't show mantissa BUT ROUND CORRECTLY
value = (value + 5 * getPower(fixedPoint - 1)) / getPower(fixedPoint);
fixedPoint = 0;
} else if (digitsNeeded < fixedPoint) {
// We can reduce precision to make it fit (round correctly)
value = (value + 5 * getPower(digitsNeeded - 1)) / getPower(digitsNeeded);
fixedPoint -= digitsNeeded;
} else {
// Error, cannot display value! Let's show stars
for (i = len - 1; i >= 0; --i) str[i] = '*';
return str;
}
}
for (i = len - 1; i >= neg; --i) {
if (fixedPoint && i == len - fixedPoint - 1) {
// Insert decimal point at the right location
// str[i] = Msg(MSG_KOMMA)[0];
str[i] = ',';
fixedPoint = 0; // Now in integer part
} else {
str[i] = (value % 10) + '0';
value /= 10;
// Break if we're in integer part and there are only zeros from this point on
if (value == 0 && fixedPoint == 0) {
--i;
break;
}
}
}
// Insert sign
if (neg) str[i--] = '-';
// Rest is blank
for (; i >= 0; --i)
str[i] = ' ';
return str;
}
// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit
// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450
int32_t floatStrToInt(const char *s, int32_t power1)
{ char *endPtr;
int32_t v = strtol(s, &endPtr, 10);
if (*endPtr == '.') {
for (s = endPtr + 1; *s && power1; s++) {
v = v * 10 + (*s - '0');
--power1;
}
}
if (power1) {
// Table to avoid multiple multiplications
v = v * getPower(power1);
}
return v;
}
// Delay helper
void delay_ms100x(uint8_t delay)
{
for ( uint8_t i=0; i<delay; i++)
_delay_ms(100);
}
//
///************************************************************************************/
///* */
///* Zeitanzeige */
///* */
///************************************************************************************/
//
//uint32_t TimeBase60(char *str, uint32_t time, uint8_t idx)
//{ uint32_t tmp = time % 60;
//
// str[idx] = (tmp / 10) + '0';
// str[idx + 1] = (tmp % 10) + '0';
// return time / 60;
//}
//
//void Displ_TimeMS(int32_t time)
//{ char str[7];
//
// str[6] = '\0';
// if (time < 0) {
// time = abs(time);
// str[0] = '-';
// }
// else
// str[0] = ' ';
// time = TimeBase60(str, time, 4);
// str[3] = ':';
// TimeBase60(str, time, 1);
// lcdPuts(str);
//}
//
//void Displ_TimeHMS(uint32_t time)
//{ char str[9];
//
// time /= T2SECDIV; // Zähler aller 500µs
// str[8] = '\0';
// time = TimeBase60(str, time, 6);
// str[5] = ':';
// time = TimeBase60(str, time, 3);
// str[2] = ':';
// TimeBase60(str, time, 0);
// lcdPuts(str);
//}
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/tools.h
0,0 → 1,15
#ifndef _TOOLS_H_
#define _TOOLS_H_
#include <avr/io.h>
//char *my_itoa(int32_t value, uint8_t sign, uint8_t len, uint8_t fixedPoint, uint8_t afterPoint);
char *my_itoa(int32_t value, uint8_t len, uint8_t fixedPoint, uint8_t afterPoint);
int32_t floatStrToInt(const char *s, int32_t power1);
void delay_ms100x(uint8_t delay);
void Displ_TimeMS(int32_t time);
void Displ_TimeHMS(uint32_t time);
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/tracking.c
0,0 → 1,694
/*
* tracking.c
*
* Created on: 13.02.2012
* Author: cebra
*/
#include "../cpu.h"
#include <string.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <math.h>
#include "../main.h"
#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 "tools.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 "../tracking/mymath.h"
#include "../setup/setup.h"
#ifdef HWVERSION3_9
//#define MAX_POWER 10
//#define getPower(x) (int32_t)pgm_read_dword(&powers[x])
//const int32_t PROGMEM powers[MAX_POWER] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};
#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;
mk_param_struct_t *mk_param_struct;
HomePos_t MK_pos; // Home position of station
GPS_Pos_t currentPos; // Current position of flying object
uint8_t NMEAsatsInUse; // Number of satelites currently in use BT-Mouse
//uint8_t MKsatsInUse; // Number of satelites currently in use Mikrokopter
int32_t NMEAlatitude, NMEAlongitude; // Longitude, Latitude
int16_t NMEAaltitude; // Höhe in Meter
uint8_t posfix; // GPS Fix, 0 = Fix not available or invalid,1 = GPS SPS Mode, fix valid,
// 2 = Differential GPS, SPS Mode, fix valid, 6 = Dead Reckoning Mode, fix valid
int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser
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;
char NMEAtime[9] = "GP:Ti:me";
//char NMEADate [6];
//// 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)
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)
{
lcd_puts_at (0, 3, my_itoa(home->Home_Lat, 10, 7, 7), 0);
lcd_puts_at (11, 3, my_itoa(home->Home_Lon, 10, 7, 7), 0);
lcd_puts_at (0, 4, my_itoa(pos->Latitude, 10, 7, 7), 0);
lcd_puts_at (11, 4, my_itoa(pos->Longitude, 10, 7, 7), 0);
}
// lcd_printp_at (0, 3, PSTR("H"), 0);
// lcd_printp_at (0, 4, PSTR("M"), 0);
// 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);
lcd_puts_at (5, 5, my_itoa((uint32_t)geo.bearing, 3, 0, 0), 0);
lcd_printp_at (8, 5, PSTR("\x1e"), 0);
lcd_printp_at (9, 5, PSTR("Dist:"), 0);
lcd_puts_at (15, 5, my_itoa((uint32_t)geo.distance, 3, 1, 1), 0);
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();
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_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);
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;
// OSD_active = true; // benötigt für Navidata Ausgabe an SV2
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
flag = 0;
timer = TIMEOUT;
abo_timer = ABO_TIMEOUT;
do
{
// bt_rx_ready = 0;
if (!bt_receiveNMEA())
break;
if (rxd_buffer_locked)
{
timer = TIMEOUT;
Decode64 ();
naviData = (NaviData_t *) pRxData;
//#ifdef DEBUG
// debug_pgm(PSTR("setup Tracking_NMEA"));
//#endif
currentPos = naviData->CurrentPosition;
// currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)(naviData->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5;
// uint32_t lat = currentPos.Latitude;
// uint32_t lon = currentPos.Longitude;
// write_ndigit_number_u (2, 4, (uint16_t)(lat/10000000), 2, 0,0);
// lcd_printp_at (4, 4, PSTR("."), 0);
// write_ndigit_number_u (5, 4, (uint16_t)((lat/1000) % 10000), 4, 1,0);
// write_ndigit_number_u (9, 4, (uint16_t)((lat/10) % 100), 2, 1,0);
//
// write_ndigit_number_u (12, 4, (uint16_t)(lon/10000000), 2, 0,0);
// lcd_printp_at (14, 4, PSTR("."), 0);
// write_ndigit_number_u (15, 4, (uint16_t)((lon/1000) % 10000), 4, 1,0);
// write_ndigit_number_u (19, 4, (uint16_t)((lon/10) % 100),2, 1,0);
Tracking_NMEA();
// write_ndigit_number_u (2, 3, (uint16_t)(NMEAlatitude/10000000), 2, 0,0);
// lcd_printp_at (4, 3, PSTR("."), 0);
// write_ndigit_number_u (5, 3, (uint16_t)((NMEAlatitude/1000) % 10000), 4, 1,0);
// write_ndigit_number_u (9, 3, (uint16_t)((NMEAlatitude/10) % 100), 2, 1,0);
//
//
// write_ndigit_number_u (12, 3, (uint16_t)(NMEAlongitude/10000000), 2, 0,0);
// lcd_printp_at (14, 3, PSTR("."), 0);
// write_ndigit_number_u (15, 3, (uint16_t)((NMEAlongitude/1000) % 10000), 4, 1,0);
// write_ndigit_number_u (19, 3, (uint16_t)((NMEAlongitude/10) % 100), 2, 1,0);
// lcd_printp_at (0, 2, PSTR("GPS Time: "), 0);
if (!CheckGPS)
{
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;
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 (rxd_buffer_locked)
if (!timer)
{
OSD_Timeout(flag);
flag = 0;
error = 1;
}
} //end do
while (!get_key_press (1 << KEY_ENTER) || !receiveNMEA==true || error ==1);
// while (!get_key_press (1 << KEY_ENTER));
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);
// if (error ==1) lcd_printp_at (0, 2, PSTR("lost Wi.232 data"), 0);
lcd_printp_at (0, 3, PSTR("GPS trennen"), 0);
}
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();
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);
OSD_Element_SatsInUse( 18, 1, 1);
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;
}
}
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;
}
} //rx_buffer_locked
if (!timer)
{
OSD_Timeout(1);
error = 1;
}
} //end do
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)
{
get_key_press(KEY_ALL);
lcd_cls ();
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);
lcd_printp_at (16, 7, PSTR("Ende"), 0);
if (Config.gps_UsedGPSMouse==GPS_Bluetoothmouse1) PKT_trackingBT();
if (Config.gps_UsedGPSMouse==GPS_Mikrokopter) PKT_trackingMK();
get_key_press(KEY_ALL);
}
//// Trying to avoid floating point maths here. Converts a floating point string to an integer with a smaller unit
//// i.e. floatStrToInt("4.5", 2) = 4.5 * 1E2 = 450
//int32_t floatStrToInt(const char *s, int32_t power1)
//{ char *endPtr;
// int32_t v = strtol(s, &endPtr, 10);
//
// if (*endPtr == '.') {
// for (s = endPtr + 1; *s && power1; s++) {
// v = v * 10 + (*s - '0');
// --power1;
// }
// }
// if (power1) {
// // Table to avoid multiple multiplications
// v = v * getPower(power1);
// }
// return v;
//}
// NMEA latitudes are in the form ddmm.mmmmm, we want an integer in 1E-7 degree steps
int32_t getLatitude(const char *s, const char *NS)
{ int32_t deg = (s[0] - '0') * 10 + s[1] - '0'; // First 2 chars are full degrees
int32_t min = floatStrToInt(&s[2], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps
deg = deg * 10000000 + min;
if (*NS == 'S') deg = -deg;
return deg;
}
// NMEA longitudes are in the form dddmm.mmmmm, we want an integer in 1E-7 degree steps
int32_t getLongitude(const char *s, const char *WE)
{ int32_t deg = ((s[0] - '0') * 10 + s[1] - '0') * 10 + s[2] - '0'; // First 3 chars are full degrees
int32_t min = floatStrToInt(&s[3], 6) / 6; // Minutes * 1E5 * 100 / 60 = Minutes * 1E6 / 6 = 1E-7 degree steps
deg = deg * 10000000 + min;
if (*WE == 'W') deg = -deg;
return deg;
}
void getNMEATime( const char *s)
{
uint8_t sem = 0;
uint8_t i;
for ( i=0;i < 6; i++ )
{
NMEAtime[sem++] = s[i];
if (i==1 || i==3) NMEAtime[sem++] = ':';
}
NMEAtime[sem] = '\0';
}
//$GPGGA,191410.000,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45
// ^ ^ ^ ^ ^ ^ ^ ^
// | | | | | | | |
// | | | | | | | Höhe Geoid minus
// | | | | | | | Höhe Ellipsoid (WGS84)
// | | | | | | | in Metern (48.0,M)
// | | | | | | |
// | | | | | | Höhe über Meer (über Geoid)in Metern (351.5,M)
// | | | | | |
// | | | | | HDOP (horizontal dilution
// | | | | | of precision) Genauigkeit
// | | | | |
// | | | | Anzahl der erfassten Satelliten
// | | | |
// | | | Qualität der Messung
// | | | (0 = ungültig)
// | | | (1 = GPS)
// | | | (2 = DGPS)
// | | | (6 = geschätzt nur NMEA-0183 2.3)
// | | |
// | | Längengrad
// | |
// | Breitengrad
// |
// Uhrzeit
void Tracking_NMEA(void)
{
char *token;
if (decodeNMEA()) {
token = strtok((char*)data_decode, ",");
if (!strcmp(token, "GPGGA"))
{
// $GPGGA,220613.400,4843.5080,N,00922.9583,E,1,7,2.23,287.1,M,48.0,M,,
// Time
getNMEATime(strtok(0, ".")); //Zeit
strtok(0, ","); // Skip Rest von der Zeit
// Latitude
NMEAlatitude = getLatitude(strtok(0, ","), strtok(0, ",")); //N
// Longitude
NMEAlongitude = getLongitude(strtok(0, ","), strtok(0, ","));//E
// Signal valid? (Position Fix Indicator)
posfix = atoi(strtok(0, ",")); // Qualität
// Satellites in use
NMEAsatsInUse = atoi(strtok(0, ",")); //Anzahl Sats
// Dilition, best = 0.0
HDOP = floatStrToInt(strtok(0, ","),1); //Dilution
// // Altitude
NMEAaltitude = floatStrToInt(strtok(0, ","), 1);
// currentPos.Altitude = altitude;
// currentPos.Latitude = latitude;
// currentPos.Longitude = longitude;
//TODO: erstmal test if ((coldstart) && (satsInUse > 5)) {
// First position after reboot (or change of mode) will be the home position (facing north)
MK_pos.Home_Lon = NMEAlongitude;
MK_pos.Home_Lat = NMEAlatitude;
MK_pos.Home_Alt = NMEAaltitude;
MK_pos.direction = 0;
// coldstart = 0;
// Double_Beep(DBEEPNMEAFIX, DBEEPMEAFIXP);
do_tracking();
// }
}
}
// Displ_GPS(); // letzte empfangene Daten auch bei ausgeschalteter NMEA sichtbar
}
uint8_t hexDigitToInt(uint8_t digit)
{
if (digit >= '0' && digit <= '9') return digit - '0';
if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10;
if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10;
return 0;
}
uint8_t decodeNMEA(void)
{
uint8_t ret = 0;
uint8_t crc;
uint8_t tmpCRC = 0;
uint8_t i;
if (bt_rx_ready == 1 && bt_rx_len > 0) {
// Calculate checksum
for (i = 1; i < bt_rx_len && bt_rx_buffer[i] != '*'; i++) {
tmpCRC ^= bt_rx_buffer[i];
}
if (bt_rx_len >= i + 3) {
crc = hexDigitToInt(bt_rx_buffer[i + 1]) << 4 | hexDigitToInt(bt_rx_buffer[i + 2]);
if (crc == tmpCRC) {
bt_rx_buffer[i] = 0;
strcpy(data_decode, &bt_rx_buffer[1]); // Data without $, crc
ret = 1;
// wi232RX = 1; // So antenna-symbol will blink
// cli();
// rx_timeout = 0; // Got valid data, reset counter
// sei();
}
}
}
// if (rx_timeout < RX_TIME_OLD) wi232RX = 1;
bt_rx_ready = 0; // Unlock buffer, next NMEA string can be received
return ret;
}
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/tracking/tracking.h
0,0 → 1,67
/*
* tracking.h
*
* Created on: 13.02.2012
* Author: cebra
*/
#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
//#define NC_FLAG_GPS_OK 0
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 NMEAsatsInUse; // Number of satelites currently in use
extern int32_t NMEAlatitude, NMEAlongitude;
extern uint8_t posfix; // GPS Fix, 0 = Fix not available or invalid,1 = GPS SPS Mode, fix valid,
// 2 = Differential GPS, SPS Mode, fix valid, 6 = Dead Reckoning Mode, fix valid
extern int16_t HDOP; // Horizontal Dilution of Precision, 1.1 -xx.x, niederiger = besser
extern int16_t NMEAaltitude; // Höhe in Meter
extern char NMEAtime[9];
extern uint8_t coldstart; // Flag erstmaliger MK-Start(Motore) nur nach GPS-Fix
void Tracking_NMEA(void);
uint8_t decodeNMEA(void);
void PKT_tracking(void);
#endif /* TRACKING_H_ */
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/uart/uart1.c
0,0 → 1,371
/*************************************************************************
Title: Interrupt UART library with receive/transmit circular buffers
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury
File: $Id: uart.c,v 1.6.2.2 2009/11/29 08:56:12 Peter Exp $
Software: AVR-GCC 4.1, AVR Libc 1.4.6 or higher
Hardware: any AVR with built-in UART,
License: GNU General Public License
DESCRIPTION:
An interrupt is generated when the UART has finished transmitting or
receiving a byte. The interrupt handling routines use circular buffers
for buffering received and transmitted data.
The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define
the buffer size in bytes. Note that these variables must be a
power of 2.
USAGE:
Refere to the header file uart.h for a description of the routines.
See also example test_uart.c.
NOTES:
Based on Atmel Application Note AVR306
LICENSE:
Copyright (C) 2006 Peter Fleury
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
*************************************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdbool.h>
#include "uart1.h"
#include "../main.h"
#include "../bluetooth/bluetooth.h"
#include "../tracking/tracking.h"
//
// constants and macros
//
#if defined HWVERSION1_3W || defined HWVERSION3_9
// size of RX/TX buffers
#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1)
#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1)
#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK )
#error RX buffer size is not a power of 2
#endif
#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK )
#error TX buffer size is not a power of 2
#endif
// ATmega with two USART
#define ATMEGA_USART1
#define UART1_STATUS UCSR1A
#define UART1_CONTROL UCSR1B
#define UART1_DATA UDR1
#define UART1_UDRIE UDRIE1
//
// module global variables
//
uint8_t receiveNMEA = false;
#if defined( ATMEGA_USART1 )
static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE];
static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE];
static volatile unsigned char UART1_TxHead;
static volatile unsigned char UART1_TxTail;
static volatile unsigned char UART1_RxHead;
static volatile unsigned char UART1_RxTail;
static volatile unsigned char UART1_LastRxError;
volatile uint16_t UART1_RxError;
#endif
void SetBaudUart1(uint8_t Baudrate)
{
switch (Baudrate)
{
case Baud_2400: uart1_init( UART_BAUD_SELECT(2400,F_CPU) ); break;
case Baud_4800: uart1_init( UART_BAUD_SELECT(4800,F_CPU) ); break;
// case Baud_9600: uart1_init( UART_BAUD_SELECT(9600,F_CPU) ); break;
case Baud_9600: uart1_init( 129); break;
case Baud_19200: uart1_init( UART_BAUD_SELECT(19200,F_CPU) ); break;
case Baud_38400: uart1_init( UART_BAUD_SELECT(38400,F_CPU) ); break;
case Baud_57600: uart1_init( UART_BAUD_SELECT(57600,F_CPU) ); break;
// case Baud_57600: uart1_init( 21); break;
// case Baud_115200: uart1_init( UART_BAUD_SELECT(115200,F_CPU) ); break;
case Baud_115200: uart1_init( 10 ); break;
}
}
//
// these functions are only for ATmegas with two USART
//
#if defined( ATMEGA_USART1 )
//--------------------------------------------------------------
// Function: UART1 Receive Complete interrupt
// Purpose: called when the UART1 has received a character
//--------------------------------------------------------------
ISR(USART1_RX_vect)
{
unsigned char tmphead;
unsigned char data;
unsigned char usr;
unsigned char lastRxError;
// read UART status register and UART data register
usr = UART1_STATUS;
data = UART1_DATA;
lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) );
// calculate buffer index
tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK;
if ( tmphead == UART1_RxTail )
{
// error: receive buffer overflow
lastRxError = UART_BUFFER_OVERFLOW >> 8;
UART1_RxError++;
}
else
{
// store new index
UART1_RxHead = tmphead;
// store received data in buffer
UART1_RxBuf[tmphead] = data;
}
UART1_LastRxError = lastRxError;
// if (receiveNMEA==true)
// {
// if (bt_receiveNMEA()) Tracking_NMEA();
// else receiveNMEA = false;
//
// }
}
//--------------------------------------------------------------
// Function: UART1 Data Register Empty interrupt
// Purpose: called when the UART1 is ready to transmit the next byte
//--------------------------------------------------------------
ISR(USART1_UDRE_vect)
{
unsigned char tmptail;
if ( UART1_TxHead != UART1_TxTail)
{
// calculate and store new buffer index
tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK;
UART1_TxTail = tmptail;
// get one byte from buffer and write it to UART
UART1_DATA = UART1_TxBuf[tmptail]; // start transmission
}
else
{
// tx buffer empty, disable UDRE interrupt
UART1_CONTROL &= ~_BV(UART1_UDRIE);
}
}
//--------------------------------------------------------------
// Function: uart1_init()
// Purpose: initialize UART1 and set baudrate
// Input: baudrate using macro UART_BAUD_SELECT()
// Returns: none
//--------------------------------------------------------------
void uart1_init(unsigned int baudrate)
{
UART1_TxHead = 0;
UART1_TxTail = 0;
UART1_RxHead = 0;
UART1_RxTail = 0;
UART1_RxError = 0;
// Set baud rate
if ( baudrate & 0x8000 )
{
UART1_STATUS = (1<<U2X1); //Enable 2x speed
baudrate &= ~0x8000;
}
UBRR1H = (unsigned char)(baudrate>>8);
UBRR1L = (unsigned char) baudrate;
// Enable USART receiver and transmitter and receive complete interrupt
UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1);
// Set frame format: asynchronous, 8data, no parity, 1stop bit
#ifdef URSEL1
UCSR1C = (1<<URSEL1)|(3<<UCSZ10);
#else
UCSR1C = (3<<UCSZ10);
// UCSR1C = (1<<UCSZ11) | (1<<UCSZ10); // 8data Bit
#endif
}
//--------------------------------------------------------------
// Function: uart1_getc()
// Purpose: return byte from ringbuffer
// Returns: lower byte: received byte from ringbuffer
// higher byte: last receive error
//--------------------------------------------------------------
unsigned int uart1_getc(void)
{
unsigned char tmptail;
unsigned char data;
if ( UART1_RxHead == UART1_RxTail )
{
return UART_NO_DATA; // no data available
}
// calculate /store buffer index
tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK;
UART1_RxTail = tmptail;
// get data from receive buffer
data = UART1_RxBuf[tmptail];
return (UART1_LastRxError << 8) + data;
}/* uart1_getc */
//--------------------------------------------------------------
// Function: uart1_putc()
// Purpose: write byte to ringbuffer for transmitting via UART
// Input: byte to be transmitted
// Returns: 1 on succes, 0 if remote not ready
//--------------------------------------------------------------
int uart1_putc(unsigned char data)
{
unsigned char tmphead;
tmphead = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK;
while ( tmphead == UART1_TxTail )
{;} // wait for free space in buffer
UART1_TxBuf[tmphead] = data;
UART1_TxHead = tmphead;
// enable UDRE interrupt
UART1_CONTROL |= _BV(UART1_UDRIE);
return (UART1_LastRxError << 8) + data;
}
//--------------------------------------------------------------
// Function: uart1_puts()
// Purpose: transmit string to UART1
// Input: string to be transmitted
// Returns: none
//--------------------------------------------------------------
void uart1_puts(const char *s )
{
while (*s)
uart1_putc(*s++);
}
//--------------------------------------------------------------
// Function: uart1_puts_p()
// Purpose: transmit string from program memory to UART1
// Input: program memory string to be transmitted
// Returns: none
//--------------------------------------------------------------
void uart1_puts_p(const char *progmem_s )
{
register char c;
while ( (c = pgm_read_byte(progmem_s++)) )
uart1_putc(c);
}
//--------------------------------------------------------------
// Function: uart1_available()
// Purpose: Determine the number of bytes waiting in the receive buffer
// Input: None
// Returns: Integer number of bytes in the receive buffer
//--------------------------------------------------------------
uint16_t uart1_available(void)
{
return (UART_RX_BUFFER_MASK + UART1_RxHead - UART1_RxTail) % UART_RX_BUFFER_MASK;
}
//--------------------------------------------------------------
// Function: uart1_flush()
// Purpose: Flush bytes waiting the receive buffer. Acutally ignores them.
// Input: None
// Returns: None
//--------------------------------------------------------------
void uart1_flush(void)
{
UART1_RxHead = UART1_RxTail;
}
/*************************************************************************
Function: utoa()
Purpose: convert unsigned integer to ascii
Input: string_buffer, string_buffer_size, unsigned integer value
Returns: first ascii character
**************************************************************************/
char *utoa1(char* buffer, const unsigned int size, unsigned int value)
{
char *p;
// p points to last char
p = buffer+size-1;
// Set terminating Zero
*p='\0';
do
{
*--p = value%10 + '0';
value = value/10;
} while (value!=0 && p>buffer);
return p;
}/* utoa */
#endif
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/uart/uart1.h
0,0 → 1,175
/************************************************************************
Title: Interrupt UART library with receive/transmit circular buffers
Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury
File: $Id: uart.h,v 1.8.2.1 2007/07/01 11:14:38 peter Exp $
Software: AVR-GCC 4.1, AVR Libc 1.4
Hardware: any AVR with built-in UART, tested on AT90S8515 & ATmega8 at 4 Mhz
License: GNU General Public License
Usage: see Doxygen manual
LICENSE:
Copyright (C) 2006 Peter Fleury
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
************************************************************************/
//
// @defgroup pfleury_uart UART Library
// @code #include <uart.h> @endcode
//
// @brief Interrupt UART library using the built-in UART with transmit and receive circular buffers.
//
// This library can be used to transmit and receive data through the built in UART.
//
// An interrupt is generated when the UART has finished transmitting or
// receiving a byte. The interrupt handling routines use circular buffers
// for buffering received and transmitted data.
//
// The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE constants define
// the size of the circular buffers in bytes. Note that these constants must be a power of 2.
// You may need to adapt this constants to your target and your application by adding
// CDEFS += -DUART_RX_BUFFER_SIZE=nn -DUART_RX_BUFFER_SIZE=nn to your Makefile.
//
// @note Based on Atmel Application Note AVR306
// @author Peter Fleury pfleury@gmx.ch http://jump.to/fleury
//
#ifndef UART_H
#define UART_H
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !"
#endif
// constants and macros
// @brief UART Baudrate Expression
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600
//
#define UART_BAUD_SELECT(baudRate,xtalCpu) ((xtalCpu)/((baudRate)*16l)-1)
// @brief UART Baudrate Expression for ATmega double speed mode
// @param xtalcpu system clock in Mhz, e.g. 4000000L for 4Mhz
// @param baudrate baudrate in bps, e.g. 1200, 2400, 9600
//
#define UART_BAUD_SELECT_DOUBLE_SPEED(baudRate,xtalCpu) (((xtalCpu)/((baudRate)*8l)-1)|0x8000)
// Size of the circular receive buffer, must be power of 2
#ifndef UART_RX_BUFFER_SIZE
#define UART_RX_BUFFER_SIZE 1024
#endif
// Size of the circular transmit buffer, must be power of 2
#ifndef UART_TX_BUFFER_SIZE
#define UART_TX_BUFFER_SIZE 64
#endif
// test if the size of the circular buffers fits into SRAM
#if ( (UART_RX_BUFFER_SIZE+UART_TX_BUFFER_SIZE) >= (RAMEND-0x60 ) )
#error "size of UART_RX_BUFFER_SIZE + UART_TX_BUFFER_SIZE larger than size of SRAM"
#endif
//global variable
extern uint8_t receiveNMEA;
extern volatile uint16_t UART1_RxError;
// high byte error return code of uart_getc()
#define UART_FRAME_ERROR 0x0800 // Framing Error by UART
#define UART_OVERRUN_ERROR 0x0400 // Overrun condition by UART
#define UART_BUFFER_OVERFLOW 0x0200 // receive ringbuffer overflow
#define UART_NO_DATA 0x0100 // no receive data available
#define TRACKING_RSSI 1
#define TRACKING_GPS 2
#define TRACKING_MKCOCKPIT 3
#define TRACKING_NMEA 4
//
// function prototypes
//
//
// @brief Initialize UART and set baudrate
// @param baudrate Specify baudrate using macro UART_BAUD_SELECT()
// @return none
//
extern void uart_init(unsigned int baudrate);
//
// @brief Get received byte from ringbuffer
//
// Returns in the lower byte the received character and in the
// higher byte the last receive error.
// UART_NO_DATA is returned when no data is available.
//
// @param void
// @return lower byte: received byte from ringbuffer
// @return higher byte: last receive status
// - \b 0 successfully received data from UART
// - \b UART_NO_DATA
// <br>no receive data available
// - \b UART_BUFFER_OVERFLOW
// <br>Receive ringbuffer overflow.
// We are not reading the receive buffer fast enough,
// one or more received character have been dropped
// - \b UART_OVERRUN_ERROR
// <br>Overrun condition by UART.
// A character already present in the UART UDR register was
// not read by the interrupt handler before the next character arrived,
// one or more received characters have been dropped.
// - \b UART_FRAME_ERROR
// <br>Framing Error by UART
//
extern unsigned int uart_getc(void);
//
// @brief Put byte to ringbuffer for transmitting via UART
// @param data byte to be transmitted
// @return none
//
// @brief Set Baudrate USART1
extern void SetBaudUart1(uint8_t Baudrate);
// @brief Initialize USART1 (only available on selected ATmegas) @see uart_init
extern void uart1_init(unsigned int baudrate);
// @brief Get received byte of USART1 from ringbuffer. (only available on selected ATmega) @see uart_getc
extern unsigned int uart1_getc(void);
// @brief Put byte to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_putc
//extern void uart1_putc(unsigned char data);
extern int uart1_putc(unsigned char data);
// @brief Put string to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts
extern void uart1_puts(const char *s );
// @brief Put string from program memory to ringbuffer for transmitting via USART1 (only available on selected ATmega) @see uart_puts_p
extern void uart1_puts_p(const char *s );
// @brief Macro to automatically put a string constant into program memory
#define uart1_puts_P(__s) uart1_puts_p(PSTR(__s))
extern char *utoa1(char* buffer, const unsigned int size, unsigned int value);
extern uint16_t uart1_available(void);
#endif // UART_H
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/uart/usart.c
0,0 → 1,688
/*****************************************************************************
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* Copyright (C) 2011 Harald Bongartz *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include "../cpu.h"
#include <util/delay.h>
#include <stdarg.h>
#include "../main.h"
#include "usart.h"
#include "../lcd/lcd.h"
#include "../timer/timer.h"
#include "uart1.h"
#include "../eeprom/eeprom.h"
#include "../osd/osd.h"
uint8_t buffer[30];
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t txd_complete = TRUE;
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
volatile uint8_t ReceivedBytes = 0;
volatile uint8_t *pRxData = 0;
volatile uint8_t RxDataLen = 0;
volatile uint16_t stat_crc_error = 0;
volatile uint16_t stat_overflow_error = 0;
volatile uint8_t rx_byte;
volatile uint8_t rxFlag = 0;
#define UART_RXBUFSIZE 64
#define UART_NO_DATA 0x0100 /* no receive data available */
volatile static uint8_t rxbuf[UART_RXBUFSIZE];
volatile static uint8_t *volatile rxhead, *volatile rxtail;
/*
//-----------------------------------------------------------------------------
// USART1 transmitter ISR
ISR (USART1_TX_vect)
{
static uint16_t ptr_txd1_buffer = 0;
uint8_t tmp_tx1;
if(!txd1_complete) // transmission not completed
{
ptr_txd1_buffer++; // [0] was already sent
tmp_tx1 = txd1_buffer[ptr_txd1_buffer];
// if terminating character or end of txd buffer was reached
if((tmp_tx1 == '\r') || (ptr_txd1_buffer == TXD_BUFFER_LEN))
{
ptr_txd1_buffer = 0; // reset txd pointer
txd1_complete = TRUE; // stop transmission
}
UDR1 = tmp_tx1; // send current byte will trigger this ISR again
}
// transmission completed
else ptr_txd1_buffer = 0;
}
*/
#ifdef USART_INT
//-----------------------------------------------------------------------------
// USART0 transmitter ISR
ISR (USART_TX_vect)
{
static uint16_t ptr_txd_buffer = 0;
uint8_t tmp_tx;
if(!txd_complete) // transmission not completed
{
ptr_txd_buffer++; // [0] was already sent
tmp_tx = txd_buffer[ptr_txd_buffer];
// if terminating character or end of txd buffer was reached
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
{
ptr_txd_buffer = 0; // reset txd pointer
txd_complete = TRUE; // stop transmission
}
UDR = tmp_tx; // send current byte will trigger this ISR again
}
// transmission completed
else ptr_txd_buffer = 0;
}
#endif
void SetBaudUart0(uint8_t Baudrate)
{
switch (Baudrate)
{
case Baud_2400: USART_Init( UART_BAUD_SELECT(2400,F_CPU) ); break;
case Baud_4800: USART_Init( UART_BAUD_SELECT(4800,F_CPU) ); break;
case Baud_9600: USART_Init( UART_BAUD_SELECT(9600,F_CPU) ); break;
case Baud_19200: USART_Init( UART_BAUD_SELECT(19200,F_CPU) ); break;
case Baud_38400: USART_Init( UART_BAUD_SELECT(38400,F_CPU) ); break;
case Baud_57600: USART_Init( UART_BAUD_SELECT(57600,F_CPU) ); break;
// case Baud_115200: USART_Init( UART_BAUD_SELECT(115200,F_CPU) ); break;
// Macro erechnet falschen Wert (9,85 = 9) für 115200 Baud mit 20Mhz Quarz, zu grosse Abweichung
//#warning "Baurate prüfen wenn kein 20 Mhz Quarz verwendet wird"
case Baud_115200: USART_Init( 10 ); break;
}
}
//-----------------------------------------------------------------------------
//
//
//uint8_t uart_getc_nb(uint8_t *c)
//{
// if (rxhead==rxtail) return 0;
// *c = *rxtail;
// if (++rxtail == (rxbuf + UART_RXBUFSIZE)) rxtail = rxbuf;
// return 1;
//}
ISR (USART0_RX_vect)
{
static uint16_t crc;
static uint8_t ptr_rxd_buffer = 0;
uint8_t crc1, crc2;
uint8_t c;
// IdleTimer = 0;
if (current_hardware == Wi232)
{
// rx_byte = c;
// rxFlag = 1;
int diff;
uint8_t c;
c=UDR;
diff = rxhead - rxtail;
if (diff < 0) diff += UART_RXBUFSIZE;
if (diff < UART_RXBUFSIZE -1)
{
*rxhead = c;
++rxhead;
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf;
};
// USART_putc (c);
return;
}
if (current_hardware == MKGPS)
{
// rx_byte = c;
// rxFlag = 1;
int diff;
uint8_t c;
c=UDR;
diff = rxhead - rxtail;
if (diff < 0) diff += UART_RXBUFSIZE;
if (diff < UART_RXBUFSIZE -1)
{
*rxhead = c;
++rxhead;
if (rxhead == (rxbuf + UART_RXBUFSIZE)) rxhead = rxbuf;
};
return;
}
c = UDR; // catch the received byte
if (OSD_active && Config.OSD_SendOSD) // Daten an SV2 senden
uart1_putc(c);
if (rxd_buffer_locked)
return; // if rxd buffer is locked immediately return
// the rxd buffer is unlocked
if ((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
{
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
crc = c; // init crc
}
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
{
if(c != '\r') // no termination character
{
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
crc += c; // update crc
}
else // termination character was received
{
// the last 2 bytes are no subject for checksum calculation
// they are the checksum itself
crc -= rxd_buffer[ptr_rxd_buffer-2];
crc -= rxd_buffer[ptr_rxd_buffer-1];
// calculate checksum from transmitted data
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
// compare checksum to transmitted checksum bytes
if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
{ // checksum valid
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
if (mode == rxd_buffer[2])
{
rxd_buffer_locked = TRUE; // lock the rxd buffer
// if 2nd byte is an 'R' enable watchdog that will result in an reset
if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
}
}
else
{ // checksum invalid
stat_crc_error++;
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
ptr_rxd_buffer = 0; // reset rxd buffer pointer
}
}
else // rxd buffer overrun
{
stat_overflow_error++;
ptr_rxd_buffer = 0; // reset rxd buffer
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
}
//-----------------------------------------------------------------------------
// Function: uart0_getc()
// Purpose: return byte from ringbuffer
// Returns: lower byte: received byte from ringbuffer
// higher byte: last receive error
//-----------------------------------------------------------------------------
char USART_getc(void)
{
char val;
// while(rxhead==rxtail) ;
if (rxhead==rxtail)
return val=0;
// IdleTimer = 0;
val = *rxtail;
if (++rxtail == (rxbuf + UART_RXBUFSIZE))
rxtail = rxbuf;
return val;
}
uint8_t uart_getc_nb(uint8_t *c)
{
if (rxhead==rxtail)
return 0;
// IdleTimer = 0;
*c = *rxtail;
if (++rxtail == (rxbuf + UART_RXBUFSIZE))
rxtail = rxbuf;
return 1;
}
//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------
//
void USART_Init (unsigned int baudrate)
{
// set clock divider
// #undef BAUD
// #define BAUD baudrate
// #include <util/setbaud.h>
// UBRRH = UBRRH_VALUE;
// UBRRL = UBRRL_VALUE;
//#ifndef F_CPU
///* In neueren Version der WinAVR/Mfile Makefile-Vorlage kann
// F_CPU im Makefile definiert werden, eine nochmalige Definition
// hier wuerde zu einer Compilerwarnung fuehren. Daher "Schutz" durch
// #ifndef/#endif
//
// Dieser "Schutz" kann zu Debugsessions führen, wenn AVRStudio
// verwendet wird und dort eine andere, nicht zur Hardware passende
// Taktrate eingestellt ist: Dann wird die folgende Definition
// nicht verwendet, sondern stattdessen der Defaultwert (8 MHz?)
// von AVRStudio - daher Ausgabe einer Warnung falls F_CPU
// noch nicht definiert: */
//#warning "F_CPU war noch nicht definiert, wird nun nachgeholt mit 4000000"
//#define F_CPU 18432000UL // Systemtakt in Hz - Definition als unsigned long beachten
// Ohne ergeben sich unten Fehler in der Berechnung
//#endif
//#define BAUD 115200UL // Baudrate
//
//// Berechnungen
//#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1) // clever runden
//#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1))) // Reale Baudrate
//#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD) // Fehler in Promille, 1000 = kein Fehler.
//
//
//#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010))
// #error "Systematischer Fehler der Baudrate grösser 1% und damit zu hoch!"
//#endif
UBRRH = (unsigned char)(baudrate>>8);
UBRRL = (unsigned char) baudrate;
// UBRRH = (unsigned char)(BAUD_REAL>>8);
// UBRRL = (unsigned char) BAUD_REAL;
#if USE_2X
UCSRA |= (1 << U2X); // enable double speed operation
#else
UCSRA &= ~(1 << U2X); // disable double speed operation
#endif
// set 8N1
#if defined (__AVR_ATmega8__) || defined (__AVR_ATmega32__)
UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0);
#else
UCSRC = (1 << UCSZ1) | (1 << UCSZ0);
#endif
UCSRB &= ~(1 << UCSZ2);
// flush receive buffer
while ( UCSRA & (1 << RXC) ) UDR;
UCSRB |= (1 << RXEN) | (1 << TXEN);
#ifdef USART_INT
UCSRB |= (1 << RXCIE) | (1 << TXCIE);
#else
UCSRB |= (1 << RXCIE);
#endif
rxhead = rxtail = rxbuf;
}
//-----------------------------------------------------------------------------
// disable the txd pin of usart
void USART_DisableTXD (void)
{
#ifdef USART_INT
UCSRB &= ~(1 << TXCIE); // disable TX-Interrupt
#endif
UCSRB &= ~(1 << TXEN); // disable TX in USART
DDRB &= ~(1 << DDB3); // set TXD pin as input
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin
}
//-----------------------------------------------------------------------------
// enable the txd pin of usart
void USART_EnableTXD (void)
{
DDRB |= (1 << DDB3); // set TXD pin as output
PORTB &= ~(1 << PORTB3); // disable pullup on TXD pin
UCSRB |= (1 << TXEN); // enable TX in USART
#ifdef USART_INT
UCSRB |= (1 << TXCIE); // enable TX-Interrupt
#endif
}
//-----------------------------------------------------------------------------
// short script to directly send a request thorugh usart including en- and disabling it
// where <address> is the address of the receipient, <label> is which data set to request
// and <ms> represents the milliseconds delay between data
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms)
{
USART_EnableTXD (); // re-enable TXD pin
unsigned char mstenth = ms/10;
SendOutData(cmd, addr, 1, &mstenth, 1);
// wait until command transmitted
while (txd_complete == FALSE);
USART_DisableTXD (); // disable TXD pin again
}
//-----------------------------------------------------------------------------
//
void USART_putc (char c)
{
#ifdef USART_INT
#else
loop_until_bit_is_set(UCSRA, UDRE);
UDR = c;
#endif
}
//-----------------------------------------------------------------------------
//
void USART_puts (char *s)
{
#ifdef USART_INT
#else
while (*s)
{
USART_putc (*s);
s++;
}
#endif
}
//-----------------------------------------------------------------------------
//
void USART_puts_p (const char *s)
{
#ifdef USART_INT
#else
while (pgm_read_byte(s))
{
USART_putc (pgm_read_byte(s));
s++;
}
#endif
}
//-----------------------------------------------------------------------------
//
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
{
va_list ap;
uint16_t pt = 0;
uint8_t a,b,c;
uint8_t ptr = 0;
uint16_t tmpCRC = 0;
uint8_t *pdata = 0;
int len = 0;
txd_buffer[pt++] = '#'; // Start character
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
txd_buffer[pt++] = cmd; // Command
va_start(ap, numofbuffers);
if(numofbuffers)
{
pdata = va_arg (ap, uint8_t*);
len = va_arg (ap, int);
ptr = 0;
numofbuffers--;
}
while(len)
{
if(len)
{
a = pdata[ptr++];
len--;
if((!len) && numofbuffers)
{
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
}
else
a = 0;
if(len)
{
b = pdata[ptr++];
len--;
if((!len) && numofbuffers)
{
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
}
else
b = 0;
if(len)
{
c = pdata[ptr++];
len--;
if((!len) && numofbuffers)
{
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
}
else
c = 0;
txd_buffer[pt++] = '=' + (a >> 2);
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
txd_buffer[pt++] = '=' + ( c & 0x3f);
}
va_end(ap);
for(a = 0; a < pt; a++)
{
tmpCRC += txd_buffer[a];
}
tmpCRC %= 4096;
txd_buffer[pt++] = '=' + tmpCRC / 64;
txd_buffer[pt++] = '=' + tmpCRC % 64;
txd_buffer[pt++] = '\r';
txd_complete = FALSE;
#ifdef USART_INT
UDR = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
#else
for(a = 0; a < pt; a++)
{
loop_until_bit_is_set(UCSRA, UDRE);
UDR = txd_buffer[a];
}
txd_complete = TRUE;
#endif
}
//-----------------------------------------------------------------------------
//
void Decode64 (void)
{
uint8_t a,b,c,d;
uint8_t ptrIn = 3;
uint8_t ptrOut = 3;
uint8_t len = ReceivedBytes - 6;
while (len)
{
a = rxd_buffer[ptrIn++] - '=';
b = rxd_buffer[ptrIn++] - '=';
c = rxd_buffer[ptrIn++] - '=';
d = rxd_buffer[ptrIn++] - '=';
//if(ptrIn > ReceivedBytes - 3) break;
if (len--)
rxd_buffer[ptrOut++] = (a << 2) | (b >> 4);
else
break;
if (len--)
rxd_buffer[ptrOut++] = ((b & 0x0f) << 4) | (c >> 2);
else
break;
if (len--)
rxd_buffer[ptrOut++] = ((c & 0x03) << 6) | d;
else
break;
}
pRxData = &rxd_buffer[3];
RxDataLen = ptrOut - 3;
}
//-----------------------------------------------------------------------------
//
void SwitchToNC (void)
{
if(hardware == NC)
{
// switch to NC
USART_putc (0x1b);
USART_putc (0x1b);
USART_putc (0x55);
USART_putc (0xaa);
USART_putc (0x00);
current_hardware = NC;
_delay_ms (50);
}
}
//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------
//
void SwitchToWi232 (void)
{
// if(hardware == NC)
{
// switch to Wi232
current_hardware = Wi232;
_delay_ms (50);
}
}
//-----------------------------------------------------------------------------
//
void SwitchToFC (void)
{
uint8_t cmd;
if (current_hardware == NC)
{
// switch to FC
cmd = 0x00; // 0 = FC, 1 = MK3MAG, 2 = MKGPS
SendOutData('u', ADDRESS_NC, 1, &cmd, 1);
current_hardware = FC;
_delay_ms (50);
}
}
//-----------------------------------------------------------------------------
//
void SwitchToMAG (void)
{
uint8_t cmd;
if (current_hardware == NC)
{
// switch to MK3MAG
cmd = 0x01; // 0 = FC, 1 = MK3MAG, 2 = MKGPS
SendOutData('u', ADDRESS_NC, 1, &cmd, 1);
current_hardware = MK3MAG;
_delay_ms (50);
}
}
//-----------------------------------------------------------------------------
//
void SwitchToGPS (void)
{
uint8_t cmd;
if (current_hardware == NC)
{
// switch to MKGPS
cmd = 0x02; // 0 = FC, 1 = MK3MAG, 2 = MKGPS
SendOutData('u', ADDRESS_NC, 1, &cmd, 1);
current_hardware = MKGPS;
_delay_ms (50);
}
}
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/uart/usart.h
0,0 → 1,150
/*****************************************************************************
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* Copyright (C) 2011 Harald Bongartz *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
#ifndef _USART_H
#define _USART_H
//--------------------------------------------------------------
//
#ifndef FALSE
#define FALSE 0
#endif
#ifndef TRUE
#define TRUE 1
#endif
// addresses
#define ADDRESS_ANY 0
#define ADDRESS_FC 1
#define ADDRESS_NC 2
#define ADDRESS_MAG 3
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes
#define TXD_BUFFER_LEN 180
#define RXD_BUFFER_LEN 180
// Baud rate of the USART
#define USART_BAUD 57600
//#define USART_BAUD 125000
//--------------------------------------------------------------
//
extern uint8_t buffer[30];
extern volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
extern volatile uint8_t txd_complete;
extern volatile uint8_t txd1_buffer[TXD_BUFFER_LEN];
extern volatile uint8_t txd1_complete;
extern volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
extern volatile uint8_t rxd_buffer_locked;
extern volatile uint8_t ReceivedBytes;
extern volatile uint8_t *pRxData;
extern volatile uint8_t RxDataLen;
extern volatile uint16_t stat_crc_error;
extern volatile uint16_t stat_overflow_error;
extern volatile uint8_t rxFlag;
extern volatile uint8_t rx_byte;
//--------------------------------------------------------------
//
void SetBaudUart0(uint8_t Baudrate);
void USART_Init (unsigned int baudrate);
void USART_DisableTXD (void);
void USART_EnableTXD (void);
void USART_request_mk_data (uint8_t cmd, uint8_t addr, uint8_t ms);
void USART_putc (char c);
void USART_puts (char *s);
void USART_puts_p (const char *s);
extern char USART_getc(void);
void SendOutData (uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...); // uint8_t *pdata, uint8_t len, ...
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, uint8_t *pdata, uint8_t len); // uint8_t *pdata, uint8_t len, ...
void Decode64 (void);
void SwitchToNC (void);
void SwitchToFC (void);
void SwitchToMAG (void);
void SwitchToGPS (void);
void SwitchToWi232 (void);
void debug1(void);
uint8_t uart_getc_nb(uint8_t*);
//--------------------------------------------------------------
//Anpassen der seriellen Schnittstellen Register
#define USART_RXC_vect USART0_RX_vect
//--------------------------------------------------------------
#define UCSRA UCSR0A
#define UCSRB UCSR0B
#define UCSRC UCSR0C
#define UDR UDR0
#define UBRRL UBRR0L
#define UBRRH UBRR0H
// UCSRA
#define RXC RXC0
#define TXC TXC0
#define UDRE UDRE0
#define FE FE0
#define UPE UPE0
#define U2X U2X0
#define MPCM MPCM0
// UCSRB
#define RXCIE RXCIE0
#define TXCIE TXCIE0
#define UDRIE UDRIE0
#define TXEN TXEN0
#define RXEN RXEN0
#define UCSZ2 UCSZ02
#define RXB8 RXB80
#define TXB8 TXB80
// UCSRC
#define UMSEL1 UMSEL01
#define UMSEL0 UMSEL00
#define UPM1 UPM01
#define UPM0 UPM00
#define USBS USBS0
#define UCSZ1 UCSZ01
#define UCSZ0 UCSZ00
#define UCPOL UCPOL0
#endif
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/waypoints/waypoints.c
0,0 → 1,360
/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// + for non-commercial use (directly or indirectly)
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
//
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdlib.h>
#include <avr/io.h>
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
//#include "91x_lib.h"
#include "waypoints.h"
#include "../mk-data-structs.h"
#include "../uart/uart1.h"
#include "../eeprom/eeprom.h"
#include <util/delay.h>
// the waypoints list
NaviData_t *NaviData;
//Point_t PointList[MAX_LIST_LEN];
uint8_t WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
uint8_t POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
uint8_t WPCount = 0; // number of waypoints
uint8_t PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN
uint8_t POICount = 0;
uint8_t WPActive = false;
uint8_t PointList_Init(void)
{
return PointList_Clear();
}
uint8_t PointList_Clear(void)
{
uint8_t i;
WPIndex = 0; // real list position are 1 ,2, 3 ...
POIIndex = 0; // real list position are 1 ,2, 3 ...
WPCount = 0; // no waypoints
POICount = 0;
PointCount = 0; // no contents
WPActive = false;
NaviData->WaypointNumber = WPCount;
NaviData->WaypointIndex = 0;
for(i = 0; i < MAX_LIST_LEN; i++)
{
Config.PointList[i].Position.Status = INVALID;
Config.PointList[i].Position.Latitude = 0;
Config.PointList[i].Position.Longitude = 0;
Config.PointList[i].Position.Altitude = 0;
Config.PointList[i].Heading = 361; // invalid value
Config.PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
Config.PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
Config.PointList[i].Type = POINT_TYPE_INVALID;
Config.PointList[i].Event_Flag = 0; // future implementation
Config.PointList[i].AltitudeRate = 0; // no change of setpoint
}
return true;
}
uint8_t PointList_GetCount(void)
{
return PointCount; // number of points in the list
}
Point_t* PointList_GetAt(uint8_t index)
{
if((index > 0) && (index <= PointCount)) return(&(Config.PointList[index-1])); // return pointer to this waypoint
else return(NULL);
}
uint8_t PointList_SetAt(Point_t* pPoint)
{
// if index is in range
if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
{
// check list entry before update
switch(Config.PointList[pPoint->Index-1].Type)
{
case POINT_TYPE_INVALID: // was invalid
switch(pPoint->Type)
{
default:
case POINT_TYPE_INVALID:
// nothing to do
break;
case POINT_TYPE_WP:
WPCount++;
PointCount++;
break;
case POINT_TYPE_POI:
POICount++;
PointCount++;
break;
}
break;
case POINT_TYPE_WP: // was a waypoint
switch(pPoint->Type)
{
case POINT_TYPE_INVALID:
WPCount--;
PointCount--;
break;
default:
case POINT_TYPE_WP:
//nothing to do
break;
case POINT_TYPE_POI:
POICount++;
WPCount--;
break;
}
break;
case POINT_TYPE_POI: // was a poi
switch(pPoint->Type)
{
case POINT_TYPE_INVALID:
POICount--;
PointCount--;
break;
case POINT_TYPE_WP:
WPCount++;
POICount--;
break;
case POINT_TYPE_POI:
default:
// nothing to do
break;
}
break;
}
memcpy(&Config.PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
NaviData->WaypointNumber = WPCount;
return pPoint->Index;
}
else return(0);
}
// returns the pointer to the first waypoint within the list
Point_t* PointList_WPBegin(void)
{
uint8_t i;
WPIndex = 0; // set list position invalid
if(WPActive == false) return(NULL);
POIIndex = 0; // set invalid POI
if(PointCount > 0)
{
// search for first wp in list
for(i = 0; i <MAX_LIST_LEN; i++)
{
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID))
{
WPIndex = i + 1;
break;
}
}
if(WPIndex) // found a WP in the list
{
NaviData->WaypointIndex = 1;
// update index to POI
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
else POIIndex = 0;
}
else // some points in the list but no WP found
{
NaviData->WaypointIndex = 0;
//Check for an existing POI
for(i = 0; i < MAX_LIST_LEN; i++)
{
if((Config.PointList[i].Type == POINT_TYPE_POI) && (Config.PointList[i].Position.Status != INVALID))
{
POIIndex = i + 1;
break;
}
}
}
}
else // no point in the list
{
POIIndex = 0;
NaviData->WaypointIndex = 0;
}
if(WPIndex) return(&(Config.PointList[WPIndex-1]));
else return(NULL);
}
// returns the last waypoint
Point_t* PointList_WPEnd(void)
{
uint8_t i;
WPIndex = 0; // set list position invalid
POIIndex = 0; // set invalid
if(WPActive == false) return(NULL);
if(PointCount > 0)
{
// search backward!
for(i = 1; i <= MAX_LIST_LEN; i++)
{
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
{
WPIndex = MAX_LIST_LEN - i + 1;
break;
}
}
if(WPIndex) // found a WP within the list
{
NaviData->WaypointIndex = WPCount;
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
else POIIndex = 0;
}
else // list contains some points but no WP in the list
{
// search backward for a POI!
for(i = 1; i <= MAX_LIST_LEN; i++)
{
if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
{
POIIndex = MAX_LIST_LEN - i + 1;
break;
}
}
NaviData->WaypointIndex = 0;
}
}
else // no point in the list
{
POIIndex = 0;
NaviData->WaypointIndex = 0;
}
if(WPIndex) return(&(Config.PointList[WPIndex-1]));
else return(NULL);
}
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
Point_t* PointList_WPNext(void)
{
uint8_t wp_found = 0;
if(WPActive == false) return(NULL);
if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
{
uint8_t i;
for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
{
if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) // jump over POIs
{
wp_found = i+1;
break;
}
}
}
if(wp_found)
{
WPIndex = wp_found; // update list position
NaviData->WaypointIndex++;
if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
else POIIndex = 0;
return(&(Config.PointList[WPIndex-1])); // return pointer to this waypoint
}
else
{ // no next wp found
NaviData->WaypointIndex = 0;
POIIndex = 0;
return(NULL);
}
}
void PointList_WPActive(uint8_t set)
{
if(set)
{
WPActive = true;
PointList_WPBegin(); // uopdates POI index
}
else
{
WPActive = false;
POIIndex = 0; // disable POI also
}
}
Point_t* PointList_GetPOI(void)
{
return PointList_GetAt(POIIndex);
}
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property

/Transportables_Koptertool/branch/test2/GPL_PKT_V3_6_7f_FC090b/waypoints/waypoints.h
0,0 → 1,56
#ifndef _WAYPOINTS_H
#define _WAYPOINTS_H
//#include "ubx.h"
#include "../mk-data-structs.h"
#define POINT_TYPE_INVALID 255
#define POINT_TYPE_WP 0
#define POINT_TYPE_POI 1
#define INVALID 0x00
//typedef struct
//{
// int32_t Longitude; // in 1E-7 deg
// int32_t Latitude; // in 1E-7 deg
// int32_t Altitude; // in mm
// uint8_t Status;// validity of data
//} __attribute__((packed)) GPS_Pos_t;
//typedef struct
//{
// GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details
// int16_t Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List
// uint8_t ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered
// uint8_t HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
// uint8_t Event_Flag; // future implementation
// uint8_t Index; // to indentify different waypoints, workaround for bad communications PC <-> NC
// uint8_t Type; // typeof Waypoint
// uint8_t WP_EventChannelValue; //
// uint8_t AltitudeRate; // rate to change the setpoint
// uint8_t reserve[8]; // reserve
//} __attribute__((packed)) Point_t;
// Init List, return TRUE on success
uint8_t PointList_Init(void);
// Clear List, return TRUE on success
uint8_t PointList_Clear(void);
// Returns number of points in the list
uint8_t PointList_GetCount(void);
// return pointer to point at position
Point_t* PointList_GetAt(uint8_t index);
// set a point in the list at index, returns its index on success, else 0
uint8_t PointList_SetAt(Point_t* pPoint);
// goto the first WP in the list and return pointer to it
Point_t* PointList_WPBegin(void);
// goto the last WP in the list and return pointer to it
Point_t* PointList_WPEnd(void);
// goto next WP in the list and return pointer to it
Point_t* PointList_WPNext(void);
// enables/disables waypoint function
void PointList_WPActive(uint8_t set);
// returns pointer to actual POI
Point_t* PointList_GetPOI(void);
#endif // _WAYPOINTS_H
Property changes:
Added: svn:mime-type
+text/plain
\ No newline at end of property