Winkelgeschwindigkeit Integrieren

[Tomatenbaum]

Ja gut, werde ich machen mit den Fragen.

Ich bleib erst mal beim Integrieren und danach mit ACC verrechnen.
Bleibt nun die Frage nach dem dt und dem scale, also aufbereiten, so wie auf dem Paper.

Das verstehe ich noch nicht.

[Che Guevara]

Also bei deinem Kopter ist das Integral nicht sooo wichtig, ein Kopter fliegt auch mit einem reinen P-Anteil (zumindest im ACRO (HH) Mode).
Ich persönlich würde mich nicht nur stur an das Paper halten, sondern einfach selbst einwenig experimentieren. So erhältst du auch einen tieferen Einblick und weißt im Endeffekt dann mehr über das Thema, als wenn du 10 Papers liest

Gruß
Chris

[Tomatenbaum]

Ja Danke Chris

Ist der liebe Chris der einzige hier der sich mit Integrieren und Komplementärfilter auskennt?
Wo sind denn die ganzen Profis hier? :--)

[RP6conrad]

Bei meinen Balance Roboter wird jeden 4 ms der Gyro und die ACC gesampled. Danach wird den offset von Gyro abgesogen und dan integriert. Wichtig ist diesen festen Sampletime, oder du muss gans genau diesen Periode messen konnen zwischen zwei Messungen. Den offset verlauft mit temperatur, so das wird auch nog mal gemessen und korrigiert.
Ergebnis kannst du here sehen :

/*
De gyro wordt elke 4 mS uitgelezen via adc0. Dit wordt 10 keer geintegreerd (oversampling) . Bij stilstand geeft dit ca 6232.
Elke 40 mS wordt de hoek berekend. 
Ook wordt elke 400 mS de temperatuur van de gyro gemeten via ADC1. Hiermee wordt de driftcorrectie aangepast. 
In principe kan de drift kleiner zijn dan 0.1 °graad/sek
*/
void task_initial(void)
{ 
 static uint8_t i=0;
 static uint8_t j=0;
 
 
 int16_t offset_temp=6140;
 
 if(getStopwatch1() > 3) {    //elke 4 mS worden de gyro + ADXL345 uitgelezen + opgeteld
  setStopwatch1(0);     //sample rate is zeer belangrijk voor integraal gyro !!!
  adc0=adc0+readADC(ADC_0); //oversamplen gyro signaal
  adc5= adc5+readADC(ADC_5); //oversamplen acc X signaal
  adc6= adc6+readADC(ADC_6); //oversamplen acc Y signaal
  i++;
  
  if(i>9){       //na 40 ms worden de gemiddelde waarde gebruikt
   aXX=(adc5-4200);adc5=0; // 0 g = 4200 voor de X-as
   aYY=(adc6-4300);adc6=0; // 0 g = 4300 voor de Y-as
   hoek=atan2(aYY,-aXX)/M_PI*1800;//hieruit volgt dan de hoek van de versnellingsvector in 0,1°
   i=0;    
   gyro_d=(adc0-6323);adc0=0; //turnrate gyro over 10 samples-offset, 6323 bij stilstand als i>9
   gyro_hoek=gyro/35;//dit geeft dan de hoek weer in 0.1 graden
   gyro=gyro-gyro_d+0.25*(hoek-gyro_hoek); //sommatie = actuele hoek van gyro+ compensatie drift van ACC
   drift=drift+hoek-gyro_hoek;
   oldgyro=oldgyro-gyro_d;   //dummy variable om de drift te kunnen zien
   oldgyro_hoek=oldgyro/35;
   if(program>1) task_balance();else setMotorPower(0,0);
   j++;
   if (j>9){
    adc1=0.8*adc1+0.2*readADC(ADC_1); //uitlezen temp gyro voor driftcorrectie 
    offset_temp=(adc1-640); //temp compensatie gyrodrift, 640 is ca 20°C 
    gyro=gyro-offset_temp; //elke 400 ms wordt de temperatuur correctie uitgevoerd
    j=0;
    } 
  } 
 }
 
}

[Tomatenbaum]

Hallo RP6conrad

Vielen Dank für deine Antwort.
Ich muss mir deinen Code richtig anschauen, einiges verwirrt mich noch, könnte auch an der Sprache liegen. :--)

Das Video von deinem Balance Roboter ist toll, man sieht schön die Regelung.

Ich schaue mal ob ich deinen Code verstehe, brauche vermutlich etwas Zeit.

Gruss

[RP6conrad]

Lass mich das kurz erklaren :
Erst werden den Gyro, ACC_X_achse und ACC_Y_achse jeden 4 ms gemessen mit einen 10 bit AD (Mega32). Diesen Werten werden 10 mal addiert, so nach 40 ms habe ich eine Mittelwert von jeden Sensor. Dan wird aus diesen Werten die Drehrate von gyro berechnet(gyro_d=(adc0-6323);adc0=0; //turnrate gyro over 10 samples-offset, 6323 bei stillstand als i>9). Das ist nog alles in integer, so noch keine Umrechnung in °/sek.
Zweitens wird dan auch die Winkel berechnet von ACC : hoek=atan2(aYY,-aXX)/M_PI*1800;
Aus diesen Gyro-Drehrate wird dan die absolute Winkel berechnet rein durch integrieren, und gleich auch noch eine drift-ausgleich gemacht mit die Winkel von ACC gegeben : gyro=gyro-gyro_d+0.25*(hoek-gyro_hoek);


In diesen graph erkennen sie das integrierte Gyro-signal (blaue curve) das leicht wegdriftet, und die Winkel von ACC gemessen (rote curve). In meinen algoritmus wird diesen drift dan corrigiert.