Ich hab sie mal reingepackt, bekomm das nicht in den anhang
//RSSI Tracker für den FPV Einsatz, Version 0.98
#include <LiquidCrystal.h>
#include <Servo.h>
//SETUP
int wartezeit = 150; //Standart Wartezeit zwischen den Servo Bewegungen
int Vert_mitte = 125; //Initialisierungswerte für die Servos
int Hori_mitte = 90; //Initialisierungswerte für die Servos
int schrittweitehori = 4;
int low_calibrate1 = 0; //linke Helix
int low_calibrate2 = 0; //mittlere Helix
int low_calibrate3 = 0; //rechte Helix
int low_calibrate4 = 0; //SPW
int high_calibrate1 = 0; //linke Helix
int high_calibrate2 = 0; //mittlere Helix
int high_calibrate3 = 0; //rechte Helix
int high_calibrate4 = 0; //SPW
int low_site_korrektur = 0; //Korrektur Wert für die seitlichen Helix Antennen, da selten alle Empfänger im Grundrauschen den gleichen RSSI Werte haben, ausgeschaltetes Video
int high_site_korrektur = 0; //Korrektur Wert für die seitlichen Helix Antennen, da selten alle Empfänger im Grundrauschen den gleichen RSSI Werte haben, eingeschaltetes Video
int horizontalwert = 0;
int hori=0;
int vert=0;
int mitte_alt=0;
int gefunden = 0;
int mitte_korrektur = 0; //Korrekturwert für die mittlere Helix falls unterschiedliche Windungszahlen zum Einsatz kommen
int fein = 7; //feine Schrittweite beim horzontalen Tracking
int grob = 15; //grobe Schrittweite beim horzontalen Tracking
int rauschminderung = 0; //Gibt die Differenz zwischen der rechten und linken Helix an, bei der noch kein Tracking statt findet
int initialfund = 0; // Wenn "initialfund = 0" wird zum Anfang versucht den Flieger mit großen Schritten zu finden
int debug = 1; //Wenn "debug" = 1 werden die Low und High Werte im Setup Bereich auf dem LCD Display ausgegeben
int abbruchkriterium = 1; // Wenn 0 wird die Differenz (low Werte) zwischen linker und rechter Helix im Setup Prozess verglichen und gegebenenfalls abgebrochen
int modus = A5; //Taster Abfrage, ist mit 47KOhm auf 5V hochgezogen und wird mit dem Taster auf Masse geschaltet
float spannung = A4; //Spannungsüberwachung der Bodenstation
int rssispw = A3; //SPW Antenne
int rssilinks = A2; //linke Helix wenn man in richtung Modell schaut
int rssimitte = A1; //Bild gebende Helix wenn man in richtung Modell schaut
int rssirechts = A0; //rechte Helix wenn man in richtung Modell schaut
int SPKR = 9;
int links = 0;
int mitte = 0;
int rechts = 0;
int high = 0;
int low = 0;
int spw = 0;
int x=0;
Servo myservo1; // create servo object to control a servo
Servo myservo2; // create servo object to control a servo
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
void playTone(int tone, int duration)
{
for (long i = 0; i < duration * 1000L; i += tone * 2)
{
digitalWrite(SPKR, HIGH);
delayMicroseconds(tone);
digitalWrite(SPKR, LOW);
delayMicroseconds(tone);
}
}
void DisplayValues (int links, int rechts, int mitte, int spw)
{
//Display rssi values
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("L:");
lcd.setCursor(2, 0);
lcd.print(links);
lcd.setCursor(6, 0);
lcd.print("R:");
lcd.setCursor(8, 0);
lcd.print(rechts);
lcd.setCursor(0, 1);
lcd.print("M:");
lcd.setCursor(2, 1);
lcd.print(mitte);
lcd.setCursor(6, 1);
lcd.print("SPW:");
lcd.setCursor(10, 1);
lcd.print(spw);
lcd.setCursor(12, 0);
lcd.print (float(analogRead(spannung)/293.0)*5.0); //Betriebsspannung der Bodenstation
}
void check_switch()
{
if (analogRead(modus)==0) //Überprüfe den Schalter für das niedrigere vertikale Tracking
{
if (Vert_mitte == 125) //Schalte auf einen niedrigeren vertikalen Wert um und zurück
{
Vert_mitte = 140;
}
else
{
Vert_mitte = 125;
}
}
}
void read_rssi_value()
{
// Messe die Werte mittleren Helix und der SPW
mitte = (map(analogRead(rssimitte), low_calibrate2, high_calibrate2, 1, 100) + mitte_korrektur); //korrigiere den Messwert der mittleren Helix, falls unterschiedliche Antennen zum Einsatz kommen
spw = map(analogRead(rssispw), low_calibrate4, high_calibrate4, 1, 100);
// Messe die Werte der äußeren Helix und kalibriere den Low Wert auf das Grundrauschen am jeweiligen Ort
if (low > 1) //linke Helix hat ein stärkeres Low Signal
{
links = map((analogRead(rssilinks) - low_site_korrektur), low_calibrate1, high_calibrate1, 1, 100); // korrigiere den Messwert um die Abweichung an Flugort
rechts = map(analogRead(rssirechts),low_calibrate3, high_calibrate3, 1, 100);
}
else //rechte Helix hat ein stärkeres Low Signal
{
rechts = map((analogRead(rssirechts) - low_site_korrektur) ,low_calibrate3, high_calibrate3, 1, 100); // korrigiere den Messwert um die Abweichung an Flugort
links = map(analogRead(rssilinks), low_calibrate1, high_calibrate1, 1, 100);
}
}
void setup()
{
lcd.begin(16, 2);
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Joern Kossert");
lcd.setCursor(0, 1);
lcd.print("RSSI-FPV-Tracker");
myservo1.attach(11); // attaches the horizontal servo on pin 10 to the servo object
myservo1.write(Hori_mitte);
myservo2.attach(10); // attaches vertical the servo on pin 11 to the servo object
myservo2.write(Vert_mitte);
// Serial.begin(9600);
pinMode(SPKR, OUTPUT); //set the speaker as output
//Kalibrierung des LOW RSSI Wertes bei ausgeschaltetem Video Sender
//-----------------------------------------------------------------
low_calibrate1=analogRead(rssilinks);
low_calibrate2=analogRead(rssimitte);
low_calibrate3=analogRead(rssirechts);
low_calibrate4=analogRead(rssispw);
low_site_korrektur = abs (low_calibrate1 - low_calibrate3);
if (low_calibrate1 <= low_calibrate3)
{
low = 1; //rechte Helix hat ein stärkeres Low Signal
low_calibrate3 = low_calibrate3 - low_site_korrektur;
}
else
{
low = 2; //linke Helix hat ein stärkeres Low Signal
low_calibrate1 = low_calibrate1- low_site_korrektur;
}
playTone(1950,300);
playTone(1700,300);
playTone(1519,300);
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Video Sender");
lcd.setCursor(0, 1);
lcd.print("einschalten!");
//Countdown zum Einschalten des Video Senders
//-------------------------------------------
for ( x = 1; x < 10; x++)
{
delay(1000);
lcd.setCursor(14, 1);
lcd.print(" ");
lcd.setCursor(14, 1);
lcd.print(10-x);
}
playTone(1500,100); //Kalibrierung der High Werte
//Kalibrierung des HIGH RSSI Wertes bei eingeschaltetem Video Sender
//-------------------------------------------------------------------
high_calibrate1=analogRead(rssilinks);
high_calibrate2=analogRead(rssimitte);
high_calibrate3=analogRead(rssirechts);
high_calibrate4=analogRead(rssispw);
high_site_korrektur = abs (high_calibrate1 - high_calibrate3);
if (high_calibrate1 <= high_calibrate3)
{
high = 1; //rechte Helix hat ein stärkeres High Signal
high_calibrate3 = high_calibrate3 - high_site_korrektur;
}
else
{
high = 2; //linke Helix hat ein stärkeres High Signal
high_calibrate1 = high_calibrate1- high_site_korrektur;
}
// Ausgabe der Debug Werte aus der Intialisierung
// ----------------------------------------------
if (debug == 1)
{
playTone(1500,100); //Debug Ausgabe
// Ausgabe der Werte im LOW Bereich
lcd.clear();
lcd.setCursor(13, 0);
lcd.print("Low");
lcd.setCursor(0, 0);
lcd.print("L:");
lcd.setCursor(2, 0);
lcd.print(low_calibrate1);
lcd.setCursor(6, 0);
lcd.print("R:");
lcd.setCursor(8, 0);
lcd.print(low_calibrate3);
lcd.setCursor(0, 1);
lcd.print("M:");
lcd.setCursor(2, 1);
lcd.print(low_calibrate2);
lcd.setCursor(6, 1);
lcd.print("SPW:");
lcd.setCursor(10, 1);
lcd.print(low_calibrate4);
for ( x = 1; x < 6; x++)
{
delay(1000);
}
playTone(1500,100); //Anzeige der High Werte
// Ausgabe der Werte im HIGH Bereich
lcd.clear();
lcd.setCursor(12, 0);
lcd.print("High");
lcd.setCursor(0, 0);
lcd.print("L:");
lcd.setCursor(2, 0);
lcd.print(high_calibrate1);
lcd.setCursor(6, 0);
lcd.print("R:");
lcd.setCursor(8, 0);
lcd.print(high_calibrate3);
lcd.setCursor(0, 1);
lcd.print("M:");
lcd.setCursor(2, 1);
lcd.print(high_calibrate2);
lcd.setCursor(6, 1);
lcd.print("SPW:");
lcd.setCursor(10, 1);
lcd.print(high_calibrate4);
for ( x = 1; x < 6; x++)
{
delay(1000);
}
} // Ende debug
//Abbruch Kriterium falls die Werte von rechter und linker Helix zu weit auseinander liegen
//kann mit der neuen ortsungebundenen Kalibrierung eigentlich nicht mehr auftreten
//-----------------------------------------------------------------------------------------
if ((abs (low_calibrate1 - low_calibrate3) > 10) && abbruchkriterium == 1) //Unterschied von linker zu rechter Helix zu groß für zuverlässiges Tracking
{
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("LOW Werte");
lcd.setCursor(0, 1);
lcd.print("ungueltig! ENDE");
x=1;
do //Endlosschleife für neue Initialisierung
{}
while (x==1);
}
//Initales schnell Finden des Fliegers, diese Routine scannt in groben Schritten den vollen Servo Bereich nach dem Flieger
//------------------------------------------------------------------------------------------------------------------------
while (initialfund == 0)
{
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
mitte = mitte +10; // Hilft beim initalen Finden auf kurze Entfernungen
DisplayValues(links,rechts,mitte,spw);
if (horizontalwert > 85)
{
horizontalwert = -85;
}
if(mitte > links && mitte > rechts) //Flieger gefunden
{
initialfund = 1;
}
else
{
horizontalwert = horizontalwert + 5;
hori = horizontalwert + Hori_mitte; // Bestimme den aktuellen Horizontalwert für das Servo
myservo1.write(hori); //Schicke geänderten horizontalen Wert an das Servo
}
delay(350);
} //ENDE While Schleife "Initiales Finden des Fliegers"
} // Ende Setup
//Hauptprogramm
//-------------
void loop()
{
check_switch(); // Taster für niedrigere default Vertikalwert abfragen
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
DisplayValues(links,rechts,mitte,spw); //Anzeigen der Werte im LCD Display
if (abs((links - rechts)) > 20) //Schrittweite in Abhängigkeit der Differenz anpassen
{
schrittweitehori = grob;
}
else
{
schrittweitehori = fein;
}
// Test auf optimale Ausrichtung
//------------------------------
if(mitte < links || mitte < rechts) //Ausrichtung ist nicht optimal
{
vert = 0; // Reset der vertikalen Korrektur
if(abs(links - rechts) > rauschminderung) //Rauschminderung, wenn der Wert zwichen links und rechts nur minimal schwankt, mache nichts
{
if (links > mitte && links >= rechts) //Flieger ist links
{
horizontalwert = horizontalwert + schrittweitehori; //Horizontalwert um die Schrittweite erhöhen
lcd.setCursor(15, 1);
lcd.print("<");
}
if (rechts > mitte && rechts >= links) //Flieger ist rechts
{
horizontalwert = horizontalwert - schrittweitehori; //Horizontalwert um die Schrittweite verringern
lcd.setCursor(15, 1);
lcd.print(">");
}
hori = horizontalwert + Hori_mitte; // Errechne den aktuellen Horizontalwert für das Servo
//Wenn der Flieger bereits im Bereich der Endanschlags des Servos ist, nehme die Horizontale Änderung zurück und setzte das Servo auf Endanschlag -2 Grad
if (hori >= 178)
{
horizontalwert = horizontalwert - schrittweitehori; //Rücknahme der letzten Änderung, da das Servo diesen Schwenk bauartbedingt nicht ausführen kann
hori = 178; //Endanschlag -2 Grad
//playTone(1500,100); //Warnton Flieger verlässt den Tracking Bereich
}
if (hori <= 2)
{
horizontalwert = horizontalwert + schrittweitehori; //Rücknahme der letzten Änderung, da das Servo diesen Schwenk bauartbedingt nicht ausführen kann
hori = 2; //Endanschlag -2 Grad
//playTone(1500,100); //Warnton Flieger verlässt den Tracking Bereich
}
myservo1.write(hori); //Schicke geänderten horizontalen Wert an das Servo
myservo2.write(Vert_mitte); //Schicke den Standard vertikalen Wert an das Servo
delay(wartezeit);
gefunden = 0; //Status auf Flieger nicht gefunden setzen
}
else
{
delay(wartezeit);
gefunden = 0; //Status auf Flieger nicht gefunden setzen
}
}
else // Else Zweig wenn mittlere Helix den höchsten Wert hat
{
// Ausrichtung fast optimal, Flieger gefunden, versuche durch vertikale Änderung und horizontales Folgen zu optimieren
// -------------------------------------------------------------------------------------------------------------------
lcd.setCursor(15, 1);
lcd.print("^"); // Zeichen für aktives Tracking mit der mittleren Helix
lcd.setCursor(13, 0);
if (gefunden == 0 )
{
playTone(956,300); // Ton das der Flieger jetzt im optimalen Berecih der mittleren Helix ist
gefunden = 1; // setzte ein Flag damit er nicht jedesmal piept
}
mitte_alt=mitte; // Merke den Messwert der vorherigen Messung
check_switch(); // Taster für niedrigere default Vertikalwert abfragen
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
// feine horizontale Korrektur im Modus "Flieger gefunden"
if (((horizontalwert + Hori_mitte) >= 4) && ((horizontalwert + Hori_mitte) <= 176)) // ist das Servo im gültigen Bereich?
{
if ((rechts - links) > 4) // wenn ja, Flieger verlässt den optimalen Bereich obwohl die mittlere Helix noch den besten Wert liefert
{
horizontalwert = horizontalwert - 2; // führe horizontal nach
}
else if ((links - rechts) > 4) // wenn ja, Flieger verlässt den optimalen Bereich obwohl die mittlere Helix noch den besten Wert liefert
{
horizontalwert = horizontalwert + 2; // führe horizontal nach
}
}
myservo1.write(Hori_mitte + horizontalwert); // Setze den neuen Horizontalwert für das Servo
// feine vertikale Korrektur im Modus "Flieger gefunden"
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
mitte_alt = mitte; // Merke den aktuellen Wert für die nächste Vergleichsmessung
vert = vert + 1; // schwenke die Helix um ein Grad nach unten
myservo2.write(Vert_mitte + vert); // Setze den neuen Vertikalwert für das Servo
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
if (mitte_alt > mitte) // wenn ja - gab es eine Verschlechterung durch die letzte vertikale Änderung
{
vert = vert - 1; // schwenke die Helix um ein Grad nach oben
myservo2.write(Vert_mitte + vert); // Setze den neuen Vertikalwert für das Servo
}
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
mitte_alt = mitte; // Merke den aktuellen Wert für die nächste Vergleichsmessung
vert = vert - 1; // schwenke die Helix um ein Grad nach oben
myservo2.write(Vert_mitte + vert); // Setze den neuen Vertikalwert für das Servo
read_rssi_value(); // Einlesen der RSSI Werte für alle Antennen
if (mitte_alt > mitte) // wenn ja - gab es eine Verschlechterung durch die letzte vertikale Änderung
{
vert = vert + 1; // schwenke die Helix um ein Grad nach unten
myservo2.write(Vert_mitte + vert); // Setze den neuen Vertikalwert für das Servo
}
delay(wartezeit);
}
} //Ende Loop
