WebSVN - FlightCtrl - Diff - Rev 2108 and 2109 - /branches/dongfang_FC_fixedwing/arduino

 volatile uint8_t RCQuality;
 uint8_t lastRCCommand = COMMAND_NONE;
+uint8_t lastFlightMode = FLIGHT_MODE_NONE;
-uint8_t lastFlightMode = FLIGHT_MODE_NONE;
+#define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s))
 /***************************************************************
  *  16bit timer 1 is used to decode the PPM-Signal
  ***************************************************************/
 void RC_Init(void) {
   uint8_t sreg = SREG;
   // disable all interrupts before reconfiguration
   cli();
-  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
-  DDRD &= ~(1<<6);
-  PORTD |= (1<<PORTD6);
-  // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
-  // set as output
-  DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3);
-  // low level
-  PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3));
-  // PD3 can't be used if 2nd UART is activated
-  // because TXD1 is at that port
   // if (CPUType != ATMEGA644P) {
-  //  DDRD |= (1<<PORTD3);
+  // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
-  //  PORTD &= ~(1<<PORTD3);
+  DDRB &= ~(1<<0);
-  // }
+  PORTB |= (1<<PORTB0);
   // Trigger on positive edge of the input capture pin (bit: ICES1=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));
   TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
-  TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
+  TCCR1B |= (1 << CS11) | (1 << ICES1) | (1 << ICNC1);
   TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
   // Timer/Counter1 Interrupt Mask Register
   // Enable Input Capture Interrupt (bit: ICIE1=1)
  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;
+  int16_t signal, tmp;
   static int16_t index;
   static uint16_t oldICR1 = 0;
   // 16bit Input Capture Register ICR1 contains the timer value TCNT1
   // 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)
+  //sync gap? (3.5 ms < signal < 25.6 ms)
+  if (signal > TIME(3.5)) {
-  if ((signal > 1100) && (signal < 8000)) {
+    // never happens...
     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.1984 ms)
-      // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
+      // check for valid signal length (0.8 ms < signal < 2.2 ms)
-      if ((signal > 250) && (signal < 687)) {
+      if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
+        // shift signal to zero symmetric range  -154 to 159
-        // shift signal to zero symmetric range  -154 to 159
+        //signal -= 3750; // theoretical value
-        signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
+        signal -= (TIME(1.5) + RC_TRIM); // best value with my Futaba in zero trim.
         // check for stable signal
-        if (abs(signal - PPM_in[index]) < 6) {
+        if (abs(signal - PPM_in[index]) < TIME(0.05)) {
           if (RCQuality < 200)
             RCQuality += 10;
           else
             RCQuality = 200;
+    }
+  }
+}
-#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
-#define COMMAND_THRESHOLD 85
+#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
 #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
-#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
-#define RC_SCALING 4
+#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 uint8_t getControlModeSwitch(void) {
-        int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET;
-        uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL :
+        int16_t channel = RCChannel(CH_MODESWITCH);
-                (channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
+        uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE);
         return flightMode;
+}
                 return lastRCCommand;
+        }
         int16_t channel = RCChannel(CH_THROTTLE);
+        if (channel <= -TIME(0.55)) {
-        if (channel <= -140) { // <= 900 us
+          lastRCCommand = COMMAND_GYROCAL;
-                lastRCCommand = COMMAND_GYROCAL;
+          debugOut.analog[17] = 1;
+        } else {
-        } else {
+          lastRCCommand = COMMAND_NONE;
-          lastRCCommand = COMMAND_NONE;
+          debugOut.analog[17] = 0;
+        }
         return lastRCCommand;
     debugOut.analog[20] = RCChannel(CH_ELEVATOR);
     debugOut.analog[21] = RCChannel(CH_AILERONS);
     debugOut.analog[22] = RCChannel(CH_RUDDER);
     debugOut.analog[23] = RCChannel(CH_THROTTLE);
-    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR) * RC_SCALING;
+    PRYT[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR) / RC_SCALING;
-    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS) * RC_SCALING;
+    PRYT[CONTROL_AILERONS]   = RCChannel(CH_AILERONS) / RC_SCALING;
-    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)   * RC_SCALING;
+    PRYT[CONTROL_RUDDER]     = RCChannel(CH_RUDDER)   / RC_SCALING;
-    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE) * RC_SCALING;
+    PRYT[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE) / RC_SCALING;
   } // if RCQuality is no good, we just do nothing.
+}
 /*
  * 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) + POT_OFFSET;
+    return (RCChannel(varNum + CH_POTS) >> 2) + VARIABLE_OFFSET;
   /*
    * Let's just say:
    * The RC variable i is hardwired to channel i, i>=4
    */
-  return PPM_in[varNum] + POT_OFFSET;
+  return (PPM_in[varNum] >> 2) + VARIABLE_OFFSET;
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c @ 2111 - Rev 2108 → 2109