WebSVN - FlightCtrl - Diff - Rev 2122 and 2124 - /branches/dongfang_FC

 #include "commands.h"
 #include "output.h"
 // The channel array is 0-based!
+volatile int16_t PPM_in[MAX_CHANNELS];
+volatile uint16_t RC_buffer[MAX_CHANNELS];
+volatile uint8_t inBfrPnt = 0;
 volatile int16_t PPM_in[MAX_CHANNELS];
 volatile uint8_t RCQuality;
   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;
+  }
+}
 /********************************************************************/
 /*         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, tmp;
+void RC_process(void) {
-  static int16_t index;
+        if (RCQuality) RCQuality--;
-  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.
+  for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
-  signal = (uint16_t) ICR1 - oldICR1;
+        uint16_t signal = RC_buffer[channel];
-  oldICR1 = ICR1;
-  //sync gap? (3.5 ms < signal < 25.6 ms)
-  if (signal > TIME(3.5)) {
+        if (signal != 0) {
-    index = 0;
-  } else { // within the PPM frame
-    if (index < MAX_CHANNELS) { // PPM24 supports 12 channels
-      // check for valid signal length (0.8 ms < signal < 2.2 ms)
+          RC_buffer[channel] = 0; // reset to flag value already used.
       if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
-        // shift signal to zero symmetric range  -154 to 159
-        //signal -= 3750; // theoretical value
         signal -= (TIME(1.5) - 128 + channelMap.HWTrim);
-        // check for stable signal
-        if (abs(signal - PPM_in[index]) < TIME(0.05)) {
+        if (abs(signal - PPM_in[channel]) < TIME(0.05)) {
+                // With 7 channels and 50 frames/sec, we get 350 channel values/sec.
           if (RCQuality < 200)
-            RCQuality += 10;
+            RCQuality += 2;
-          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 >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) {
-          // In addition, if the signal is very close to 0, just set it to 0.
-        if (signal >= -1 && signal <= 1) {
-          tmp = 0;
-        //} else {
-        //  tmp = PPM_in[index];
-        //  }
-        } else
-          tmp = signal;
+        PPM_in[channel] = signal;
-        PPM_in[index] = tmp; // update channel value
+      }
-      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;
-      //  }
+    }
+  }
+  debugOut.analog[31] = RCQuality;
+}
-#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
-#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
-#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
+#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
 uint8_t getControlModeSwitch(void) {
         int16_t channel = RCChannel(CH_MODESWITCH);
+        }
         int16_t channel = RCChannel(CH_THROTTLE);
+        if (channel <= -TIME(0.55)) {
+          int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);
+          if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
+                lastRCCommand = COMMAND_GYROCAL;
-        if (channel <= -TIME(0.55)) {
+          else
-          lastRCCommand = COMMAND_GYROCAL;
+                lastRCCommand = COMMAND_RCCAL;
         } else {
           lastRCCommand = COMMAND_NONE;
+        }
 /*
  * Get Pitch, Roll, Throttle, Yaw values
  */
-void RC_periodicTaskAndPRYT(int16_t* PRYT) {
-  if (RCQuality) {
+void RC_periodicTaskAndPRYT(int16_t* PRYT) {
-    RCQuality--;
+  RC_process();
-    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR)   - rcTrim.trim[CH_ELEVATOR];
+  PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR)   - rcTrim.trim[CH_ELEVATOR];
-    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS)   - rcTrim.trim[CH_AILERONS];
+  PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS)   - rcTrim.trim[CH_AILERONS];
-    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)     - rcTrim.trim[CH_RUDDER];
+  PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)     - rcTrim.trim[CH_RUDDER];
-    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);  // no trim on throttle!
+  PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);  // no trim on throttle!
-    debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
+  debugOut.analog[20] = PRYT[CONTROL_ELEVATOR];
-    debugOut.analog[21] = PRYT[CONTROL_AILERONS];
+  debugOut.analog[21] = PRYT[CONTROL_AILERONS];
-    debugOut.analog[22] = PRYT[CONTROL_RUDDER];
-    debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
+  debugOut.analog[22] = PRYT[CONTROL_RUDDER];
-} // if RCQuality is no good, we just do nothing.
+  debugOut.analog[23] = PRYT[CONTROL_THROTTLE];
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/rc.c - Rev 2122 → 2124