WebSVN - FlightCtrl - Diff - Rev 1910 and 2099 - /branches/dongfang_FC

 #include <stdlib.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
-#include "rc.h"
-#include "uart0.h"
+#include "rc.h"
 #include "controlMixer.h"
 #include "configuration.h"
+#include "commands.h"
-#include "commands.h"
+#include "output.h"
-// The channel array is 1-based. The 0th entry is not used.
-volatile int16_t PPM_in[MAX_CHANNELS];
+// The channel array is 0-based!
-volatile uint8_t NewPpmData = 1;
-volatile int16_t RC_Quality = 0;
+volatile int16_t PPM_in[MAX_CHANNELS];
-int16_t RC_PRTY[4];
+volatile uint8_t RCQuality;
-uint8_t lastRCCommand = COMMAND_NONE;
-uint8_t commandTimer = 0;
-// Useless. Just trim on the R/C instead.
+uint8_t lastRCCommand = COMMAND_NONE;
-// int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
+uint8_t commandTimer = 0;
 /***************************************************************
  *  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 << DDD6);
+  DDRD &= ~(1<<6);
-  PORTD |= (1 << PORTD6);
+  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);
+  DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3);
   // low level
-  PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3));
+  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);
+    DDRD |= (1<<PORTD3);
-    PORTD &= ~(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)
+  // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
-  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
+  // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
-  // Enable input capture noise cancler (bit: ICNC1=1)
+  // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
-  // 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
+  // Enable input capture noise cancler (bit: ICNC1=1)
-  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
+  // Trigger on positive edge of the input capture pin (bit: ICES1=1),
-  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0)
+  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
-      | (1 << WGM11) | (1 << WGM10));
+  // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
-  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
+  TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
-  TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
+  TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
-  TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
+  TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1);
   TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
   // Timer/Counter1 Interrupt Mask Register
   // Timer/Counter1 Interrupt Mask Register
   // Enable Input Capture Interrupt (bit: ICIE1=1)
   // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
   // Enable Overflow Interrupt (bit: TOIE1=0)
-  TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1));
+  TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
-  TIMSK1 |= (1 << ICIE1);
+  TIMSK1 |= (1<<ICIE1);
-  RC_Quality = 0;
+  RCQuality = 0;
   SREG = sreg;
+}
 /********************************************************************/
 /*         Every time a positive edge is detected at PD6            */
 /********************************************************************/
 /*                               t-Frame
- <----------------------------------------------------------------------->
+    <----------------------------------------------------------------------->
- ____   ______   _____   ________                ______    sync gap      ____
+     ____   ______   _____   ________                ______    sync gap      ____
- |    | |      | |     | |        |              |      |                |
+    |    | |      | |     | |        |              |      |                |
- |    | |      | |     | |        |              |      |                |
- ___|    |_|      |_|     |_|        |_.............|      |________________|
+    |    | |      | |     | |        |              |      |                |
- <-----><-------><------><-------->              <------>                <---
+ ___|    |_|      |_|     |_|        |_.............|      |________________|
- t0       t1      t2       t4                     tn                     t0
+    <-----><-------><------><-----------            <------>                <---
  t0       t1      t2       t4                     tn                     t0
  The PPM-Frame length is 22.5 ms.
- Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
+ The PPM-Frame length is 22.5 ms.
   signal = (uint16_t) ICR1 - oldICR1;
   oldICR1 = ICR1;
   //sync gap? (3.52 ms < signal < 25.6 ms)
-  if ((signal > 1100) && (signal < 8000)) {
-    // if a sync gap happens and there where at least 4 channels decoded before
-    // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
-    if (index >= 4) {
-      NewPpmData = 0; // Null means NewData for the first 4 channels
-    }
-    // synchronize channel index
+  if ((signal > 1100) && (signal < 8000)) {
-    index = 1;
+    index = 0;
   } else { // within the PPM frame
-    if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels
+    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.5 to 2.0ms/3.2us = 625
+      // 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.2µs = 1.5008 ms)
+        signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms)
-        // Signal is now in the +/- 156 range (nominally).
+        // check for stable signal
         if (abs(signal - PPM_in[index]) < 6) {
-          if (RC_Quality < 200)
+          if (RCQuality < 200)
-            RC_Quality += 10;
+            RCQuality += 10;
           else
-            RC_Quality = 200;
+            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[index] = signal; // update channel value
+        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;
-      index++; // next channel
+      //  }
+    }
+  }
 }
-#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
-/*
- * This must be called (as the only thing) for each control loop cycle (488 Hz).
- */
-void RC_update() {
-  if (RC_Quality) {
-    RC_Quality--;
-    if (NewPpmData-- == 0) {
-        RC_PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * staticParams.StickElevatorP * 2/ 10;
+#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
-        RC_PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * staticParams.StickAileronsP * 2 / 10;
+#define COMMAND_THRESHOLD 85
-        RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * 2 + 310;
-      if (RC_PRTY[CONTROL_THROTTLE] < 0)
-        RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
-        RC_PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * staticParams.StickRudderP * 2 / 10;
-    }
-  } else { // Bad signal
-    RC_PRTY[CONTROL_ELEVATOR] = RC_PRTY[CONTROL_AILERONS] = RC_PRTY[CONTROL_THROTTLE]
-        = RC_PRTY[CONTROL_RUDDER] = 0;
 #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
 #define COMMAND_CHANNEL_HORIZONTAL CH_YAW
+/*
-/*
+ * Get Pitch, Roll, Throttle, Yaw values
- * Get Pitch, Roll, Throttle, Yaw values
+ */
+void RC_periodicTaskAndPRTY(int16_t* PRTY) {
+  if (RCQuality) {
+    RCQuality--;
+    PRTY[CONTROL_ELEVATOR]   = RCChannel(CH_ELEVATOR);
+    PRTY[CONTROL_AILERONS]   = RCChannel(CH_AILERONS);
+    PRTY[CONTROL_THROTTLE]   = RCChannel(CH_THROTTLE);
+    PRTY[CONTROL_RUDDER]     = RCChannel(CH_RUDDER);
+    uint8_t command = COMMAND_NONE; //RC_getStickCommand();
+    if (lastRCCommand == command) {
+      // Keep timer from overrunning.
+      if (commandTimer < COMMAND_TIMER)
+        commandTimer++;
+    } else {
+      // There was a change.
+      lastRCCommand = command;
- */
+      commandTimer = 0;
 int16_t* RC_getEATR(void) {
-  return RC_PRTY;
+  } // 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 + 4) + POT_OFFSET;
+    // 0th variable is 5th channel (1-based) etc.
-  /*
-   * Let's just say:
-   * The RC variable 4 is hardwired to channel 5
-   * The RC variable 5 is hardwired to channel 6
+    return RCChannel(varNum + CH_POTS) + POT_OFFSET;
-   * The RC variable 6 is hardwired to channel 7
+  /*
-   * The RC variable 7 is hardwired to channel 8
+   * Let's just say:
-   * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
+   * The RC variable i is hardwired to channel i, i>=4
    */
-  return PPM_in[varNum + 1] + POT_OFFSET;
+  return PPM_in[varNum] + POT_OFFSET;
 }
 uint8_t RC_getSignalQuality(void) {
-  if (RC_Quality >= 160)
+  if (RCQuality >= 160)
     return SIGNAL_GOOD;
-  if (RC_Quality >= 140)
+  if (RCQuality >= 140)
     return SIGNAL_OK;
  */
 void RC_calibrate(void) {
   // Do nothing.
+}
-/*
- if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
- // In HH, it s OK to trim the R/C. The effect should not be conteracted here.
- stickOffsetPitch = stickOffsetRoll = 0;
- } else {
- stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
- stickOffsetRoll = RCChannel(CH_ROLL)   * staticParams.StickP;
- }
- }
- */
+uint8_t RC_getCommand(void) {
+  if (commandTimer == COMMAND_TIMER) {
+    // Stick has been held long enough; command committed.
+    return lastRCCommand;
-uint8_t RC_getCommand(void) {
+  }
-  // Noy impplemented - not from RC at least.
+  // Not yet sure what the command is.
   return COMMAND_NONE;
+}
  * + <--
  *    0
+ *
  * Not in any of these positions: 0
  */
-uint8_t RC_getArgument(void) {
-  return 0;
-}
+#define ARGUMENT_THRESHOLD 70
+#define ARGUMENT_CHANNEL_VERTICAL CH_ELEVATOR
+#define ARGUMENT_CHANNEL_HORIZONTAL CH_AILERONS
+uint8_t RC_getArgument(void) {
+  if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
+    // vertical is up
+    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
+      return 2;
+    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
+      return 4;
+    return 3;
+  } else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
+    // vertical is down
+    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
+      return 8;
+    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
+      return 6;
+    return 7;
+  } else {
+    // vertical is around center
+    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
+      return 1;
+    if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
-uint8_t RC_testCompassCalState(void) {
+      return 5;
+    return 0;
   return 0;
-}
-/*
- * Abstract controls are not used at the moment.
- t_control rc_control = {
- RC_getPitch,
- RC_getRoll,
- RC_getYaw,
- RC_getThrottle,
- RC_getSignalQuality,
- RC_calibrate
- };

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/rc.c - Rev 1910 → 2099