Einfacher RTH Code

Status
Nicht offen für weitere Antworten.

cesco1

Erfahrener Benutzer
#1
Ich nehme an ein gps nmea parser ist vorhanden und liefert eine position mit 1 oder 2 hz. Es hat 2 abläufe, berechnungen nach einer neuen gps position (0.5 oder 1 sek), und berechnungen vor jedem pid loop (bei mir 3 bis 20 ms).

Bei neuer gps-position:
direction_to_home und distance_to_home berechnen
GPS_heading (aktueller kurs) berechnen falls kein mag

vor jedem pid:
act_heading aus GPS_heading und gyro daten zusammenbasteln (falls kein mag)
direction_err_to_home berechnen, wohin muss ich steuern.



Beim flieger:
direction_err_to_home auf seitenruder und quer aufmischen, nur P anteil. Dazu noch etwas abs(P) auf höhenruder geben damit der flieger nicht sinkt.

Beim quad:
PD = distance_to_home - distance_speed (new_dist - old dist gefiltert)
PD * cos(direction_err_to_home) auf pitch aufmischen,
PD * sin(direction_err_to_home) auf roll aufmischen
Der D anteil muss sehr gross sein. Der quad muss aktiv und heftig "auf die bremse treten" bevor er home erreicht. Idealerweise speed = 0 bei home position. Der fehlende I anteil bewirkt bis zu 10m versatz bei wind.



Files:
nav.ino sind die berechnungen.
rth.txt sind die funktionsaufrufe von main aus.
gps.ino ist der von multiwii kopierte nmea gps parser.

https://www.rcgroups.com/forums/show...6&d=1496803524

Für flieger ohne mag muss der gps-kurs zwischen messungen mit gyrodaten korrigiert werden, sonst fliegt der flieger dauer-schlangenlinie. Dazu hab ich was gebastelt (würg), aber ich denke die neue IMU von cleanflight/betaflight kann das besser. Die alte multiwii imu kanns nicht.

Das ganze atmega32u4 projekt für flieger (hardcoded waypoints, althold ist schrott):
https://www.rcgroups.com/forums/showatt.php?attachmentid=10101796&d=1496836236

Für copter brauchts immer ein mag. Die copter geschwindikeit beim einschalten von RTH wird nicht beachtet. Das bewirkt ein "krummes" anfliegen der home position und sollte korrigiert werden.

Das ganze atmega2560 copter projekt (basiert auf mwii 2.2):
https://www.rcgroups.com/forums/showatt.php?attachmentid=10101816&d=1496838168

btw
die formel GPS_heading = atan2(dLon,dLat); ist richtig, was apm und clones (mwii, baseflight) benutzen ist würg.
 
Zuletzt bearbeitet:

cesco1

Erfahrener Benutzer
#2
Die PD regelung der RTH funktion beim copter:

Beachte die regelung ist eindimensional.
P-term ist die auf einen maximalwert beschränkte position (distanz von home)
D-term ist die auf maximalwert beschränkte geschwindigkeit mit der der copter auf home zufliegt.
PD - term ist P minus D, wobel P und D noch mit den pd faktoren skaliert werden.

Man sieht zuerst dominiert der P anteil und beschleunigt den copter richtung home, dann nahe bei home dominiert der D anteil, PD wird negativ und bremst den copter wieder ab. Das sieht eindrücklich aus. Der copter rast auf dich zu, und 10m vor dir kippt er nach hinten und bremst bis stillstand.



Dasselbe mit geringeren pd werten:
 
Zuletzt bearbeitet:

brm

Erfahrener Benutzer
#3
hmm, verwende nur mehr ublox dinger und den normalen ublox binary stream.
aber wenn man die geschichte an den normalen grössen aufhängt müsste jedes gps gerät die daten liefern können.

anstelle eines einfachen pid reglers würde ich einen kaskadierten regler verwenden.

aber nach dem heutigen geburri fest ist mir nur nicht nach denken/mathe zumute.

danke für den ansatz!
 

brm

Erfahrener Benutzer
#4
Zuletzt bearbeitet:

cesco1

Erfahrener Benutzer
#5
die geschichte an den normalen grössen aufhängt
Das verwendet nur gps position, sonst gar nix. Woher das kommt ist eigentlich egal.

Bei distanzen von 100km sicher unnötig.

Was anderes:
Beim copter ist das zurückfliegen manchmal nicht exakt auf home sondern etwas daneben, und erst dann kriecht er auf home. Ich weiss nicht ob das mag fehler sind oder die schon vorhandene copter-geschwindikeit beim einschalten, oder die "small angle approx" beim zumischen der gps winkel zur copter lageregelung oder einfach wind.
 
Zuletzt bearbeitet:

brm

Erfahrener Benutzer
#6
Das verwendet nur gps position, sonst gar nix. Woher das kommt ist eigentlich egal.
Bei distanzen von 100km sicher unnötig.
sehe ich nicht so. beim setzen der gps home position kann/muss man das einmal rechnen.
ich denke nicht, dass noch 100km fliegst ;-)

die ungenauigkeit kann daher rühren, dass eben die details in der berechnung nicht stimmen.
die zweite möglichkeit ist, dass ein kaskadierter pid regler deinen copter besser positioniert.
 

brm

Erfahrener Benutzer
#7
b.png
korrektur.png

es macht eben schon ein unterschied aus wie man rechnet.
einfach mal die korrigerten gps werte visualisieren.
die werte kommen aus einem gps filter.

im obere bild sind die rohdaten und das andere die geglätteten werte.
 
Zuletzt bearbeitet:

cesco1

Erfahrener Benutzer
#8
beim setzen der gps home position kann/muss man das einmal rechnen.
Der "longitude scaledown" wird bei sethome() auch gerechnet. Ich denke mehr ist nicht nötig.
Die ‘haversine’ berechnung macht im nahbereich nur mm aus. Oder?

Das problem bei der filterung ist der eingebrachte delay. Bei einem 1hz gps ist selbst 1 sample delay 1 sekunde. Gutes GPS, meine sind in nähe von häusern zickiger.

ein kaskadierter pid regler deinen copter besser positioniert.
Für mich zu kompliziert. Ziel ist zuverlässiges zurückkommen in sichtbereich. Das heisst +-10m ist voll OK. Ich möchte aber verstehen was systematische fehler sind und was sensorungenauigkeit / wind ausmacht.

Hast du schon erlebt dass du in 2km distanz heim wolltest und das fpv bild wird schwarz weil die sonne genau hinter "home" steht? Da verlier ich sofort die orientierung und genau da muss rth helfen.
 
Zuletzt bearbeitet:

brm

Erfahrener Benutzer
#9
Hast du schon erlebt dass du in 2km distanz heim wolltest und das fpv bild wird schwarz weil die sonne genau hinter "home" steht? Da verlier ich sofort die orientierung und genau da muss rth helfen.
haha, schon mal das runcam logo im blick gehabt?
bin deswegen 2 mal getaucht ...
dann habe ich da noch ein china cmos dingsbums ... das wird schwarz mit blick auf die sonne ...
also blind eine 180 grad drehung :)

wenn du nur einfachst filter verwendest, dann kann ich dich verstehen.
der obige filter hat eine korrektur - bei niedrigen geschwindigkeiten sehe ich keinen delay.

aber der 'alte' arduplane code ist soweit praxis erprobt.
 

MarenB

Runter kommen sie immer!
#10
Hast du schon erlebt dass du in 2km distanz heim wolltest und das fpv bild wird schwarz weil die sonne genau hinter "home" steht? Da verlier ich sofort die orientierung und genau da muss rth helfen.
Wäre da ein Wechsel auf eine Kamera mit brauchbarem WDR nicht einfacher? Sonne ist bei mir nur ein schmaler senkrechter weißer Streifen, rechts und links davon ist alles gut zu erkennen...
 

cesco1

Erfahrener Benutzer
#11
eine Kamera mit brauchbarem WDR nicht einfacher?
Eine kleine einführung wäre da nicht schlecht. Nicht für den flieger, der braucht rth auch um wegpunkte anzufliegen die ausserhalb der funkreichweite sind, aber mein $30 copter (eachine EX105) hat das problem auch. Bei tiefstehender sonne ist in einer richtung das bild schwarz. Was ist WDR ?
 

MarenB

Runter kommen sie immer!
#12
Eine kleine einführung wäre da nicht schlecht. Nicht für den flieger, der braucht rth auch um wegpunkte anzufliegen die ausserhalb der funkreichweite sind, aber mein $30 copter (eachine EX105) hat das problem auch. Bei tiefstehender sonne ist in einer richtung das bild schwarz. Was ist WDR ?
Wide-Dynamic-Range, das ermöglicht der Kamera größere Helligkeitsunterschiede darzustellen. Die gute alte PZ0420(m) hat schon D-WDR, also Dual-WDR, aber die einfachen kleinen CMOS-Cams haben das meist nicht.
Deine Lösung könnte in der neuen Runcam Micro-Swift liegen, das ist die derzeit kleinste und leichteste Cam mit dem Sony Super-HAD II Chip, den auch die PZ0420 verwendet. Vielleicht passt die ja auf den EX105?
 
Status
Nicht offen für weitere Antworten.
FPV1

Banggood

Oben Unten