WebSVN - FlightCtrl - Diff - Rev 2125 and 2132 - /branches/dongfang_FC_fixedwing/arduino

 #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.trim-128); // best value with my Futaba in zero trim.
+        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;
-      //  }
+    }
+  }
+}
-#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.
-        if (channel <= -TIME(0.55)) {
+                lastRCCommand = COMMAND_GYROCAL;
-          lastRCCommand = COMMAND_GYROCAL;
+          else
-          debugOut.analog[17] = 1;
+                lastRCCommand = COMMAND_RCCAL;
-        } else {
-          lastRCCommand = COMMAND_NONE;
+        } else {
-          debugOut.analog[17] = 0;
+          lastRCCommand = COMMAND_NONE;
+        }
         return lastRCCommand;
 /*
  * Get Pitch, Roll, Throttle, Yaw values
  */
-void RC_periodicTaskAndPRYT(int16_t* PRYT) {
-  if (RCQuality) {
+void RC_periodicTaskAndPRYT(int16_t* PRYT) {
+  RC_process();
-    RCQuality--;
-    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR);
+  PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR)   - rcTrim.trim[CH_ELEVATOR];
-    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS);
+  PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS)   - rcTrim.trim[CH_AILERONS];
-    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER);
+  PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)     - rcTrim.trim[CH_RUDDER];
-    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);
-  } // if RCQuality is no good, we just do nothing.
+  PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);  // no trim on throttle!
+}
 /*
  * Get other channel value
  */
 int16_t RC_getVariable(uint8_t varNum) {
   if (varNum < 4)
     // 0th variable is 5th channel (1-based) etc.
-    return (RCChannel(varNum + CH_POTS) >> 2) + channelMap.variableOffset;
+    return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset;
   /*
    * Let's just say:
    * The RC variable i is hardwired to channel i, i>=4
    */
-  return (PPM_in[varNum] >> 2) + channelMap.variableOffset;
+  return (PPM_in[varNum] >> 3) + channelMap.variableOffset;
+}
     return SIGNAL_BAD;
   return SIGNAL_LOST;
+}
+void RC_calibrate(void) {
+  rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
+  rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
-void RC_calibrate(void) {
+  rcTrim.trim[CH_RUDDER]   = RCChannel(CH_RUDDER);
-  // Do nothing.
+  rcTrim.trim[CH_THROTTLE] = 0;
 }
-int16_t RC_getZeroThrottle() {
+int16_t RC_getZeroThrottle(void) {
+        return TIME (-0.5);
+}
+void RC_setZeroTrim(void) {
+  for (uint8_t i=0; i<MAX_CHANNELS; i++) {
+    rcTrim.trim[i] = 0;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c - Rev 2125 → 2132