WebSVN - FlightCtrl - Diff - Rev 2062 and 2067 - /branches/dongfang_FC

 #define HEF4017R_ON     PORTC |=  (1<<PORTC6)
 #define HEF4017R_OFF    PORTC &= ~(1<<PORTC6)
 /*****************************************************
  *              Initialize Timer 2
  *****************************************************/
 void timer2_init(void) {
-        uint8_t sreg = SREG;
+    uint8_t sreg = SREG;
-        // disable all interrupts before reconfiguration
+    // disable all interrupts before reconfiguration
-        cli();
+    cli();
-        // set PD7 as output of the PWM for pitch servo
+    // set PD7 as output of the PWM for pitch servo
-        DDRD |= (1 << DDD7);
+    DDRD |= (1 << DDD7);
-        PORTD &= ~(1 << PORTD7); // set PD7 to low
+    PORTD &= ~(1 << PORTD7); // set PD7 to low
-        DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017)
+    DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017)
-        HEF4017R_ON; // enable reset
+    HEF4017R_ON; // enable reset
-        // Timer/Counter 2 Control Register A
+    // Timer/Counter 2 Control Register A
-        // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0)
+    // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0)
-        // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0)
+    // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0)
-        // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
+    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
-        TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22));
+    TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22));
-        TCCR2A |= (1 << COM2A1) | (1 << WGM21);
+    TCCR2A |= (1 << COM2A1) | (1 << WGM21);
-        // Timer/Counter 2 Control Register B
+    // Timer/Counter 2 Control Register B
-        // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz
+    // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz
-        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us
+    // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us
-        // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms
+    // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms
-        TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22));
+    TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22));
-        TCCR2B |= CS2;
+    TCCR2B |= CS2;
-        // Initialize the Timer/Counter 2 Register
+    // Initialize the Timer/Counter 2 Register
-        TCNT2 = 0;
+    TCNT2 = 0;
-        // Initialize the Output Compare Register A used for signal generation on port PD7.
+    // Initialize the Output Compare Register A used for signal generation on port PD7.
-        OCR2A = 255;
+    OCR2A = 255;
-        // Timer/Counter 2 Interrupt Mask Register
+    // Timer/Counter 2 Interrupt Mask Register
-        // Enable timer output compare match A Interrupt only
+    // Enable timer output compare match A Interrupt only
-        TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
+    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
-        TIMSK2 |= (1 << OCIE2A);
+    TIMSK2 |= (1 << OCIE2A);
-        for (uint8_t axis=0; axis<2; axis++)
+    for (uint8_t axis=0; axis<2; axis++)
-          previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
+      previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
-        SREG = sreg;
+    SREG = sreg;
+}
 /*
 void servo_On(void) {
-        servoActive = 1;
+    servoActive = 1;
+}
 void servo_Off(void) {
-        servoActive = 0;
+    servoActive = 0;
-        HEF4017R_ON; // enable reset
+    HEF4017R_ON; // enable reset
+}
 */
   // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
   // That is a divisor of about 1<<14. Same conclusion as H&I.
   value *= staticParams.servoConfigurations[axis].stabilizationFactor;
   value /= 256L;
   if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
-        return -value;
+    return -value;
   return value;
+}
 // With constant-speed limitation.
 uint16_t servoValue(uint8_t axis) {
   int16_t value;
   if (axis<2) value = featuredServoValue(axis);
   else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8.
   // Shift out of the [0..255*SCALE_FACTOR] space
   value -= (128 * SCALE_FACTOR);
   if (value < -SERVOLIMIT) value = -SERVOLIMIT;
   else if (value > SERVOLIMIT) value = SERVOLIMIT;
   // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
   return value + NEUTRAL_PULSELENGTH;
+}
 void calculateServoValues(void) {
   if (!recalculateServoTimes) return;
   for (uint8_t axis=0; axis<MAX_SERVOS; axis++) {
-        servoValues[axis] = servoValue(axis);
+    servoValues[axis] = servoValue(axis);
+  }
   recalculateServoTimes = 0;
+}
 ISR(TIMER2_COMPA_vect) {
   static uint16_t remainingPulseTime;
   static uint8_t servoIndex = 0;
   static uint16_t sumOfPulseTimes = 0;
   if (!remainingPulseTime) {
     // Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
+    }
+  }
   // Schedule the next OCR2A event. The counter is already reset at this time.
   if (remainingPulseTime > 256+128) {
     // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
     // as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
     OCR2A = 255;
     TCCR2A &= ~(1<<COM2A0);
     remainingPulseTime-=256;
   } else if (remainingPulseTime > 256) {
     // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard
     // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt
     // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
     remainingPulseTime-=128;
     OCR2A=127;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/timer2.c @ 1720 - Rev 2062 → 2067