**** DAS IST EIN BETATEST ******
So, jetzt gibts mal wieder was.
Also, diese Version ist NICHT "ausgiebig" getestet, aber geflogen und für richtig brauchbar befunden.
Die Änderungen betreffen nicht nur das Höheändern (Gasknüppelmitte) sondern auch den Altholdalgorithmus. Im Prinzip ist das, was im Gelaber hier:
http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=208149&viewfull=1#post208149 beschrieben wurde umgesetzt. Ich schreibe hier, in diesem Post gleich (später) noch eine genaue Anleitung/Erklärung per EDIT. Eure config.h könnt ihr weiter verwenden, die "ALT_HOLD_THROTTLE_NEUTRAL_ZONE" ist jetzt bei 80 - nicht erschrecken, klappt trotzdem butterweich. "AltHoldBlindTime" gibts nicht mehr, ist eigentlich auch klar. Eure Alt PIDs der final3 sollten, trotz der Änderungen, weiter gültig sein. EEPROMclear ist nur bei Versionswechsel (von DEV oder so) erforderlich (dies ist 2.1). GUI - ACC Calibration kann, wie immer, trotzdem nicht schaden. Unser Bigbretl - mod ist auch drin.
Wie gesagt, später mehr, ich wollte nur schon mal die Version zum Testen bei letztem Sonnenlicht zur Verfügung Stellen.
LG
Rob
EDIT:
Funktionsweise:
Sobald Althold eingeschaltet wird, wird die Höhe mit den eingestellten altPids gehalten, die Gasknüppelstellung wird für 1sec ignoriert, ab dann ist die Gasknüppelmitte (definiert durch "MIDRC", sollte man nicht ändern) der neue Referenzpunkt. Wird der Gasknüppel aus der definierten neutral Zone (ALT_HOLD_THROTTLE_NEUTRAL_ZONE steht auf 80) um die Mitte herum herausbewegt, erfolgt ein Steigen oder Sinken. Dabei werden die Werte immer auf die Neutralzone bezogen, d.h. z.B nach Verlassen der Neutralzone nach oben ergibt es z.B einen Wert von +1, d.h. egal wie Ihr die Neutralzone einstellt, es kann nach ihrem Verlassen keine sprunghaften Änderungen geben.
Beim Ausschalten des Altholdmodus wird ohne Übergang direkt wieder die herkömmliche Gassteuerung aktiv!
Starten im Altholdmodus:
Nur mit voreingestellten Alt PIDs ratsam, sonst kommt er nicht richtig, oder zu gut vom Boden weg!
Wenn man z.B mit den üblichen Stickbewegungen die Motoren scharf schaltet, steht das Gas natürlich ganz unten, d.h. es würde eine neue Sollhöhe weit unterhalb des Bodens bestimmt und wenn man dann starten wollte mit dem Gasstick über Mitte, würde erstmal nichts passieren, weil er dabei ist, die stark negative Sollhöhe wieder nach oben zu zählen. Damit das NICHT passiert wird bei der unteren Gasstick position (definiert durch MINTHROTTLE) nicht die Sollhöhe nach unten verschoben. Natürlich kann man das austricksen, indem man im Stand den Gasstick über die untere Position hinaus bewegt, dann wird tatsächlich die Sollhöhe nach unten gezählt (max -21474 KM !). Baro aus/anschalten resettet natürlich die Sollhöhe.
Landen im Altholdmodus:
Kein Problem, nur muss man aufpassen, dass nach man nach erfolgreicher Landung den Gasstick direkt ganz nach unten zieht. Damit zählt er zum einen die Sollhöhe nicht mehr herunter und zum anderen führt ein versehentliches Ausschalten der Barofunktion nicht zu einem ungewollten Starterlebnis (je nach Stickposition). Wenn man jetzt mit der gleichen Akkuladung weiterfliegen möchte, ist es sinnvoll den Baro kurz aus und wieder einzuschalten damit die Sollhöhe wieder zurückgesetzt wird.
Infos & Möglichkeiten & Probleme:
Im Hinblick auf die kommenden, offiziellen Versionen und dem gewünschten GPS Gebrauch, wird der Programmspeicher von promini und micro knapp - d.h. so viele Faxen können wir hier nicht mehr machen. Diese Version ist schon auf Diät gesetzt, aber ca. 1KB länger als die original 2.1.
Die Hoffnung, durch den vereinfachten PosHold Override (GPS) Code von Mahowik Bytes sparen zu können, hat sich zunächst zerschlagen, nach User Berichten ("flyrobot") funktioniert es (noch) nicht. Deswegen bleibt es bei Bigbretl.
Zu der 1-Sekunden Lösung: Sie ist bytetechnisch kurz und erfüllt ihren Zweck. Schöner (und machbar) wäre es, wenn man nach aktiviertem Althold warten würde, bis der Gasstick die mittlere Neutralzone erreicht hat, bevor sich Gasknüppeländerungen auswirken. Damit hätte man auch gleich die Scherzkekse mit erwischt, die durch etwas Gas im Stand den Erdkern als Sollhöhe setzten wollen. Allerdings wäre dieses dann immer noch während einer Zwischenlandung im Althold möglich. Dann müsste noch eine Zwischenlandungserkennung her, also z.B wenn der anliegende Motorgaswert MINTHROTTLE+20 erreicht wird, ist von einer Landung aus zu gehen. Die könnte natürlich bei einem schnellen Abstieg fehlinterpretiert werden, deswegen müsste man die Abfrage noch um das Variometer und eine Zeitkomponente erweitern also z.B: Wenn der Motorgaswert MINTHROTTLE+20 für 2Sek unterschritten wird und sich keine signifikannte Höhenänderung in dieser Zeit ergeben hat, dann ist von einer Landung aus zu gehen. Eine Begrenzung der negativen Sollhöhe auf z.B -10m ist m.E auch nicht sinnvoll. Zum einen schwank der Luftdruck, ok bei einer Flugzeit von 20Min wäre eine Schwankung von mehr als +- 1 bis 5m eigentlich nicht zu erwarten, zum anderen kann man auch von einer Anhöhe aus starten (Hochsitz etc) und zu "Tal" fliegen, wenn dann die -10m unterschritten werden sollen, ist es nicht möglich und der Pilot wundert sich, warum ausgerechnet sein Kopter vom Boden abgestossen wird. Naja, das sind so meine Gedanken zu den Problemen, wie seht Ihr das? Was ist sinnvoll, Stichwort: Foolproof? Was sollte noch rein?
Dann noch was: Beim Höheändern wird unsere Variometerbremse (klar, die stört dann) nicht ganz ausgeschaltet, sondern in ihrer Stärke halbiert. Ich bilde mir ein, dadurch ist es noch etwas weicher. Wer damit experimentieren will:
Original Zeile 979:
Code:
if (HightChange == 1) BaroI = BaroI>>1; // do Baro 1/2"I" on Hightchange
Ausschalten der Variometerbremse bei Höhenänderung wäre dann z.B:
Code:
if (HightChange == 1) BaroI = 0; // do Baro 1/2"I" on Hightchange
Oder nur 20% "I" würde z.B so aussehen
Code:
if (HightChange == 1) BaroI = (BaroI*20)/100; // do Baro 1/2"I" on Hightchange
Info Gasnachführung, Ziel: Keine weiteren Einstellungsparameter, kein zusätzlicher ThrottlePID kontroller (z.B Arducopter)
Intern wird das Gas nachgeführt, und da bin ich gespannt, ob das bei anderen Setups auch so funktioniert. Das läuft so: 10 mal pro Sekunde (10Hz sei die Minimalanforderung für Echtzeitanwendungen lt. DIY Drones) wird der Gaswert, der sich aus eurem "AltP" ergibt, ohne Fliesskommarest durch 100 geteilt und das Ergebnis dem "Sockelgas", das bei Einschalten des Baros definiert wurde hinzugefügt oder abgezogen. Z.B laut P Regelung ist ein Gaswert von 99 nötig zur Höhenkorrektur 99/100 = 0,99 ergibt dann für die Nachführung den Wert 0, Gaswerte von 100 - 199 dann eine Nachführung von 1.
So, das wars jetzt erstmal, die genauen Codeänderungen kommen noch.
Lg
Rob
Codeänderungen dieser Testversion gegenüber der "NewBaroPIDVario3Final"
Änderungen bei IMU
Weniger Variablen:
Code:
Original:
static uint8_t newbaroalt=0; //Crashpilotmod
static int32_t GroundAlt=0;
static int16_t BaroClimbRate = 0; // in cm/s
static uint32_t AltHoldBlindTimer=0,PosHoldBlindTimer=0;
static int16_t LastThrottle;
static uint32_t ThrTime;
static uint8_t HightChange=0,PSholdChange=0;
static int16_t ThrottleRate; // Ticks/sec
static float accVelScale;
Jetzt:
static int32_t GroundAlt=0;
static uint32_t AltRCTimer0,PosHoldBlindTimer=0;
static uint8_t newbaroalt=0,HightChange,PSholdChange=0;
static int16_t BaroP,BaroI,BaroD;
static float accVelScale;
Andere Initialisierung:
Code:
Original:
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
HightChange=0;
BaroPID=0;
AltHoldBlindTimer=0;
initialThrottleHold = rcCommand[THROTTLE];
}
} else {
f.BARO_MODE = 0;
}
#endif
Jetzt:
#if BARO
if (rcOptions[BOXBARO]) {
if (!f.BARO_MODE) {
f.BARO_MODE = 1;
AltHold = EstAlt;
AltRCTimer0 = currentTime+1000000; // The User has 1sec to center throttle after Althold is engaged
HightChange = 0;
initialThrottleHold = rcCommand[THROTTLE];
}
} else {
f.BARO_MODE = 0;
}
#endif
Andere Logik:
Code:
Original:
#if BARO
Baro_update(); // Do Baro every time!
getEstimatedAltitude();
if (currentTime >= ThrTime){ // Get ThrottleRate in Ticks/sec Upmovement=+ Polling at 10Hz
ThrTime = currentTime+100000;
ThrottleRate = (rcCommand[THROTTLE]-LastThrottle)*10;
if (abs(ThrottleRate) < 150) ThrottleRate = 0;
LastThrottle=rcCommand[THROTTLE];}
if (rcOptions[BOXBARO]){ // New Logic: When User exceeds neutral zone and stops fiddeling with throttle, then after 1 second a new Althold is defined
if (abs(rcCommand[THROTTLE] - initialThrottleHold)<ALT_HOLD_THROTTLE_NEUTRAL_ZONE && HightChange==0){
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE],MINTHROTTLE+1,MAXTHROTTLE-50);}
else {
HightChange=1;
if (ThrottleRate==0 && AltHoldBlindTimer==0) AltHoldBlindTimer = currentTime+AltHoldBlindTime;}
if (ThrottleRate !=0) AltHoldBlindTimer = 0;
if (currentTime >= AltHoldBlindTimer && AltHoldBlindTimer !=0) f.BARO_MODE = 0; // Set new ref hight
}
#endif
Jetzt:
#if BARO
Baro_update(); // Do Baro every time!
getEstimatedAltitude();
if (rcOptions[BOXBARO])
{
if (currentTime >= AltRCTimer0){ // 10Hz Loop
AltRCTimer0 = currentTime+100000;
int16_t thrstick = rcData[THROTTLE];
int16_t thrdiff = thrstick-MIDRC;
if (abs(thrdiff) > ALT_HOLD_THROTTLE_NEUTRAL_ZONE){
HightChange = 1;
initialThrottleHold = initialThrottleHold + BaroP/100; // Adjust Baselinethr by 1% of BaroP
if (thrdiff > 0) thrstick = thrdiff - ALT_HOLD_THROTTLE_NEUTRAL_ZONE;
if (thrdiff < 0){
thrstick = thrdiff + ALT_HOLD_THROTTLE_NEUTRAL_ZONE;
if (rcData[THROTTLE] <= MINTHROTTLE) thrstick = 0;}
}
else{
thrstick = 0;
HightChange = 0;}
AltHold = AltHold + (thrstick>>3); // BaroStickDevider = 5000/MaxBaroStickClimbrate(in cm/s)
} // End of 10 Hz Loop
if (HightChange == 1) BaroI = BaroI>>1; // do Baro 1/2"I" on Hightchange
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroP + BaroD - BaroI,MINTHROTTLE+1,MAXTHROTTLE-50);
}
#endif
Änderungen im Bereich IMU:
Code:
Original:
void getGroundAlt() // Executed in Setup takes approx 4.5 sec
{
uint8_t gacnt=0;
while(gacnt < 150){
while(newbaroalt ==0){ // Wait for new Baroval
Baro_update();
}
newbaroalt = 0; // Reset Boolean
GroundAlt=GroundAlt+BaroAlt;
gacnt++;
}
GroundAlt=GroundAlt/150;
accVelScale = 9.80665f/acc_1G/10000.0f;
}
///////////////////////////////////////////////
//Crashpilot1000 Mod getEstimatedAltitude ACC//
///////////////////////////////////////////////
#define VarioTabsize 8
#define BaroTabsize 5
void getEstimatedAltitude()
{
static uint8_t Vidx=0,Bidx=0;
static int32_t LastEstAlt=0;
static int16_t BaroP;
static int8_t VarioTab[VarioTabsize];
static int32_t BaroTab[BaroTabsize];
static float velz = 0.0f;
int8_t IdxCnt,ThrAngle;
int16_t AltI,ThrD,BaroDiff;
int32_t tmp32;
int16_t accZ = (accADC[ROLL]*EstG.V.X+accADC[PITCH]*EstG.V.Y+accADC[YAW]*EstG.V.Z)*InvSqrt(fsq(EstG.V.X)+fsq(EstG.V.Y)+fsq(EstG.V.Z)) - acc_1G; //if (abs(accZ)<acc_1G/25) accZ=0;
velz = velz+accZ*accVelScale*cycleTime;
ThrAngle = EstG.V.Z/acc_1G * 100.0f;
if (BaroPID==0) BaroP=0; // Reset BaroP if necessary, so I and D can be done before first Barovalue
if (newbaroalt!=0)
{
newbaroalt = 0; // Reset Boolean
// Get EstAlt
BaroTab[Bidx] = BaroAlt-GroundAlt; Bidx++;
if (Bidx==BaroTabsize) Bidx=0;
tmp32=0; IdxCnt = 0;
while(IdxCnt<BaroTabsize){tmp32=tmp32+BaroTab[IdxCnt]; IdxCnt++;}// EstAlt = (EstAlt+(tmp32/BaroTabsize))/2;
EstAlt = tmp32/BaroTabsize;
// Baro Climbrate
VarioTab[Vidx] = constrain(EstAlt-LastEstAlt,-126,126); Vidx++;
if (Vidx==VarioTabsize) Vidx=0;
LastEstAlt = EstAlt;
tmp32 = 0; IdxCnt = 0;
while(IdxCnt<VarioTabsize){tmp32=tmp32+VarioTab[IdxCnt]; IdxCnt++;}// BaroClimbRate = (BaroClimbRate+((tmp32*33)/VarioTabsize))/2;//BaroClimbRate in cm/sec // + is up // 30ms * 33 = 990ms
BaroClimbRate = (tmp32*33)/VarioTabsize; //BaroClimbRate in cm/sec // + is up // 30ms * 33 = 990ms
velz = velz * 0.982f + BaroClimbRate * 0.018f; //velz= Baro&Acc Climbrate
// Baro P
BaroDiff = constrain(AltHold-EstAlt,-200,200); // BaroDiff = Difference to locked hight neg if risen
BaroP = constrain(((int16_t)conf.P8[PIDALT]*BaroDiff)/200,-200,200); // Limit Baro P to +/- 200
}
if (ThrAngle < 40){BaroPID=0; return;} // Don't do BaroPID if copter too tilted// EstAlt & Baroclimbrate are always done :)
AltI = constrain((((int32_t)conf.I8[PIDALT]*velz)/50),-150,150); // Baro I
ThrD = (((int16_t)conf.D8[PIDALT]) * (100 - ThrAngle))/25; // "D"
BaroPID = (BaroPID+constrain(BaroP-AltI+ThrD,-250,250))/2;
}
Jetzt:
void getGroundAlt() // Executed in Setup takes approx 4.5 sec
{
uint8_t gacnt=0;
while(gacnt < 150){
while(newbaroalt ==0) Baro_update(); // Wait for new Baroval
newbaroalt = 0; // Reset Boolean
GroundAlt=GroundAlt+BaroAlt;
gacnt++;}
GroundAlt=GroundAlt/150;
accVelScale = 9.80665f/10000.0f/acc_1G;
}
///////////////////////////////////////////////
//Crashpilot1000 Mod getEstimatedAltitude ACC//
///////////////////////////////////////////////
#define VarioTabsize 8 // Increasing over 8 -> Risk of 16 Bit Overflow in Baro Climbrate
#define BaroTabsize 5
void getEstimatedAltitude()
{
static uint8_t Vidx=0,Bidx=0;
static int32_t LastEstAltBaro=0;
static int8_t VarioTab[VarioTabsize];
static int32_t BaroTab[BaroTabsize];
static float velz = 0.0f, accalt = 0.0f;
int8_t IdxCnt,ThrAngle;
int16_t accZ = (accADC[ROLL]*EstG.V.X+accADC[PITCH]*EstG.V.Y+accADC[YAW]*EstG.V.Z)*InvSqrt(fsq(EstG.V.X)+fsq(EstG.V.Y)+fsq(EstG.V.Z)) - acc_1G;
velz = velz+accZ*accVelScale*cycleTime;
accalt = accalt+velz*cycleTime*0.000001f;
ThrAngle = EstG.V.Z/acc_1G * 100.0f;
if (newbaroalt!=0)
{
newbaroalt = 0; // Reset Boolean
BaroTab[Bidx] = BaroAlt-GroundAlt; Bidx++; // Get EstAltBaro
if (Bidx==BaroTabsize) Bidx=0;
int32_t tmp32=0; IdxCnt = 0;
while(IdxCnt<BaroTabsize){tmp32 = tmp32 + BaroTab[IdxCnt]; IdxCnt++;}
int32_t EstAltBaro = tmp32/BaroTabsize;
VarioTab[Vidx] = constrain(EstAltBaro - LastEstAltBaro,-120,120); Vidx++; // Baro Climbrate
if (Vidx==VarioTabsize) Vidx=0;
LastEstAltBaro = EstAltBaro;
int16_t tmp16 = 0; IdxCnt = 0;
while(IdxCnt<VarioTabsize){tmp16 = tmp16 + VarioTab[IdxCnt]; IdxCnt++;}
int16_t BaroClimbRate = (tmp16*33)/VarioTabsize; // BaroClimbRate in cm/sec // + is up // 30ms * 33 = 990ms
velz = velz * 0.982f + BaroClimbRate * 0.018f; // velz= Baro&Acc Climbrate
accalt = accalt * 0.85f + EstAltBaro * 0.15f;
}
int16_t ClimbRate = velz/50; // put float into 16Bit
EstAlt = accalt;
BaroP = 0,BaroI = 0,BaroD = 0;
if (ThrAngle < 40) return; // Don't do BaroPID if copter too tilted// EstAlt & Baroclimbrate are always done :)
BaroP = ((AltHold-EstAlt)*conf.P8[PIDALT])/200;
BaroI = constrain(ClimbRate*conf.I8[PIDALT],-150,150);
BaroD = (((int16_t)conf.D8[PIDALT]) * (100 - ThrAngle))/25;
}
