WebSVN - FlightCtrl - Diff - Rev 1645 and 1775 - /branches/dongfang_FC

 #include <stdlib.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include "rc.h"
-#include "rc.h"
+#include "uart0.h"
 #include "controlMixer.h"
+#include "configuration.h"
-#include "configuration.h"
+#include "commands.h"
 // The channel array is 1-based. The 0th entry is not used.
 volatile int16_t PPM_in[MAX_CHANNELS];
 volatile int16_t PPM_diff[MAX_CHANNELS];
 volatile uint8_t NewPpmData = 1;
+volatile int16_t RC_Quality = 0;
+int16_t RC_PRTY[4];
+uint8_t lastRCCommand  = COMMAND_NONE;
-volatile int16_t RC_Quality = 0;
+uint8_t commandTimer = 0;
 int16_t RC_PRTY[4];
 // Useless. Just trim on the R/C instead.
-int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
+// int16_t stickOffsetPitch = 0, stickOffsetRoll = 0;
   DDRD &= ~(1<<DDD6);
   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);
+  DDRD |= (1<<DDD5)| (1<<DDD4) | (1<<DDD3);
   // low level
-  PORTD &= ~(/*(1<<PORTD5)|*/(1<<PORTD4));
+  PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3));
   // PD3 can't be used if 2nd UART is activated
   // because TXD1 is at that port
   } else { // within the PPM frame
     if(index < MAX_CHANNELS-1) // 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
-        if((signal > 250) && (signal < 687))
+        if((signal > 250) && (signal < 687)) {
-          {
             // shift signal to zero symmetric range  -154 to 159
-            signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
+            signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
             // check for stable signal
             if(abs(signal - PPM_in[index]) < 6) {
               if(RC_Quality < 200) RC_Quality +=10;
               else RC_Quality = 200;
+            }
-            // calculate exponential history for signal
+            // If signal is the same as before +/- 1, just keep it there.
-            tmp = (3 * PPM_in[index] + signal) / 4;
+            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(tmp > signal + 1) tmp--; else
+              if (signal >=-1 && signal <= 1) {
+                tmp = 0;
+              } else {
+                tmp = PPM_in[index];
+              }
+            }
+            else
-              if(tmp < signal-1) tmp++;
+              tmp = signal;
             // calculate signal difference on good signal level
             if(RC_Quality >= 195)
               PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
             else PPM_diff[index] = 0;
             PPM_in[index] = tmp; // update channel value
+  }
+}
 #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
+#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
+#define COMMAND_THRESHOLD 85
+#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
+#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
+// Internal.
+uint8_t RC_getStickCommand(void) {
+  if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
+    // vertical is up
+    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
+      return COMMAND_GYROCAL;
+    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
+      return COMMAND_ACCCAL;
+    return COMMAND_NONE;
+  } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
+    // vertical is down
+    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
+      return COMMAND_STOP;
+    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
+      return COMMAND_START;
+    return COMMAND_NONE;
+  }
+    // vertical is around center
+  return COMMAND_NONE;
 #define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]]
 /*
  * This must be called (as the only thing) for each control loop cycle (488 Hz).
  */
 void RC_update() {
   int16_t tmp1, tmp2;
   if(RC_Quality) {
     RC_Quality--;
     if (NewPpmData-- == 0) {
-      RC_PRTY[CONTROL_PITCH]    = RCChannel(CH_PITCH) * staticParams.StickP - stickOffsetPitch + RCDiff(CH_PITCH) * staticParams.StickD;
+      RC_PRTY[CONTROL_PITCH]    = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD;
-      RC_PRTY[CONTROL_ROLL]     = RCChannel(CH_ROLL)  * staticParams.StickP - stickOffsetRoll  + RCDiff(CH_ROLL)  * staticParams.StickD;
+      RC_PRTY[CONTROL_ROLL]     = RCChannel(CH_ROLL)  * staticParams.StickP - /* stickOffsetRoll */  + RCDiff(CH_ROLL)  * staticParams.StickD;
       RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120;
       if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
       tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
-      // exponential stick sensitivity in yawring rate
+      // exponential stick sensitivity in yawing rate
       tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
       tmp2 += (staticParams.StickYawP * tmp1) / 4;
+      RC_PRTY[CONTROL_YAW] = tmp2;
+    }
+    uint8_t command = RC_getStickCommand();
+    if (lastRCCommand == command) {
+      // Keep timer from overrunning.
+      if (commandTimer < COMMAND_TIMER) commandTimer++;
+    } else {
+      // There was a change.
+      lastRCCommand = command;
-      RC_PRTY[CONTROL_YAW] = tmp2;
+      commandTimer = 0;
+    }
   } else { // Bad signal
     RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
+}
 /*
  * Get other channel value
  */
-int16_t RC_getVariable(uint8_t varNum) {
+int16_t RC_getVariable (uint8_t varNum) {
   if (varNum < 4)
     // 0th variable is 5th channel (1-based) etc.
-    return RCChannel(varNum + 5);
+    return RCChannel(varNum + 4) + POT_OFFSET;
   /*
    * Let's just say:
    * The RC variable 4 is hardwired to channel 5
    * The RC variable 5 is hardwired to channel 6
    * The RC variable 6 is hardwired to channel 7
    * The RC variable 7 is hardwired to channel 8
    * Alternatively, one could bind them to channel (4 + varNum) - or whatever...
    */
-  return PPM_in[varNum + 1];
+  return PPM_in[varNum + 1] + POT_OFFSET;
+}
 uint8_t RC_getSignalQuality(void) {
  * R617 receiver.) This calibration is not strictly necessary, but
  * for control logic that depends on the exact (non)center position
  * of a stick, it may be useful.
  */
 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;
+  }
+}
+*/
-#define COMMAND_THRESHOLD 85
-#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
-#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
-uint8_t RC_getCommand(void) {
-  if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
-    // vertical is up
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
-      return COMMAND_GYROCAL;
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
-      return COMMAND_ACCCAL;
-    return COMMAND_NONE;
-  } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
-    // vertical is down
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
-      return COMMAND_STOP;
-    if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
+uint8_t RC_getCommand(void) {
-      return COMMAND_START;
-    return COMMAND_NONE;
-  } else {
+  if (commandTimer == COMMAND_TIMER) {
-    // vertical is around center
+    // Stick has been held long enough; command committed.
-    return COMMAND_NONE;
+    return lastRCCommand;
+  }
+  // Not yet sure what the command is.
   return COMMAND_NONE;
+}
 /*
+  }
   return looping;
+}
+uint8_t RC_testCompassCalState(void) {
+  static uint8_t stick = 1;
+  // if pitch is centered or top set stick to zero
+  if(RCChannel(CH_PITCH) > -20) stick = 0;
+  // if pitch is down trigger to next cal state
+  if((RCChannel(CH_PITCH) < -70) && !stick) {
+    stick = 1;
+    return 1;
+  }
   return 0;
+}
 /*
  * Abstract controls are not used at the moment.
  t_control rc_control = {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c @ 2159 - Rev 1645 → 1775