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

 #include <stdlib.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include "rc.h"
-#include "controlMixer.h"
+//#include "controlMixer.h"
 #include "configuration.h"
 #include "commands.h"
-#include "output.h"
+#include "definitions.h"
 // The channel array is 0-based!
-volatile int16_t PPM_in[MAX_CHANNELS];
+volatile int16_t PPM_in[MAX_CONTROLCHANNELS];
-volatile int16_t PPM_diff[MAX_CHANNELS];
+volatile int16_t PPM_diff[MAX_CONTROLCHANNELS];
-volatile uint16_t RC_buffer[MAX_CHANNELS];
+volatile uint16_t RC_buffer[MAX_CONTROLCHANNELS];
-volatile uint8_t inBfrPnt = 0;
+volatile uint8_t inBfrPnt;
 volatile uint8_t RCQuality;
-uint8_t lastRCCommand = COMMAND_NONE;
+uint8_t lastRCCommand;
-uint8_t commandTimer = 0;
+uint8_t commandTimer;
-#define TIME(s) ((int16_t)(((long)F_CPU/(long)64000)*(float)s + 0.5f))
+#define TIME(s) ((int16_t)(((F_CPU/8000)*(float)s + 0.5f)))
     PORTD &= ~(1<<PORTD3);
+  }
   // 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)
+  // Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=1)
-  // Enable input capture noise cancler (bit: ICNC1=1)
+  // Enable input capture noise canceler (bit: ICNC1=1)
-  // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
+  // Therefore the counter increments 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<<ICNC1);
+  TCCR1B |= (1<<CS11)    | (1<<ICNC1);
   TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
   if (channelMap.RCPolarity) {
   // 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<<ICIE1);
   RCQuality = 0;
   SREG = sreg;
+}
 /*
   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) {
+  } else if (inBfrPnt<MAX_CONTROLCHANNELS) {
         RC_buffer[inBfrPnt++] = signal;
         if (RCQuality <= 200-4) RCQuality+=4; else RCQuality = 200;
+  }
+}
  the syncronization gap.
  */
 void RC_process(void) {
   if (RCQuality) RCQuality--;
-  for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
+  for (uint8_t channel=0; channel<MAX_CONTROLCHANNELS; channel++) {
         uint16_t signal = RC_buffer[channel];
         if (signal != 0) {
           RC_buffer[channel] = 0; // reset to flag value already used.
       if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
-        signal -= TIME(1.5);
+        signal -= TIME(1.5) /*  + channelMap.HWTrim */;
         PPM_diff[channel] = signal - PPM_in[channel];
         PPM_in[channel] = signal;
+      }
+    }
+  }
+}
 #define RCChannel(dimension) PPM_in[channelMap.channels[dimension]]
 #define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
-#define COMMAND_THRESHOLD 85
+#define COMMAND_THRESHOLD TIME(0.35f)
 #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
 #define COMMAND_CHANNEL_HORIZONTAL CH_YAW
+}
 /*
  * Get Pitch, Roll, Throttle, Yaw values
  */
-void RC_periodicTaskAndPRTY(int16_t* PRTY) {
+void RC_periodicTaskAndRPTY(int16_t* RPTY) {
   int16_t tmp1, tmp2;
   RC_process();
   if (RCQuality) {
     RCQuality--;
-    PRTY[CONTROL_PITCH]     = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
+    RPTY[CONTROL_ROLL]      = ((RCChannel(CH_ROLL) * staticParams.stickP) >> 3) + RCDiff(CH_ROLL) * staticParams.stickD;
-    PRTY[CONTROL_ROLL]      = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
+    RPTY[CONTROL_PITCH]     = ((RCChannel(CH_PITCH) * staticParams.stickP) >> 3) + RCDiff(CH_PITCH) * staticParams.stickD;
-    int16_t throttle = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
+    int16_t throttle        = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + TIME(0.4);
     // Negative throttle values are taken as zero.
     if (throttle > 0)
+      RPTY[CONTROL_THROTTLE]  = throttle;
+    else
+      RPTY[CONTROL_THROTTLE]  = 0;
-      PRTY[CONTROL_THROTTLE]  = throttle;
-    tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
+    tmp1 = RCChannel(CH_YAW); // - RCDiff(CH_YAW);
+    // exponential stick sensitivity in yawing rate
-    // exponential stick sensitivity in yawing rate
+    //
-    tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo  y = ax + bx^2
+    tmp2 = ((int32_t)staticParams.stickYawP * (int32_t)tmp1 * abs(tmp1)) >> 14; // expo  y = ax + bx^2
+    tmp2 += (staticParams.stickYawP * tmp1) >> 3;
-    tmp2 += (staticParams.stickYawP * tmp1) >> 2;
-    PRTY[CONTROL_YAW] = tmp2;
+    RPTY[CONTROL_YAW] = (/*(RCChannel(CH_YAW) * staticParams.stickYawP) >> 3*/ tmp2);
     uint8_t command = RC_getStickCommand();
     uint8_t command = RC_getStickCommand();
     if (lastRCCommand == command) {
       // Keep timer from overrunning.
       if (commandTimer < COMMAND_TIMER)
  * 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_VARIABLES) >> 3) + VARIABLES_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] >> 3) + VARIABLES_OFFSET;
+}
 uint8_t RC_getSignalQuality(void) {
   if (RCQuality >= 160)
  *    0
+ *
  * Not in any of these positions: 0
  */
-#define ARGUMENT_THRESHOLD 70
+#define ARGUMENT_THRESHOLD TIME(0.35f)
 #define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
 #define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL

Subversion Repositories FlightCtrl

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