WebSVN - FlightCtrl - Diff - Rev 2073 and 2160 - /branches/dongfang_FC

 #include "output.h"
 // The channel array is 0-based!
 volatile int16_t PPM_in[MAX_CHANNELS];
+volatile int16_t PPM_diff[MAX_CHANNELS];
+volatile uint16_t RC_buffer[MAX_CHANNELS];
+volatile uint8_t inBfrPnt = 0;
 volatile int16_t PPM_diff[MAX_CHANNELS];
 volatile uint8_t RCQuality;
 uint8_t lastRCCommand = COMMAND_NONE;
+uint8_t commandTimer = 0;
-uint8_t commandTimer = 0;
+#define TIME(s) ((int16_t)(((long)F_CPU/(long)64000)*(float)s + 0.5f))
 /***************************************************************
  *  16bit timer 1 is used to decode the PPM-Signal
  ***************************************************************/
   if (CPUType != ATMEGA644P) {
     DDRD |= (1<<PORTD3);
     PORTD &= ~(1<<PORTD3);
+  }
-  // Timer/Counter1 Control Register A, B, C
   // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
   // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
   // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
-  // Enable input capture noise cancler (bit: ICNC1=1)
-  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
+  // Enable input capture noise cancler (bit: ICNC1=1)
   // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
   // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
-  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
+  TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
-  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
+  TCCR1B &= ~((1<<WGM13) | (1<<WGM12)  | (1<<CS12));
+  TCCR1B |= (1<<CS11) | (1<<CS10) | (1<<ICNC1);
+  TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
+  if (channelMap.RCPolarity) {
+    TCCR1B |= (1<<ICES1);
+  } else {
+    TCCR1B &= ~(1<<ICES1);
+  }
   TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
   TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
   // Timer/Counter1 Interrupt Mask Register
   RCQuality = 0;
   SREG = sreg;
+}
+/*
+ * This new and much faster interrupt handler should reduce servo jolts.
+ */
+ISR(TIMER1_CAPT_vect) {
+  static uint16_t oldICR1 = 0;
+  uint16_t signal = (uint16_t)ICR1 - oldICR1;
+  oldICR1 = ICR1;
+  //sync gap? (3.5 ms < signal < 25.6 ms)
+  if (signal > TIME(3.5)) {
+        inBfrPnt = 0;
+  } else if (inBfrPnt<MAX_CHANNELS) {
+        RC_buffer[inBfrPnt++] = signal;
+        if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
+  }
+}
 /********************************************************************/
 /*         Every time a positive edge is detected at PD6            */
 /********************************************************************/
  The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
  The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
  The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
  the syncronization gap.
  */
-ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
-  int16_t signal = 0, tmp;
-  static int16_t index;
-  static uint16_t oldICR1 = 0;
-  // 16bit Input Capture Register ICR1 contains the timer value TCNT1
-  // at the time the edge was detected
-  // calculate the time delay to the previous event time which is stored in oldICR1
-  // calculatiing the difference of the two uint16_t and converting the result to an int16_t
-  // implicit handles a timer overflow 65535 -> 0 the right way.
-  signal = (uint16_t) ICR1 - oldICR1;
-  oldICR1 = ICR1;
-  //sync gap? (3.52 ms < signal < 25.6 ms)
-  if ((signal > 1100) && (signal < 8000)) {
-    index = 0;
+void RC_process(void) {
-  } else { // within the PPM frame
+  if (RCQuality) RCQuality--;
-    if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
-      // check for valid signal length (0.8 ms < signal < 2.1984 ms)
-      // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
-      if ((signal > 250) && (signal < 687)) {
-        // shift signal to zero symmetric range  -154 to 159
-        signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
-        // check for stable signal
+  for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
-        if (abs(signal - PPM_in[index]) < 6) {
-          if (RCQuality < 200)
-            RCQuality += 10;
+        uint16_t signal = RC_buffer[channel];
-          else
-            RCQuality = 200;
-        }
-        // If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff.
+        if (signal != 0) {
-        // if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
-          // In addition, if the signal is very close to 0, just set it to 0.
+          RC_buffer[channel] = 0; // reset to flag value already used.
-        if (signal >= -1 && signal <= 1) {
+      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
-          tmp = 0;
-        //} else {
+        signal -= TIME(1.5);
-        //  tmp = PPM_in[index];
-        //  }
-        } else
-          tmp = signal;
-        // calculate signal difference on good signal level
-        if (RCQuality >= 195)
-          PPM_diff[index] = signal - PPM_in[index]; //((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
-        else
+        PPM_diff[channel] = signal - PPM_in[channel];
-          PPM_diff[index] = 0;
-        PPM_in[index] = tmp; // update channel value
+        PPM_in[channel] = signal;
-      }
-      index++; // next channel
-      // demux sum signal for channels 5 to 7 to J3, J4, J5
-      // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the
-      // channels are usually available at the receiver anyway.
-      // if(index == 5) J3HIGH; else J3LOW;
-      // if(index == 6) J4HIGH; else J4LOW;
-      // if(CPUType != ATMEGA644P) // not used as TXD1
-      //  {
-      //    if(index == 7) J5HIGH; else J5LOW;
       //  }
+    }
+  }
+}
 /*
  * Get Pitch, Roll, Throttle, Yaw values
  */
 void RC_periodicTaskAndPRTY(int16_t* PRTY) {
   int16_t tmp1, tmp2;
+  RC_process();
   if (RCQuality) {
     RCQuality--;
     PRTY[CONTROL_PITCH]     = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
     PRTY[CONTROL_ROLL]      = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
     int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c - Rev 2073 → 2160