Funktionieren bei Euch Kompass und Baro vernünftig ?

static uint8_t newbaroalt=0;
static uint8_t newestalt=0;

...
    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();debug3 = sonarAlt;
        #endif
        #ifdef LANDING_LIGHTS_DDR
          auto_switch_landing_lights();
        #endif
        break;
    }
...

  #if BARO
    Baro_update();                                                                   // Do Baro every time!
    getEstimatedAltitude();

    if (rcOptions[BOXBARO])
     {
      if (newestalt==1){                                                             //is there a new barovalue?
        newestalt=0;
        if (abs(EstAlt*30 - AltHold*30) >100 && abs(rcCommand[THROTTLE] - initialThrottleHold) >40) baroMode = 0; // so that a new althold reference is defined
       }
      if (abs(rcCommand[THROTTLE] - initialThrottleHold) <40){                       // If Stick movement exceeds 40 - dont do BAROPID
        rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
        rcCommand[THROTTLE] = constrain(rcCommand[THROTTLE],MINTHROTTLE+1,MAXTHROTTLE-50);
       }
     }
  #endif

///////////////////////////////////////////////
//Crashpilot1000 Mod getEstimatedAltitude ACC//
///////////////////////////////////////////////

#define UPDATE_INTERVAL 20000                                   // Means: Dont be faster (next Gen Sensors?) than this, because ACCZ needs values

void getEstimatedAltitude()
{
  static uint32_t deadLine=0;
  static int32_t LastBaroAlt,BaroAltSum;
  static int16_t AccZLowest,AccZHighest;     // ACCZ extremes
  static int8_t BaroCnt=0;

  int16_t ACCZD,BaroP;
  int32_t BaroDiff;
  
  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 calc, at least while UPDATE_INTERVAL
  if (newbaroalt==1)                                            // Update only, if new Baro-values are available
   {
    newbaroalt = 0;                                             // Reset Boolean
    BaroAlt=BaroAlt/30;                                         // Limit Resolution to 30 cm

    BaroAltSum+=BaroAlt;                                        // do at least 340 ms average for Estalt
    BaroCnt++;
    if (BaroCnt==17){
      EstAlt=BaroAltSum/17;
      newestalt=1;                                              // Indicate, new EstAlt RDY
      BaroAltSum=0;
      BaroCnt=0;}
     
    BaroAlt = (BaroAlt+LastBaroAlt)/2;                          // Do Average with last BaroAlt
    LastBaroAlt = BaroAlt;
    
    BaroDiff = BaroAlt-AltHold;                                 // BaroDiff = Difference to locked hight pos if risen

    if (micros() < deadLine) return;                            // Break, if too fast
    deadLine = micros() + UPDATE_INTERVAL; 

    BaroPID = 0;                                                // Stores Result for mainroutine
    ACCZD = 0;
    BaroP = 0;

    // Baro P
    BaroP = conf.P8[PIDALT]*(BaroP-BaroDiff)/50;               // P of Baro a P of 5 in the GUI means 50
    BaroP = constrain(BaroP,-150,+150);                         // Limit Baro P to +/- 150

    // ACCZ
    if (rcOptions[BOXACC]){                                     // Only do ACCZ in Level Mode
      ACCZD -= (int32_t)conf.D8[PIDALT]*(abs(AccZHighest)+abs(AccZLowest))*(AccZHighest-abs(AccZLowest))/40; // conf.D8[PIDALT] Is scalefactor for ACCZ, if it is = 40 then this is 1:1 Passthrough
      ACCZD = constrain(ACCZD,-150,+150);                       // Limit ACC Z to +/- 150
     }

    BaroPID = BaroP+ACCZD;
    if (BaroPID < 0){
     BaroPID = constrain((BaroPID*conf.I8[PIDALT])/100,-150,0); // Lower Dropping Values by a Factor stored in GUI I. Gui I=0.100 = 100. Let I NEVER be 0 because BaroPID then turns positive!
    }
    AccZHighest=0;                                              // Reset Values anyway
    AccZLowest=0;
   }
}

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;
 } 
}

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()+10000-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()+10000-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()+4000-1;
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 4433000.0f; //centimeter
      newbaroalt=1;
      ms561101ba_ctx.state = 0;
      break;
  } 
}