/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, ¤tPos); |
// angleTilt = RAD_TO_DEG * (double)atan2((double)(currentPos.Altitude - MK_pos.Home_Alt) / 1000, geo.distance); |
// //if (geo.distance < 4 || (geo.distance < 6 && hysteresis)) { // < 4m ==> Pan-Servo in Mittelstellung. Hysterese bis 6m, damit Servo im Grenzbereich nicht wild rumschlägt |
// //geo.bearing = MK_pos.direction; |
// //angleTilt = 0; |
// //hysteresis = 1; |
// //} |
// //else { |
// //hysteresis = 0; |
// //} |
//// |
// //// egal wo der Übergangspunkt 359, 360, 1grd ist => Winkelübergangspunkt auf 0 bzw. 180grd des Servos bringen |
// //// 360 grd negative Winkelwerte als positive |
// anglePan = (geo.bearing + 450 - MK_pos.direction) % 360; // 450 = 360 + 90 |
// |
// //if (angleTilt < 0) angleTilt = 0; |
// //if (angleTilt > 180) angleTilt = 180; |
//// |
// //if (anglePan >= 180) { // zwecks 360grd-Abdeckung flipt Pan-/Tilt-Servo |
// //anglePan = anglePan - 180; |
// //angleTilt = 180 - angleTilt; |
// // |
// //} |
////angleTilt = 180; |
////angleTilt = 180; |
// |
//// servoSetAngle(0, anglePan); |
//// servoSetAngle(1, angleTilt); |
//} |
/****************************************************************/ |
/* */ |
/* MK GPS Tracking */ |
/* */ |
/****************************************************************/ |
// MK OSD-Daten lesen und verifizieren |
//uint8_t OSD_Data_valid(NaviData_t **navi_data) |
//{ uint8_t ret = 0; |
//char *tx_osd = {"#co?]==EH\r"}; |
//// char interval[2] = {10, '\0'}; |
// |
//if (rx_line_decode('O')) { // OSD-Datensatz prüfen/dekodieren |
////*navi_data = (NaviData_t*)data_decode; // dekodierte Daten mit Struktur OSD-Daten versehen |
//if (rx_timeout < RX_TIME_OLD) { // GPS-Daten nicht zu alt und ok. |
//currentPos = (*navi_data)->CurrentPosition; |
//if ((*navi_data)->NCFlags & NC_FLAG_GPS_OK) |
//ret = 1; |
//// aus MkCockpit http://forum.mikrokopter.de/topic-post216136.html#post216136 |
//// (4 * (........))/5 ==> Wichtung Luftdruck-Höhe zu GPS |
//currentPos.Altitude = MK_pos.Home_Alt + (4000 * (int32_t)((*navi_data)->Altimeter) / AltFaktor + currentPos.Altitude - MK_pos.Home_Alt) / 5; |
//satsInUse = (*navi_data)->SatsInUse; |
//} |
//} |
//// ca. 210ms keine OSD-Daten empfangen ==> sende neue Anforderung an MK |
//// if ((track_tx) && (rx_timeout > RX_TIMEOUT)) tx_Mk(NC_ADDRESS, 'o', interval, 1); // 420 * 0.5ms interval |
//if ((track_tx) && (rx_timeout > RX_TIMEOUT)) SendOutData(tx_osd); // 420 * 0.5ms interval |
//return ret; |
//} |
// |
// MK eingeschaltet und GPS-ok, danach Motoren gestartet ==> Berechnung horizontaler/vertikaler Servowinkel |
// Hauptprogramm von GPS Antennen-Nachführung |
//void Tracking_GPS(void) |
//{ //NaviData_t *navidata; |
// static uint8_t track_running = 0; |
// |
// if (!track_running) |
// { |
// //track_running = 1; // verhindert doppelten Aufruf, wenn in Eingabeschleife Menu_MK_BatteryChangeNr() !!! |
// //if (OSD_Data_valid(&naviData)) { |
// if (coldstart) |
// { |
// //// erst nach Neustart NGVideo und beim Motorstart werden Daten vom MK übernommen |
// //if (naviData->FCFlags & FC_FLAG_MOTOR_START) |
// //{ |
// MK_pos.Home_Lon = (double)naviData->HomePosition.Longitude / 10000000.0; |
// MK_pos.Home_Lat = (double)naviData->HomePosition.Latitude / 10000000.0; |
// MK_pos.Home_Lon7 = naviData->HomePosition.Longitude; |
// MK_pos.Home_Lat7 = naviData->HomePosition.Latitude; |
// MK_pos.Home_Alt = naviData->HomePosition.Altitude; |
// MK_pos.direction = naviData->CompassHeading; |
// coldstart = 0; |
// //} |
// //} |
// //else { |
// //do_tracking(); |
// } |
// //} |
// track_running = 0; |
// } |
// do_tracking(); |
//} |
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, ¤tPos); |
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 |