Baro Code Änderungen

Roberto · Aug 24, 2012

Baro Code Änderungen (Seite 1 Übersicht)

Aktuelles:

-Hier gibt es die "Final4" Version und die aktuelle Testversion:
http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=224135&viewfull=1#post224135

- Hier ist die "Final4" Version mit Autolandung:
http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=266928&viewfull=1#post266928

-Hier gibt es die "Final4" Version, verheiratet mit der MultiWii_2_1_new_serial1 von Ronco (für Bluetooth) von merlin4: http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=224710&viewfull=1#post224710

-Hier ist die aktuelle 32Bit - Naze32 Variante:
http://fpv-community.de/showthread....ative-Software&p=239813&viewfull=1#post239813
=======================================================================

Älter Version
-Hier gibt es die "Final3" Version:http://fpv-community.de/showthread....o-vern%FCnftig&p=200133&viewfull=1#post200133

-Hier gibt es die "Final3" Version, verheiratet mit der MultiWii_2_1_new_serial1 von Ronco (für Bluetooth) von merlin4: http://fpv-community.de/showthread....o-vern%FCnftig&p=203335&viewfull=1#post203335

=======================================================================

- Wer Ärger mit der Programmgrösse auf ATmega32u4 (Pro Micro, z.B "Microwii") basierten FC hat, sollte sich den Tip von JinGej anschauen: http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=210431&viewfull=1#post210431
Aber Vorsicht, das bringt auch Risiken mit sich: http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=210486&viewfull=1#post210486

Codeänderungen MultiWii_2_1_NewBaroPIDVario3Final:
========================================

Änderungen im Hauptprogramm (Changes in mainprogram):
=====================================================

Zusätzliche Variablen / Additional variables:
---------------------------------------------

Code:

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;

Gelöschte Variable / Deleted variable:
--------------------------------------

Code:

static int16_t  errorAltitudeI = 0;

Eingefügt / Added line:
-----------------------
In setup() nach "initSensors();" einfügen:
Insert this in setup() after "initSensors();"

Code:

  #if BARO
  getGroundAlt();
  #endif

Bei Zeile 795 das löschen / Delete this at line 795:
----------------------------------------------------

Code:

    #if BARO
      if (rcOptions[BOXBARO]) {
        if (!f.BARO_MODE) {
          f.BARO_MODE = 1;
          AltHold = EstAlt;
          initialThrottleHold = rcCommand[THROTTLE];
          errorAltitudeI = 0;
          BaroPID=0;
        }
      } else {
        f.BARO_MODE = 0;
      }
    #endif

Und durch das ersetzen / Replace with this:

Code:

    #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

Bei Zeile 849 das löschen / Delete this at line 849:
----------------------------------------------------

Code:

        if (rcOptions[BOXGPSHOLD]) {
          if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {
            f.GPS_HOLD_MODE = 1;
            GPSNavReset = 0;
            GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
          }
        } else {
          f.GPS_HOLD_MODE = 0;
        }

Und durch das ersetzen / Replace with this:

Code:

        if (rcOptions[BOXGPSHOLD] && PSholdChange == 0)
        {
          if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {
            f.GPS_HOLD_MODE = 1;
            GPSNavReset = 0;
            GPS_I2C_command(I2C_GPS_COMMAND_POSHOLD,0);
          }
        } else {
          f.GPS_HOLD_MODE = 0;
        }

Bei Zeile 871 das löschen / Delete this at line 871:
----------------------------------------------------

Code:

       if (rcOptions[BOXGPSHOLD]) {
          if (!f.GPS_HOLD_MODE) {
            f.GPS_HOLD_MODE = 1;
            GPS_hold[LAT] = GPS_coord[LAT];
            GPS_hold[LON] = GPS_coord[LON];
            GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
            nav_mode = NAV_MODE_POSHOLD;
          }
        } else {
          f.GPS_HOLD_MODE = 0;
        }

Und durch das ersetzen / Replace with this:

Code:

        if (rcOptions[BOXGPSHOLD] && PSholdChange == 0)
         {
          if (!f.GPS_HOLD_MODE) {
            f.GPS_HOLD_MODE = 1;
            GPS_hold[LAT] = GPS_coord[LAT];
            GPS_hold[LON] = GPS_coord[LON];
            GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON]);
            nav_mode = NAV_MODE_POSHOLD;
          }
        } else {
          f.GPS_HOLD_MODE = 0;
        }

Das einfügen vor / Insert this before "if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
-----------------------------------------------------------------------------------------

Code:

#if GPS
 #if defined(PositionHoldOverride)
  if (rcOptions[BOXGPSHOLD]){
    if (abs(rcData[PITCH]-MIDRC) > BlindZonePitch || abs(rcData[ROLL]-MIDRC) > BlindZoneRoll){
      PosHoldBlindTimer = 0;
      PSholdChange = 1;}
    else {if (PSholdChange==1 && PosHoldBlindTimer==0) PosHoldBlindTimer = currentTime+PosHoldBlindTime;}             // Ph is off and Stick back to neutralzone
    if (currentTime >= PosHoldBlindTimer && PosHoldBlindTimer !=0){
      PosHoldBlindTimer = 0;
      PSholdChange = 0;}
   }
  else{PosHoldBlindTimer = 0; PSholdChange = 0;}
 #endif
#endif

Bei Zeile 892 das löschen / Delete this at line 892:
----------------------------------------------------

Code:

 } else { // not in rc loop
    static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
    switch (taskOrder++ % 5) {
      case 0:
        #if MAG
          Mag_getADC();
        #endif
        break;
      case 1:
        #if BARO
          Baro_update();
        #endif
        break;
      case 2:
        #if BARO
          getEstimatedAltitude();
        #endif
        break;
      case 3:
        #if GPS
          if(GPS_Enable) GPS_NewData();
        #endif
        break;
      case 4:
        #if SONAR
          Sonar_update();debug[2] = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        break;
    }
  }

Und durch das ersetzen / Replace with this:

Code:

  } else { // not in rc loop
    static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
    switch (taskOrder++ % 3) {
      case 0:
        #if MAG
          Mag_getADC();
        #endif
        break;
      case 1:
        #if GPS
          if(GPS_Enable) GPS_NewData();
        #endif
        break;
      case 2:
        #if SONAR
          Sonar_update();debug[2] = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        break;
    }
  }

Bei Zeile 941 das löschen / Delete this at line 941:
----------------------------------------------------

Code:

  #if BARO
    if (f.BARO_MODE) {
      if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
        f.BARO_MODE = 0; // so that a new althold reference is defined
      }
      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
    }
  #endif

Und durch das ersetzen / Replace with this:

Code:

  #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

Änderungen unter "IMU" (Changes in "IMU"):
==========================================

In Zeile 172 das hier/ Change this in line 172:
-----------------------------------------------

Code:

void getEstimatedAttitude(){
  uint8_t axis;
  int32_t accMag = 0;
  static t_fp_vector EstG;
#if MAG
...

Ändern in / to this:

Code:

static t_fp_vector EstG;
void getEstimatedAttitude(){
  uint8_t axis;
  int32_t accMag = 0;
#if MAG

Bei Zeile 254 das löschen / Delete this at line 254:
----------------------------------------------------

Code:

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY      4000000  // 4 sec initialization delay
#define BARO_TAB_SIZE   40

void getEstimatedAltitude(){
  uint8_t index;
  static uint32_t deadLine = INIT_DELAY;

  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int32_t BaroHigh,BaroLow;
  int32_t temp32;
  int16_t last;

  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  deadLine = currentTime; 

  //**** Alt. Set Point stabilization PID ****
  //calculate speed for D calculation
  last = BaroHistTab[BaroHistIdx];
  BaroHistTab[BaroHistIdx] = BaroAlt/10;
  BaroHigh += BaroHistTab[BaroHistIdx];
  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
  BaroHigh -= BaroHistTab[index];
  BaroLow  += BaroHistTab[index];
  BaroLow  -= last;

  BaroHistIdx++;
  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;

  BaroPID = 0;
  //D
  temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
  BaroPID-=temp32;

  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
  
  temp32 = AltHold - EstAlt;
  if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small D parametr to reduce noise near zero position
  
  //P
  BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;   
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

  //I
  errorAltitudeI += temp32*conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
}

Und durch das ersetzen / Replace with this:

Code:

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;
}
float InvSqrt (float x){ 
  union{  
    int32_t i;  
    float   f; 
  } conv; 
  conv.f = x; 
  conv.i = 0x5f3759df - (conv.i >> 1); 
  return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
}
float fsq(float x){return x * x;} // no benefit from integer! Code gets longer

Das waren die Hauptneuerungen. Die Änderungen bei "Sensors" sind die gleichen wie in diesem Link beschrieben / The changes in "Sensors" have been already shown here: http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=189823&viewfull=1#post189823 Both Baro readoutroutines (BMP&MS) are tuned to put out new values every 30ms! (Beide Baro Ausleseprogramme geben jetzt alle 30ms einen neuen Wert aus!)

Hier die Änderung in def.h / Changes in def.h:
==============================================

Hinzugefügt /Added:
-------------------

Code:

/**************************************************************************************/
/***************               CRASHPILOT1000 MOD                  ********************/
/**************************************************************************************/

#if !defined(AltHoldBlindTime)
  #define AltHoldBlindTime 800000
#endif 

#if defined(PositionHoldOverride)
 #if !defined(PosHoldBlindTime)
   #define PosHoldBlindTime 300000
 #endif 
 #if !defined(BlindZonePitch)
   #define BlindZonePitch 30
 #endif 
 #if !defined(BlindZoneRoll)
   #define BlindZoneRoll 30
 #endif 
#endif

Hier die Änderung in config.h / Changes in config.h:
====================================================

Entsprechend geändert/hinzugefügt // Accordingly changed/added:

Code:

    /* ALT_HOLD_THROTTLE_NEUTRAL_ZONE: Defines the neutral zone of throttle stick during altitude hold, default setting is
       +/-40 uncommend and change the value below if you want to change it. 
       AltHoldBlindTime defines the time in micro seconds (def 0.8s) after wich a new Altitude for AltHold is defined
       when throttlestickmovement stops. You see here the default values. Uncomment and change if you want.*/
    //#define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 40 
    //#define AltHoldBlindTime 800000


  /**************************************************************************************/
  /***********************                  GPS                **************************/
  /**************************************************************************************/

    /* PositionHoldOverride Info: http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=189821&viewfull=1#post189821
     Purpose: You can fly in position hold and override it with nick(pitch) and roll movements (defined in Blindzones below). When you return
     those sticks to center a new position for pos.Hold is defined after a timeinterval defined in PosHoldBlindTime.*/

    //#define PositionHoldOverride                          // Uncomment to activate
  
    /*Parameters for Position hold Override You see here the default values. Uncomment and change if you want */
    //#define PosHoldBlindTime 300000                       // Time in microseconds (def. 0.3 sec) before PH is re-engaged after returning to Stickcenter
    //#define BlindZonePitch 30                             // Defines the Blindzone around Pitchaxiscenter
    //#define BlindZoneRoll 30                              // Defines the Blindzone around Rollaxiscenter

In EEPROM.ino wurden nur die AltPID Ausgangswerte geändert. / In EEPROM.ino only the deflaultvalues for ALTPID were changed.

Das wars ! / That was it!

So long

Rob

Roberto · Aug 24, 2012

Position hold Override oder Bigbretl - Mod

Bigbretl hatte hier auf ein Problem/Verbesserungsmöglichkeit bzgl. der Benutzung des GPS Position Hold hingeweisen http://fpv-community.de/showthread....o-vern%FCnftig&p=189218&viewfull=1#post189218, und auch gleich einen guten Lösungsvorschlag parat. Die vorgeschlagene Programmänderung führte zum gewünschten Ergebnis: http://fpv-community.de/showthread....o-vern%FCnftig&p=191585&viewfull=1#post191585.
Die Änderung ist unabhängig von dem Baromod und kann auch mit der original 2.1 durchgeführt werden. Eigentlich ist das Offtopic hier. Alle Änderungen wurden im Hauptprogramm durchgeführt:

Code:

Am Anfang des Codes vor den anderen "static" diese neuen Konstanten (und eine Bytevariable) einfügen:

/////// Parameters for Position hold Override
  #define PosHoldBlindTime 1000000                      // Time in microseconds before PH is re-engaged after returning to Stickcenter
  #define BlindZonePitch 50                             // Defines the Blindzone around Pitchaxiscenter
  #define BlindZoneRoll 50                              // Defines the Blindzone around Rollaxiscenter
/////// Parameters for Position hold Override
static uint8_t  PSholdChange=0;

Dann diese Stelle suchen (ca. Zeile 850/870)

Code:

      if (rcOptions[BOXGPSHOLD]) {
          if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {

Und ersetzen durch:

Code:

   if (rcOptions[BOXGPSHOLD] && PSholdChange == 0) {
          if (!f.GPS_HOLD_MODE & !f.GPS_HOME_MODE) {

Analog dazu, diese Stelle, kurz danach finden:

Code:

      if (rcOptions[BOXGPSHOLD]) {
          if (!f.GPS_HOLD_MODE) {

Und ersetzen durch:

Code:

 if (rcOptions[BOXGPSHOLD] && PSholdChange == 0){
          if (!f.GPS_HOLD_MODE) {

VOR dieser Stelle:

Code:

  if (rcOptions[BOXPASSTHRU]) {f.PASSTHRU_MODE = 1;}
    else {f.PASSTHRU_MODE = 0;}

Diesen Programcode einfügen:

Code:

#if GPS
  if (rcOptions[BOXGPSHOLD])
   {
    if (abs(rcData[PITCH]-MIDRC) > BlindZonePitch || abs(rcData[ROLL]-MIDRC) > BlindZoneRoll){
      PosHoldBlindTimer = 0;
      PSholdChange = 1;}
    else {if (PSholdChange==1 && PosHoldBlindTimer==0) PosHoldBlindTimer = currentTime+PosHoldBlindTime;}             // Ph is of and Stick back to neutralzone
    if (currentTime >= PosHoldBlindTimer && PosHoldBlindTimer !=0){
      PosHoldBlindTimer = 0;
      PSholdChange = 0;}
   }
  else{
    PosHoldBlindTimer = 0;
    PSholdChange = 0;}
#endif

Das müsste es gewesen sein.
Mit "#define PosHoldBlindTime 1000000 // Time in microseconds before PH is re-engaged after returning to Stickcenter"
stellt man die Zeit (1Mio Mikrosekunden=1Sek) ein, nach der das GPS Pos Hold wieder aktiv wird, wenn der Knüppel wieder zentriert ist.
Mit den Parametern:
#define BlindZonePitch 50 // Defines the Blindzone around Pitchaxiscenter
#define BlindZoneRoll 50 // Defines the Blindzone around Rollaxiscenter
Stellt man den Blindbereich um die Knüppelmitte für Pitch (Nick) und Roll (Aileron) getrennt ein.
Die Werte sind wahscheinlich für den Praxisbetrieb etwas zu gross. Da bräuchte ich Feedback, was da passt.
EDIT: Bigbretl hat hier bessere Parameter gefunden http://fpv-community.de/showthread....o-vern%FCnftig&p=191728&viewfull=1#post191728. Diese sind hier noch nicht eingearbeitet.

Wer nicht basteln will bekommt hier im Anhang die geänderte Komplettversion.

LG
Rob

Roberto · Aug 24, 2012

Codeänderungen MultiWii_2_1_NewBaroPIDVario2b:
======================================
Für Interessierte habe ich hier die Codeänderungen zusammengefasst.
Die komplette Version gibt es hier als Download: http://fpv-community.de/showthread....o-vern%FCnftig&p=188839&viewfull=1#post188839 .

Alle Dinge bzgl. den Flugeigenschaften, PIDs usw. bitte in den Hauptthread http://fpv-community.de/showthread.php?10765-Funktionieren-bei-Euch-Kompass-und-Baro-vern%FCnftig.

EDIT:
Wer Ärger mit dem geänderten "D" und seiner MPU hat (EDIT: betrifft eigentlich nur Freeimu 0.4.3 Besitzer), kann diese Änderung übergangsweise vornehmen:
http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=191169&viewfull=1#post191169

Wem die "I" Regelung zu ruppig vom Motorgeräusch her ist und einen MS Baro hat:
http://fpv-community.de/showthread....o-vern%FCnftig&p=191631&viewfull=1#post191631

Wer Probleme mit dem Anpassen des aktuellen "Truncs" (http://code.google.com/p/multiwii/source/browse/#svn/trunk/MultiWii_shared) hat, bitte hier schauen: http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=190853&viewfull=1#post190853.

Hier bitte nur programmtechnisch relevante Dinge schreiben (Fehler/Verbesserungen/Vorschläge usw.) !!!

MultiWii_2_1_NewBaroPIDVario2b VS original MultiWii_2_1

Änderungen im Hauptprogramm:
============================

Code:

Eingefügt:
static uint8_t newbaroalt=0;           //Crashpilotmod
static int32_t GroundAlt=0;
static int16_t BaroClimbRate = 0;      // in cm/s
static uint32_t AltHoldBlindTimer = 0;
static int16_t LastThrottle;
static uint32_t ThrTime;
static uint8_t HightChange=0;
static int16_t ThrottleRate;           // Ticks/sec

Gelöscht:
static int16_t  errorAltitudeI = 0;

Code:

Bei "void setup()" nach "initSensors();" eingefügt:
  #if BARO
  getGroundAlt();
  #endif

Code:

In der Hauptschleife diesen Code:
    #if BARO
      if (rcOptions[BOXBARO]) {
        if (!f.BARO_MODE) {
          f.BARO_MODE = 1;
          AltHold = EstAlt;
          initialThrottleHold = rcCommand[THROTTLE];
          errorAltitudeI = 0;
          BaroPID=0;
        }
      } else {
        f.BARO_MODE = 0;
      }
    #endif

Ersetzt durch diesen:
    #if BARO
      if (rcOptions[BOXBARO]) {
        if (!f.BARO_MODE) {
          f.BARO_MODE = 1;
          AltHold = EstAlt;
          HightChange=0;
          AltHoldBlindTimer=0;
          initialThrottleHold = rcCommand[THROTTLE];
        }
      } else {
        f.BARO_MODE = 0;
      }
    #endif

Code:

In der Hauptschleife diesen Code:
  } else { // not in rc loop
    static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
    switch (taskOrder++ % 5) {
      case 0:
        #if MAG
          Mag_getADC();
        #endif
        break;
      case 1:
        #if BARO
          Baro_update();
        #endif
        break;
      case 2:
        #if BARO
          getEstimatedAltitude();
        #endif
        break;
      case 3:
        #if GPS
          if(GPS_Enable) GPS_NewData();
        #endif
        break;
      case 4:
        #if SONAR
          Sonar_update();debug[2] = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        break;
    }
  }

Ersetzt durch diesen:
  } else { // not in rc loop
    static uint8_t taskOrder=0; // never call all functions in the same loop, to avoid high delay spikes
    switch (taskOrder++ % 3) {
      case 0:
        #if MAG
          Mag_getADC();
        #endif
        break;
      case 1:
        #if GPS
          if(GPS_Enable) GPS_NewData();
        #endif
        break;
      case 2:
        #if SONAR
          Sonar_update();debug[2] = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        break;
    }
  }

Code:

Kurz danach in der Hauptschleife diesen Code:
  #if BARO
    if (f.BARO_MODE) {
      if (abs(rcCommand[THROTTLE]-initialThrottleHold)>ALT_HOLD_THROTTLE_NEUTRAL_ZONE) {
        f.BARO_MODE = 0; // so that a new althold reference is defined
      }
      rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
    }
  #endif
  
Ersetzt durch diesen:
  #if BARO
    #define AltHoldBlindTime 1000000

    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

Änderungen bei "IMU":
=====================

Code:

Neu, eingefügt vor "void getEstimatedAltitude()", ist aber egal wo:
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;
}

Code:

Die void getEstimatedAltitude() gelöscht (kein Bildschirmfoto, zu gross):
#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY      4000000  // 4 sec initialization delay
#define BARO_TAB_SIZE   40
void getEstimatedAltitude(){
  uint8_t index;
  static uint32_t deadLine = INIT_DELAY;
  static int16_t BaroHistTab[BARO_TAB_SIZE];
  static int8_t BaroHistIdx;
  static int32_t BaroHigh,BaroLow;
  int32_t temp32;
  int16_t last;

  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  deadLine = currentTime; 
  //**** Alt. Set Point stabilization PID ****
  //calculate speed for D calculation
  last = BaroHistTab[BaroHistIdx];
  BaroHistTab[BaroHistIdx] = BaroAlt/10;
  BaroHigh += BaroHistTab[BaroHistIdx];
  index = (BaroHistIdx + (BARO_TAB_SIZE/2))%BARO_TAB_SIZE;
  BaroHigh -= BaroHistTab[index];
  BaroLow  += BaroHistTab[index];
  BaroLow  -= last;

  BaroHistIdx++;
  if (BaroHistIdx == BARO_TAB_SIZE) BaroHistIdx = 0;

  BaroPID = 0;
  //D
  temp32 = conf.D8[PIDALT]*(BaroHigh - BaroLow) / 40;
  BaroPID-=temp32;
  EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
  
  temp32 = AltHold - EstAlt;
  if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small D parametr to reduce noise near zero position
  
  //P
  BaroPID += conf.P8[PIDALT]*constrain(temp32,(-2)*conf.P8[PIDALT],2*conf.P8[PIDALT])/100;   
  BaroPID = constrain(BaroPID,-150,+150); //sum of P and D should be in range 150

  //I
  errorAltitudeI += temp32*conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  temp32 = errorAltitudeI / 500; //I in range +/-60
  BaroPID+=temp32;
}

Ersetzt durch:
///////////////////////////////////////////////
//Crashpilot1000 Mod getEstimatedAltitude ACC//
///////////////////////////////////////////////
#define Droppercent 20
#define VarioTabsize 7                                         // Stores 7 Values. New Value every 30ms so it oversees 210ms

void getEstimatedAltitude()
{
  static uint8_t Vidx=0;
  static int32_t LastEstAlt=0;
  static int8_t VarioTab[VarioTabsize];
  static int16_t AccZLowest,AccZHighest;                        // ACCZ extremes
  int8_t VarioIdxCnt;
  int16_t ACCZD,BaroP,AltI,BaroDiff,BaroClimbRateTmp;

  ACCZD = accADC[YAW]-acc_1G;                                   // So UPacc is +, DWNacc is -
  if (ACCZD > AccZHighest) AccZHighest=ACCZD;                   // Set new ACC Extremes
  if (ACCZD < AccZLowest) AccZLowest=ACCZD;                     // Every run till next Baro-values are available
  
  if (newbaroalt==0) return;                                    // Update only, if new Baro-values are available
  newbaroalt = 0;                                               // Reset Boolean

  BaroAlt = BaroAlt-GroundAlt;                                  // Set to Zero on Ini hight
  BaroAlt = EstAlt+constrain(BaroAlt-EstAlt,-15,15);
  EstAlt = (EstAlt+EstAlt+BaroAlt)/3;

  // Baro Climbrate
  VarioTab[Vidx] = constrain(EstAlt-LastEstAlt,-126,126);
  LastEstAlt = EstAlt;
  Vidx++;
  if (Vidx==VarioTabsize) Vidx=0; 
  BaroClimbRateTmp = 0;
  VarioIdxCnt = 0;
  while(VarioIdxCnt<VarioTabsize){
  BaroClimbRateTmp = BaroClimbRateTmp+VarioTab[VarioIdxCnt];
  VarioIdxCnt++;}
  BaroClimbRate = ((int32_t)BaroClimbRateTmp*33)/VarioTabsize;  //BaroClimbRate in cm/sec // + is up // 30ms * 33 = 990ms

  // 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

  // Baro I
  AltI = constrain((((int32_t)conf.I8[PIDALT]*BaroClimbRate)/100),-100,100);

  // ACCZ "D"
  ACCZD = 0;
  if (rcOptions[BOXACC]){                                       // Only do ACCZ in Level Mode
  ACCZD = (((int32_t)conf.D8[PIDALT]*(constrain (abs(AccZHighest)+abs(AccZLowest),-15,+15))*(AccZHighest-abs(AccZLowest)))*63)/((int32_t)acc_1G*acc_1G); // conf.D8[PIDALT] Is scalefactor for ACCZ
  ACCZD = constrain(ACCZD,-150,0);}                             // Limit ACC Z to only Upcorrection
  AccZHighest=0;                                                // Reset Values 
  AccZLowest=0;

  BaroPID = constrain(BaroP-AltI-ACCZD,-250,250);
  if (BaroPID < 0) BaroPID=(BaroPID*Droppercent)/100;           // Lower Dropping Values by a Factor Droppercent
}

Änderungen bei "Sensors":
=====================

Code:

Original:
void  Baro_init() {
  delay(10);
  i2c_BMP085_readCalibration();
  i2c_BMP085_UT_Start(); 
  delay(5);
  i2c_BMP085_UT_Read();
}

Ersetzt durch:
void  Baro_init() {
  delay(10);
  i2c_BMP085_readCalibration();
  i2c_BMP085_UT_Start(); 
  delay(5);
  i2c_BMP085_UT_Read();
  bmp085_ctx.state=0;
  bmp085_ctx.deadline=0;
}
Anmerk: Nur zur Sicherheit noch auf 0 gesetzt, Änderung wahrsch. unnötig

Code:

Die void Baro_update() des BMP wurde geändert.
Original:
void Baro_update() {
  if (currentTime < bmp085_ctx.deadline) return; 
  bmp085_ctx.deadline = currentTime;
  TWBR = ((F_CPU / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, BMP085 is ok with this speed
  switch (bmp085_ctx.state) {
    case 0: 
      i2c_BMP085_UT_Start(); 
      bmp085_ctx.state++; bmp085_ctx.deadline += 4600; 
      break;
    case 1: 
      i2c_BMP085_UT_Read(); 
      bmp085_ctx.state++; 
      break;
    case 2: 
      i2c_BMP085_UP_Start(); 
      bmp085_ctx.state++; bmp085_ctx.deadline += 14000; 
      break;
    case 3: 
      i2c_BMP085_UP_Read(); 
      i2c_BMP085_Calculate(); 
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
      bmp085_ctx.state = 0; bmp085_ctx.deadline += 5000; 
      break;
  } 
}
#endif

Ersetzt durch:

void Baro_update()
{
 if (micros() < bmp085_ctx.deadline) return; 
 TWBR = ((16000000L / 400000L) - 16) / 2;                                // change the I2C clock rate to 400kHz, BMP085 is ok with this speed
 switch (bmp085_ctx.state)
 {
  case 0: 
   i2c_BMP085_UT_Start(); 
   bmp085_ctx.deadline=micros()+4500-1; 
   bmp085_ctx.state++;
   break;
  case 1: 
   i2c_BMP085_UT_Read(); 
   i2c_BMP085_UP_Start(); 
   bmp085_ctx.deadline=micros()+13500-1; 
   bmp085_ctx.state++;
   break;
  case 2: 
   i2c_BMP085_UP_Read(); 
   i2c_BMP085_Calculate(); 
   BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
   newbaroalt=1;
   bmp085_ctx.state=0;
   break;
 } 
}
#endif

Code:

Jetzt die Änderungen bei dem MS Baro. Zunächst, analog zum BMP die Baro_init()
Original:
void  Baro_init() {
  delay(10);
  i2c_MS561101BA_reset();
  delay(100);
  i2c_MS561101BA_readCalibration();
}

Ersetzt durch:
void  Baro_init() {
  delay(10);
  i2c_MS561101BA_reset();
  delay(100);
  i2c_MS561101BA_readCalibration();
  ms561101ba_ctx.deadline=0;
  ms561101ba_ctx.state=0;
}

Code:

void Baro_update() für MS Baro, Original:
void Baro_update() {
  if (currentTime < ms561101ba_ctx.deadline) return; 
  ms561101ba_ctx.deadline = currentTime;
  TWBR = ((F_CPU / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, MS5611 is ok with this speed
  switch (ms561101ba_ctx.state) {
    case 0: 
      i2c_MS561101BA_UT_Start(); 
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 10000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 1: 
      i2c_MS561101BA_UT_Read(); 
      ms561101ba_ctx.state++;
      break;
    case 2: 
      i2c_MS561101BA_UP_Start(); 
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 10000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 3: 
      i2c_MS561101BA_UP_Read();
      i2c_MS561101BA_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
      ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 4000;
      break;
  } 
}
#endif

Ersetzt durch:
void Baro_update() {
  if (micros() < ms561101ba_ctx.deadline) return; 
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, MS5611 is ok with this speed
  switch (ms561101ba_ctx.state) {
    case 0: 
      i2c_MS561101BA_UT_Start(); 
      ms561101ba_ctx.deadline = micros()+9000-1; //according to the specs, the pause should be at least 8.22ms
      ms561101ba_ctx.state++;
      break;
    case 1: 
      i2c_MS561101BA_UT_Read(); 
      ms561101ba_ctx.state++;
      break;
    case 2: 
      i2c_MS561101BA_UP_Start(); 
      ms561101ba_ctx.deadline = micros()+9000-1; //according to the specs, the pause should be at least 8.22ms
      ms561101ba_ctx.state++;
      break;
    case 3: 
      i2c_MS561101BA_UP_Read();
      i2c_MS561101BA_Calculate();
      ms561101ba_ctx.deadline = micros()+3000-1;
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
      newbaroalt=1;
      ms561101ba_ctx.state = 0;
      break;
  } 
}
#endif

So, dass müssten die wesentlichen Änderungen gewesen sein.
Jetzt kommen die unwesentlichen Änderungen ohne Fotos:

Änderungen bei "EEPROM":
========================

Code:

 Andere Defaultwerte für Pidalt.
Original:
 conf.P8[PIDALT]   = 16; conf.I8[PIDALT]   = 15; conf.D8[PIDALT]   = 7;

Änderung:
conf.P8[PIDALT]   = 100; conf.I8[PIDALT]   = 40; conf.D8[PIDALT]   = 0;

Änderungen bei "def.h":
=======================

Code:

Die Neutralzone wurde vergrössert, wenn sie nicht in der config.h definiert wurde.
Original:
#if !defined(ALT_HOLD_THROTTLE_NEUTRAL_ZONE)
  #define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 20
#endif 

Änderung:
#if !defined(ALT_HOLD_THROTTLE_NEUTRAL_ZONE)
  #define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 40
#endif

Änderungen bei "RX":
====================

Die von Alexmos vorgeschlagene Änderung ist eingearbeitet (http://code.google.com/p/multiwii-alexmos/source/detail?r=36).
"- RX reading tiny bug corrected: get time before sei() (otherwise RX signal jitter may appear)"

So, dass müsste es gewesen sein!

EDIT: Da die Bildchen hier alle als 800*X jpg verunglückt sind, habe ich sie als zip Datei nachgeliefert. Das Datenaufkommen ist dadurch sogar reduziert.

LG

Rob

Roberto · Aug 24, 2012

012345678901

Grandcaravan · Aug 24, 2012

Tolle Zusammenfassung, dafür gibts eine positive Bewertung!
Kannst du das evtl gleich im Wiki verewigen? Das wäre eine super Bereicherung

Danke und beste Grüße
Heiko

Roberto · Aug 24, 2012

Danke! Diese Bewertungen habe ich noch nicht so auf dem Schirm - mir fallen da auf Anhieb 10 Leute ein, die sofort grüne Klötzchen bräuchten. (Edit: Bitte nicht falsch verstehen, ich meine: Sie verdient hätten)
Wiki? Unter welchem Punkt soll ich das denn am Besten zusammenfassen? Gibt es eine Abteilung "Multiwii Software Usermods" ?
Warum sind die gif's jetzt teilweise unleserlich kleine jpg? Ich lade sie einfach nochmal als jpg hoch.
EDIT: Das ist die 800*600 Begrenzung, das konnte nicht gut gehen......
LG
Rob

upapa · Aug 26, 2012

Roberto hat gesagt.:

Hi Roberto,
vielen Dank für die Mühe, die Du Dir mit der transparenten Schilderung Deiner Code-Änderungen gemacht hast!
So war das "Nachbasteln" ein Kinderspiel.

Leider bekomme ich bei der Kompilierung (Arduino 1.0.1) den Fehler:
-------------------------------------------------------
MultiWii.cpp: In function 'void getEstimatedAltitude()':
IMU:315: error: 'BOXACC' was not declared in this scope
-------------------------------------------------------
Glaube, dass ich den Code richtig übernommen habe...(hoffentlich

)

upapa

Roberto · Aug 27, 2012

Da müssen die im Hauptprogramm den Namen/Definition von BOXACC geändert haben. Du bist doch hier: http://code.google.com/p/multiwii/source/browse/#svn/trunk/MultiWii_shared unterwegs? Ich schau mal, was da im Hauptprogramm an den Definitionen geändert wurde. Vor so etwas ist man natürlich nicht sicher, wenn die alles umstricken.

EDIT:
Wie vermutet, gibt es kein BOXACC mehr, sondern wie ab Zeile 47 zu lesen: "BOXANGLE" und "BOXHORIZON".

Wenn Du jetzt die "Fehlerzeile":

Code:

  if (rcOptions[BOXACC]){                                       // Only do ACCZ in Level Mode

Durch diese ersetzt,

Code:

  if (rcOptions[BOXANGLE] || rcOptions[BOXHORIZON]){                                       // Only do ACCZ in Level Mode

sollte es klappen!

Ohne weitere Nachforschungen kann ich Dir allerdings nicht sagen, welche Verkippung diese beiden neuen Flugmodi zulassen. Und darum ging es. D.h. der Copter darf nicht zu stark verkippt sein für die ACCZ Korrektur. Wahrscheinlich wirst Du Dir auch für die spätere Konfiguration direkt aus dem Tunc eine neue GUI zusammenkompilieren dürfen.
LG
Rob

upapa · Aug 27, 2012

Roberto hat gesagt.:

Wie vermutet, gibt es kein BOXACC mehr, sondern wie ab Zeile 47 zu lesen: "BOXANGLE" und "BOXHORIZON".

Wenn Du jetzt die "Fehlerzeile":

Code:

  if (rcOptions[BOXACC]){                                       // Only do ACCZ in Level Mode

Durch diese ersetzt,

Code:

  if (rcOptions[BOXANGLE] || rcOptions[BOXHORIZON]){                                       // Only do ACCZ in Level Mode

sollte es klappen!

Hi Roberto,
das war es! Jetzt lässt sich der Code kompilieren.

Vielen Dank für Deinen Hinweis!

Roberto hat gesagt.:

Werde ich mal austesten...

Roberto hat gesagt.:

Ist momentan noch nicht nötig, da die MultiWiiConf.pde immer noch v1046 vom 5. August 2012 ist.
Habe aber schon mal probeweise die GUI mittels Processing kompiliert. Und es hat funktioniert.

upapa

Roberto · Aug 27, 2012

@ upapa:
Danke für Dein Feedback!
Ich antworte Dir hier auf Deine Frage aus dem Hauptthread, da ich für Dich eine Codeänderung vorschlagen wollte, die vielleicht hier besser aufgehoben ist.

Wenn Du die Zeile unter IMU/void getEstimatedAltitude()

Code:

  ACCZD = accADC[YAW]-acc_1G;                                   // So UPacc is +, DWNacc is -

änderst in:

Code:

  ACCZD = (accADC[YAW]-acc_1G)/2;                               // So UPacc is +, DWNacc is -

Vielleicht kannst Du so, mit halbierten Werten, das "D" wieder mit Deiner MPU benutzen.
Wäre doch schade drum, da der Höhenverlust beim Vorwärtsflug eigentlich durch den ACC gleich mit ausgeglichen wird.

LG
Rob

EDIT: Ich habe hier grad noch einen anderen ACC Code in Arbeit, mit dem direkt auf eine Variorate in cm/sec umgerechnet wird - unabhängig vom verbauten ACC. Die ausgespuckten Werte sind bislang ganz plausibel und scheinen brauchbar. Wenn das gut funktioniert, dürften diese Probleme für alle ACC gelöst sein.

EDIT: Im Originalcode gibt es immerhin auch Bewegung im Barobereich - man kann die Barofunktion jetzt abschalten um Speicher zu sparen

. - Naja, wenigstens schaut sich jemand mal die Sache an.
"....
/* uncomment to disable the altitude hold feature.
* This is useful if all of the following apply
* + you have a baro
* + want altitude readout
* + do not use altitude hold feature
* + want to save memory space
*/
//#define SUPPRESS_BARO_ALTHOLD
...."
Auf die Idee wäre ich allerdings nicht gekommen.
In diesem Sinne: Gute Nacht!
Rob

upapa · Aug 28, 2012

Roberto hat gesagt.:

Wenn Du die Zeile unter IMU/void getEstimatedAltitude()

Code:

  ACCZD = accADC[YAW]-acc_1G;                                   // So UPacc is +, DWNacc is -

änderst in:

Code:

  ACCZD = (accADC[YAW]-acc_1G)/2;                               // So UPacc is +, DWNacc is -

Vielleicht kannst Du so, mit halbierten Werten, das "D" wieder mit Deiner MPU benutzen.
Wäre doch schade drum, da der Höhenverlust beim Vorwärtsflug eigentlich durch den ACC gleich mit ausgeglichen wird.

Hi Roberto,
danke für den Tipp. Musste ihn doch gleich mal in die Tat umsetzen:

Kann nun D bis 20 hochschrauben. Und so ist der Höhenverlust bei kräftiger horizontaler Beschleunigung deutlich reduziert. Gibt man nur wenig Pitch, ist er fast nicht mehr vorhanden. Nimmt man Pitch weg, ist in bestimmten Situationen ein leichtes Überschwingen zu beobachten, d.h. der Copter klettert ein wenig in die Höhe, regelt sich dann aber schnell auf Sollhöhe ein.

Roberto hat gesagt.:

Das hört sich sehr interessant an!

Roberto hat gesagt.:

upapa

Luigy79 · Aug 28, 2012

GRAZIE mille!!! (;

Roberto · Aug 29, 2012

@Upapa:
Da ich momentan nicht zum Fliegen komme, und Du sehr exprementierfreudig unterwegs bist, möchte ich Dich fragen, ob Du vielleicht den neuen ACC Code testen möchtest. Er ist bislang ungeflogen.
Ich habe den ACC "D" jetzt neu geschrieben, und bin erstaunt, dass so eine Billigformel letztendlich dabei herum gekommen ist. Der Rest ist gleich geblieben. Am Anfang fragt nur der Compiler ab, welcher Baro verbaut ist und die Erfahrungswerte aus den bisherigen Rückmeldungen werden gesetzt. Wenn kein Baro definiert ist, gibts eine Fehlermeldung. Ich habe Dir auch direkt diese BOXHORIZON Kiste implementiert.
In der Hand macht mein Copter mit der FREEIMUv035_MS von Funkjan mit einem "D" von 20 einen guten Eindruck. Der ACC ist jetzt nicht mehr so "krass", weil keine Extremwerte mehr verwendet werden. EDIT: Es sollte mich aber nicht wundern, wenn der alte code noch besser arbeitet.

Mal sehen wer schneller mit dem Code in der Luft ist

....

So long
Rob
P.s.: @Luigy: prego!

EDIT: Alle Wagemutigen sind natürlich auch herzlich eingeladen!

Code:

///////////////////////////////////////////////
//Crashpilot1000 Mod getEstimatedAltitude ACC//
///////////////////////////////////////////////
#if defined(MS561101BA)
  #define VarioTabsize 10                                       // Stores 10 Values. 
#endif
#if defined(BMP085)
  #define VarioTabsize 7                                        // Stores 7 Values. New Value every 30ms so it oversees 210ms
#endif
#define Droppercent 20

void getEstimatedAltitude()
{
  static uint8_t Vidx=0,AcczSumCnt=0;
  static int32_t LastEstAlt=0,AccSum;
  static int8_t VarioTab[VarioTabsize];
  int8_t VarioIdxCnt;
  int16_t ACCZD,BaroP,AltI,BaroDiff,BaroClimbRateTmp;

  ACCZD = accADC[YAW]-acc_1G;                                   // So UPacc is +, DWNacc is -
  AccSum = AccSum+ACCZD;
  AcczSumCnt++;

  if (newbaroalt==0) return;                                    // Update only, if new Baro-values are available
  newbaroalt = 0;                                               // Reset Boolean

  BaroAlt = BaroAlt-GroundAlt;                                  // Set to Zero on Ini hight
  BaroAlt = EstAlt+constrain(BaroAlt-EstAlt,-15,15);
  EstAlt = (EstAlt+EstAlt+BaroAlt)/3;

  // Baro Climbrate
  VarioTab[Vidx] = constrain(EstAlt-LastEstAlt,-126,126);
  LastEstAlt = EstAlt;
  Vidx++;
  if (Vidx==VarioTabsize) Vidx=0; 
  BaroClimbRateTmp = 0;
  VarioIdxCnt = 0;
  while(VarioIdxCnt<VarioTabsize){
  BaroClimbRateTmp = BaroClimbRateTmp+VarioTab[VarioIdxCnt];
  VarioIdxCnt++;}
  BaroClimbRate = ((int32_t)BaroClimbRateTmp*33)/VarioTabsize;  //BaroClimbRate in cm/sec // + is up // 30ms * 33 = 990ms

  // 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

  // Baro I
  AltI = constrain((((int32_t)conf.I8[PIDALT]*BaroClimbRate)/100),-100,100);

  // ACCZ "D"
  ACCZD = 0;
//  if (rcOptions[BOXACC]){                                       // Only do ACCZ in Level Mode
  if (rcOptions[BOXANGLE] || rcOptions[BOXHORIZON]){  // Damits auch mit dem Trunc läuft....
  AccSum = ((AccSum*30)/AcczSumCnt)/acc_1G;                     // Very rough roundigs and assumptions (9,81=10, Time=30ms etc), just working //AccSum = (((AccSum*100/AcczSumCnt)/acc_1G)*981*30)/100000;
  ACCZD = constrain(((int16_t)conf.D8[PIDALT]*AccSum),-150,0);}

  // Reset ACC Values
  AccSum=0;
  AcczSumCnt=0;

  BaroPID = constrain(BaroP-AltI-ACCZD,-250,250);
  if (BaroPID < 0) BaroPID=(BaroPID*Droppercent)/100;           // Lower Dropping Values by a Factor Droppercent
}

upapa · Aug 29, 2012

Roberto hat gesagt.:

Hi Roberto,
habe Deinen neuen ACC-Code getestet. Und Du hast mit Deiner Vermutung nach EDIT recht.

Diese Variante und ein D=20 (wie bisher gut geeignet) lässt meinen CriusPro mit zuckelnden Gasstößen gen Himmel entschwinden. Letztendlich bleibt nur, das D gegen Null tendieren zu lassen...
Deshalb bin ich wieder zur vorherigen Version zurückgekehrt.

upapa

Roberto · Aug 30, 2012

@upapa: Danke für Deinen Test!! Du warst jedenfalls schneller! Ich bin nur sehr spät zu einem kurzen Garagentest gekommen, mit gleichem Ergebnis

. Es muss erst schlechter werden, bevor es besser werden kann. Wenigstens liefern beide Acc ein gleich schlechtes Ergebnis, das ist immerhin schon etwas

.

Lg
Rob

Roberto · Sep 12, 2012

Hi!
Wie hier angedroht: http://fpv-community.de/showthread....o-vern%FCnftig&p=198940&viewfull=1#post198940
Gibt es etwas neues, nicht zuletzt auch auf dem Boden der Infos hier: http://www.multiwii.com/forum/viewtopic.php?f=8&t=2371.
Hier sind noch einmal die Änderungen:

Meine aktuellen ALT PIDs sind P 10,0 // I 0,020 // D 60 . Diese Werte sind auch als Default hinterlegt. Wenn man von einer anderen 2.1 Version kommt, ist ein eeprom clear eigentlich nicht erforderlich. Die Config.h wurde nicht verändert. FÜR DIESE VERSION IST EIN ACC PFLICHT! NUR GYRO UND BARO GEHT NICHT!

Die Parameter:
P: Das P ist weitgehend gleich geblieben, also das P von "MultiWii_2_1_NewBaroPIDVario2b" kann man eigentlich weiter verwenden oder als Ausgangspunkt nehmen. Die Barowerte werden jetzt etwas stärker gefiltert, evtl. kann dieses mit dem BMP085 ein schlechteres Ergebnis bringen! Bitte um Rückmeldung!

I: Das I ist weiterhin die Variometerbremse, nur deutlich verändert!! Jetzt wird ein winkelkorrigiertes ACCZ mit einbezogen.
Ich kann nur mit der FreeIMU v0.3.5_MS, also einem BMA180 testen. THEORETISCH sollte es mit allen funktionieren. Besonders interessant wäre es zu wissen, wie es auf dem BMA020 und der MPU und da besonders in der Variante der Freeimu 0.4 funktioniert. Nunchuk ist wahrscheinlich eher untauglich.

D: Das D hat nichts mehr mit dem ACC zu tun. Da in der jetzigen ACC Integration intern eine Winkelkorrektur durchgeführt wird, fällt der ehemalige, angenehme Nebeneffekt des Anhebens der Motorgeschwindigkeit bei Verkippung weg. Daher kann man hier einstellen, wie stark das Gas bei Verkippung des Copters angehoben wird. Ein zu kleiner Wert fällt durch zu starkes Absacken im Vorwärtsflug auf. Das Höhehalten erst im Schweben (+2m !) einstellen mit leichten Bewegungen, dann das D im Flug nachstellen.

Diese Version hat noch nicht den Bigbretl Position Hold mod (http://fpv-community.de/showthread.php?14199-Baro-Code-%C4nderungen&p=189821&viewfull=1#post189821). Kommt natürlich später rein.

Die Version habe ich ausgiebig getestet. Es kam zu keinen merkwürdigen/gefährlichen Ereignissen. Falls doch etwas sein sollte, kann man alle Änderungen mit Umlegen des Baroschalters sofort deaktivieren.
Viel Spass beim Testen!

LG
Rob

Eine kurze Rückmeldung im Hauptthread (http://fpv-community.de/showthread.php?10765-Funktionieren-bei-Euch-Kompass-und-Baro-vern%FCnftig) oder hier wäre nett, und ist herzlich willkommen!

solution · Sep 12, 2012

RESPEKT und meinen Dank!!
Gerade mit einer 0.35MS und den Default-Werten getestet! Das klappte selbst im Wind schon mal sehr gut!!

Tolle Arbeit!

Gruß
Joachim

Roberto · Sep 12, 2012

Danke Joachim für Dein Feedback!
Ich habe noch weitergebastelt, I und D werden jetzt bei jedem Durchlauf aktualisiert, das Baro P und die Barosteigrate kommen alle 30ms frisch dazu. Dadurch wird schneller und weicher reagiert. Die I & D Werte muss ich jetzt noch neu erfliegen bzw. anpassen. Mit den Werten von oben überschiesst es jetzt etwas. Jetzt werde ich schnell noch meine APM mit den letzten Sonnenstrahlen fliegen. Also die Beta ist schon in den Startlöchern..

Boarder · Sep 12, 2012

Wir findest du eigentlich das APM im Vergleich zun multiwii ?

Roberto · Sep 12, 2012

Mittlerweile "liebe" ich beide Systeme, obwohl es Anfangs mit der APM nicht leicht war

. Beide Systeme haben ihre Stärken und Schwächen. Von den Möglichkeiten her gewinnt natürlich die APM. Die APM ist von der Anlage her eher ein Flugroboter, den man auch manuell steuern kann. Die Multiwii kommt aus dem Fliegerlager, mit entsprechender Gewichtung, und ist auf dem Weg autonomer zu werden. Wenn ich viel APM2 fliege, freue ich mich auf die Multiwii und umgekehrt! Mehr möchte ich in diesem Thread nicht dazu schreiben, damit das hier nicht vollkommen am Ziel vorbei geht.

LG
Rob

Baro Code Änderungen

Erfahrener Benutzer

Erfahrener Benutzer

Anhänge

Erfahrener Benutzer

Anhänge

Erfahrener Benutzer

Administrator

Erfahrener Benutzer

Erfahrener Benutzer

Anhänge

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Anhänge

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer

Erfahrener Benutzer