WebSVN - FlightCtrl - Diff - Rev 2045 and 2048 - /branches/dongfang_FC

-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Copyright (c) 04.2007 Holger Buss
-// + Nur f�r den privaten Gebrauch
-// + www.MikroKopter.com
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
-// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
-// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
-// + bzgl. der Nutzungsbedingungen aufzunehmen.
-// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
-// + Verkauf von Luftbildaufnahmen, usw.
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
-// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
-// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
-// + eindeutig als Ursprung verlinkt werden
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
-// + Benutzung auf eigene Gefahr
-// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-// + mit unserer Zustimmung zul�ssig
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
-// + this list of conditions and the following disclaimer.
-// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
-// +     from this software without specific prior written permission.
-// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
-// +     for non-commercial use (directly or indirectly)
-// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
-// +     with our written permission
-// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-// +     clearly linked as origin
-// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
-// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// +  POSSIBILITY OF SUCH DAMAGE.
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <stdlib.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include "rc.h"
 #include "uart0.h"
 #include "controlMixer.h"
 #include "configuration.h"
 #include "commands.h"
-// The channel array is now 0-based.
+// The channel array is 0-based!
-volatile int16_t PPM_in[MAX_CHANNELS];
-volatile int16_t PPM_diff[MAX_CHANNELS];
+volatile int16_t PPM_in[MAX_CHANNELS];
-volatile uint8_t newPPMData = 1;
-volatile uint8_t RCQuality;
+volatile int16_t PPM_diff[MAX_CHANNELS];
-int16_t RC_PRTY[4];
+volatile uint8_t RCQuality;
 uint8_t lastRCCommand = COMMAND_NONE;
 uint8_t commandTimer = 0;
  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
+ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms
   int16_t signal = 0, tmp;
   static int16_t index;
   static uint16_t oldICR1 = 0;
   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 >= 3) {
-      newPPMData = 0; // Null means NewData for the first 4 channels
-    }
-    // synchronize channel index
+  if ((signal > 1100) && (signal < 8000)) {
     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)
   // vertical is around center
   return COMMAND_NONE;
+}
 /*
- * This must be called (as the only thing) for each control loop cycle (488 Hz).
+ * Get Pitch, Roll, Throttle, Yaw values
  */
-void RC_periodicTask() {
+void RC_periodicTaskAndPRTY(int16_t* PRTY) {
   int16_t tmp1, tmp2;
   if (RCQuality) {
-    RCQuality--;
-    if (newPPMData-- == 0) {
+    RCQuality--;
-      RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP
-          + RCDiff(CH_PITCH) * staticParams.stickD;
+    PRTY[CONTROL_PITCH]     = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD;
-      RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP
-          + RCDiff(CH_ROLL) * staticParams.stickD;
+    PRTY[CONTROL_ROLL]      = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD;
-      RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
-          * staticParams.stickThrottleD + 120;
-      if (RC_PRTY[CONTROL_THROTTLE] < 0)
+    PRTY[CONTROL_THROTTLE]  = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120;
-        RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
+    if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
-      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))
-          / 512L; // expo  y = ax + bx^2
+    tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) / 512L; // expo  y = ax + bx^2
-      tmp2 += (staticParams.stickYawP * tmp1) >> 2;
+    tmp2 += (staticParams.stickYawP * tmp1) >> 2;
-      RC_PRTY[CONTROL_YAW] = tmp2;
-    }
+    PRTY[CONTROL_YAW] = tmp2;
     uint8_t command = RC_getStickCommand();
     uint8_t command = RC_getStickCommand();
     if (lastRCCommand == command) {
       // Keep timer from overrunning.
       if (commandTimer < COMMAND_TIMER)
         commandTimer++;
     } else {
       // There was a change.
       lastRCCommand = command;
-      commandTimer = 0;
-    }
-  } else { // Bad signal
+      commandTimer = 0;
     RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0;
+  }
   debugOut.analog[18] = RCQuality;
-}
-/*
- * Get Pitch, Roll, Throttle, Yaw values
- */
-int16_t* RC_getPRTY(void) {
-  return RC_PRTY;
+}
 /*
  * Get other channel value
  */
     return 0;
+  }
+}
-/*
-uint8_t RC_getLooping(uint8_t looping) {
-  //  static uint8_t looping = 0;
-  if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
-      & CFG_LOOP_LEFT) {
-    looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
-  } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
-      < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
-  }
-  if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
+/*
-      && staticParams.BitConfig & CFG_LOOP_RIGHT) {
-    looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
-  } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
-      > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-    looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
-  }
-  if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
-      && staticParams.BitConfig & CFG_LOOP_UP) {
-    looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
-  } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
-      < staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
-  }
-  if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
-      && staticParams.BitConfig & CFG_LOOP_DOWN) {
-    looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
-  } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
-      > -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
-    looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
-  }
-  return looping;
-}
+ * For each time the stick is pulled, returns true.
-*/
  */
 uint8_t RC_testCompassCalState(void) {
-  static uint8_t stick = 1;
+  static uint8_t stickPulled = 1;
   // if pitch is centered or top set stick to zero
   if (RCChannel(CH_PITCH) > -20)
-    stick = 0;
+    stickPulled = 0;
   // if pitch is down trigger to next cal state
-  if ((RCChannel(CH_PITCH) < -70) && !stick) {
+  if ((RCChannel(CH_PITCH) < -70) && !stickPulled) {
-    stick = 1;
+    stickPulled = 1;
     return 1;
+  }
   return 0;
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/rc.c - Rev 2045 → 2048