Code: Alles auswählen

#if defined(FBL)
ISR(TIMER0_COMPA_vect) {
  static uint8_t state = 0;
  static uint8_t count;
  if (state == 0) {
    //http://billgrundmann.wordpress.com/2009/03/03/to-use-or-not-use-writedigital/
    PORTD |= 1<<3; //Set output 3 high
    OCR0A+= 250; // 1000 us
    state++ ;
  } else if (state == 1) {
    OCR0A+= outputServo[0]; // 1000 + [0-1020] us
    state++;
  } else if (state == 2) {
      PORTD &= ~(1<<3); //set output 3 low
      PORTB |= 1<<1; //Set output 9 high
    OCR0A+= 250; // 1000 us
    state++;
  } else if (state == 3) {
    OCR0A+= outputServo[1]; // 1000 + [0-1020] us
    state++;
  } else if (state == 4) {
      PORTB &= ~(1<<1); //set output 9 low
      PORTB |= 1<<2; //Set output 10 high
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 5) {
    OCR0A+= outputServo[2]; // 1000 + [0-1020] us
    state++;
  } else if (state == 6) {
      PORTB &= ~(1<<2); //set output 10 low
      PORTB |= 1<<3; //Set output 11 high
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 7) {
    OCR0A+= outputServo[3]; // 1000 + [0-1020] us
    state++;
  } else if (state == 8) {
      PORTB &= ~(1<<3); //set output 11 low
    count = 10; // 12 x 1000 us
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 9) {
    if (count > 0) count--;
    else state = 0;
    OCR0A+= 250;
  }
}
#endif

Code: Alles auswählen

    gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
    gyroYawSmooth = gyroData[YAW];

Code: Alles auswählen

    static int16_t gyroPitchSmooth = 0;
    gyroData[PITCH] = ((int32_t)gyroPitchSmooth*10+gyroData[PITCH]+5)/11;
    gyroPitchSmooth = gyroData[PITCH];